matrix.h

Go to the documentation of this file.

#ifndef ION_MATH_MATRIX_H_
#define ION_MATH_MATRIX_H_

#include <cstring>  // For memcpy().
#include <istream>  // NOLINT
#include <ostream>  // NOLINT

#include "ion/base/logging.h"
#include "ion/base/static_assert.h"
#include "ion/base/stringutils.h"

namespace ion {
namespace math {

template <int Dimension, typename T>
class Matrix {
 public:
  enum { kDimension = Dimension };
  typedef T ValueType;

  Matrix() : elem_() {}

  Matrix(T m00, T m01, T m10, T m11);  // Only when Dimension == 2.
  Matrix(T m00, T m01, T m02,          // Only when Dimension == 3.
         T m10, T m11, T m12,
         T m20, T m21, T m22);
  Matrix(T m00, T m01, T m02, T m03,   // Only when Dimension == 4.
         T m10, T m11, T m12, T m13,
         T m20, T m21, T m22, T m23,
         T m30, T m31, T m32, T m33);

  explicit Matrix(const T array[Dimension * Dimension]);

  template <typename U> explicit Matrix(const Matrix<Dimension, U>& other);

  static Matrix Zero();

  static Matrix Identity();

  T& operator()(int row, int col) {
    CheckIndices(row, col);
    return elem_[row][col];
  }
  T* operator[](int row) {
    CheckIndices(row, 0);
    return elem_[row];
  }

  const T& operator()(int row, int col) const {
    CheckIndices(row, col);
    return elem_[row][col];
  }
  const T* operator[](int row) const {
    CheckIndices(row, 0);
    return elem_[row];
  }

  T* Data() { return &elem_[0][0]; }
  const T* Data() const { return &elem_[0][0]; }

  void operator*=(T s) { MultiplyScalar(s); }
  void operator*=(const Matrix& m) { *this = Product(*this, m); }

  Matrix operator-() const { return Negation(); }

  friend Matrix operator*(const Matrix& m, T s) { return Scale(m, s); }
  friend Matrix operator*(T s, const Matrix& m) { return Scale(m, s); }

  friend Matrix operator+(const Matrix& lhs, const Matrix& rhs) {
    return Addition(lhs, rhs);
  }

  friend Matrix operator-(const Matrix& lhs, const Matrix& rhs) {
    return Subtraction(lhs, rhs);
  }

  friend Matrix operator*(const Matrix& m0, const Matrix& m1) {
    return Product(m0, m1);
  }

  friend bool operator==(const Matrix& m0, const Matrix& m1) {
    return AreEqual(m0, m1);
  }
  friend bool operator!=(const Matrix& m0, const Matrix& m1) {
    return !AreEqual(m0, m1);
  }

 private:
  void CheckIndices(int row, int col) const {
    DCHECK(row >= 0 && row < Dimension && col >= 0 && col < Dimension);
  }

  void MultiplyScalar(T s);
  Matrix Negation() const;
  static Matrix Addition(const Matrix& lhs, const Matrix& rhs);
  static Matrix Subtraction(const Matrix& lhs, const Matrix& rhs);
  static Matrix Scale(const Matrix& m, T s);
  static Matrix Product(const Matrix& m0, const Matrix& m1);
  static bool AreEqual(const Matrix& m0, const Matrix& m1);

  T elem_[Dimension][Dimension];
};

template <int Dimension, typename T>
std::ostream& operator<<(std::ostream& out, const Matrix<Dimension, T>& m) {
  out << "M[";
  for (int row = 0; row < Dimension; ++row) {
    for (int col = 0; col < Dimension; ++col) {
      out << m(row, col);
      if (col != Dimension - 1)
        out << ", ";
    }
    if (row < Dimension - 1)
      out << " ; ";
  }
  return out << "]";
}

template <int Dimension, typename T>
std::istream& operator>>(std::istream& in, Matrix<Dimension, T>& m) {
  Matrix<Dimension, T> mm;
  if (base::GetExpectedString(in, "M[")) {
    for (int row = 0; row < Dimension; ++row) {
      for (int col = 0; col < Dimension; ++col) {
        in >> mm[row][col];
        if (col != Dimension - 1 && !base::GetExpectedChar<','>(in))
          return in;
      }
      if (row != Dimension - 1 && !base::GetExpectedChar<';'>(in))
        return in;
    }
    if (base::GetExpectedChar<']'>(in))
      m = mm;
  }
  return in;
}




template <int Dimension, typename T>
Matrix<Dimension, T>::Matrix(T m00, T m01, T m10, T m11) {
  ION_STATIC_ASSERT(Dimension == 2, "Bad Dimension in Matrix constructor");
  elem_[0][0] = m00;
  elem_[0][1] = m01;
  elem_[1][0] = m10;
  elem_[1][1] = m11;
}

template <int Dimension, typename T>
Matrix<Dimension, T>::Matrix(T m00, T m01, T m02,
                             T m10, T m11, T m12,
                             T m20, T m21, T m22) {
  ION_STATIC_ASSERT(Dimension == 3, "Bad Dimension in Matrix constructor");
  elem_[0][0] = m00;
  elem_[0][1] = m01;
  elem_[0][2] = m02;
  elem_[1][0] = m10;
  elem_[1][1] = m11;
  elem_[1][2] = m12;
  elem_[2][0] = m20;
  elem_[2][1] = m21;
  elem_[2][2] = m22;
}

template <int Dimension, typename T>
Matrix<Dimension, T>::Matrix(T m00, T m01, T m02, T m03,
                             T m10, T m11, T m12, T m13,
                             T m20, T m21, T m22, T m23,
                             T m30, T m31, T m32, T m33) {
  ION_STATIC_ASSERT(Dimension == 4, "Bad Dimension in Matrix constructor");
  elem_[0][0] = m00;
  elem_[0][1] = m01;
  elem_[0][2] = m02;
  elem_[0][3] = m03;
  elem_[1][0] = m10;
  elem_[1][1] = m11;
  elem_[1][2] = m12;
  elem_[1][3] = m13;
  elem_[2][0] = m20;
  elem_[2][1] = m21;
  elem_[2][2] = m22;
  elem_[2][3] = m23;
  elem_[3][0] = m30;
  elem_[3][1] = m31;
  elem_[3][2] = m32;
  elem_[3][3] = m33;
}

template <int Dimension, typename T>
Matrix<Dimension, T>::Matrix(const T array[Dimension * Dimension]) {
#if defined(ION_PLATFORM_WINDOWS) && defined(ION_ARCH_X86_64)
  if ((reinterpret_cast<size_t>(&array[0]) & 0xF) != 0) {
    for (int i = 0; i < Dimension; ++i) {
      for (int j = 0; j < Dimension; ++j) {
        elem_[i][j] = array[i*Dimension+j];
      }
    }
    return;
  }
#endif
  memcpy(elem_, array, sizeof(elem_));
}

template <int Dimension, typename T> template <typename U>
Matrix<Dimension, T>::Matrix(const Matrix<Dimension, U>& other) {
  for (int i = 0; i < Dimension; ++i) {
    for (int j = 0; j < Dimension; ++j)
      elem_[i][j] = static_cast<const T>(other(i, j));
  }
}

template <int Dimension, typename T>
Matrix<Dimension, T> Matrix<Dimension, T>::Zero() {
  Matrix result;
  for (int row = 0; row < Dimension; ++row) {
    for (int col = 0; col < Dimension; ++col)
      result.elem_[row][col] = static_cast<T>(0);
  }
  return result;
}

template <int Dimension, typename T>
Matrix<Dimension, T> Matrix<Dimension, T>::Identity() {
  Matrix result;
  for (int row = 0; row < Dimension; ++row) {
    for (int col = 0; col < Dimension; ++col) {
      result.elem_[row][col] =
          row == col ? static_cast<T>(1) : static_cast<T>(0);
    }
  }
  return result;
}

template <int Dimension, typename T>
void Matrix<Dimension, T>::MultiplyScalar(T s) {
  for (int row = 0; row < Dimension; ++row) {
    for (int col = 0; col < Dimension; ++col)
      elem_[row][col] *= s;
  }
}

template <int Dimension, typename T>
Matrix<Dimension, T> Matrix<Dimension, T>::Negation() const {
  Matrix result;
  for (int row = 0; row < Dimension; ++row) {
    for (int col = 0; col < Dimension; ++col)
      result.elem_[row][col] = -elem_[row][col];
  }
  return result;
}

template <int Dimension, typename T>
Matrix<Dimension, T> Matrix<Dimension, T>::Scale(const Matrix& m, T s) {
  Matrix result;
  for (int row = 0; row < Dimension; ++row) {
    for (int col = 0; col < Dimension; ++col)
      result.elem_[row][col] = m.elem_[row][col] * s;
  }
  return result;
}

template<int Dimension, typename T>
Matrix<Dimension, T> Matrix<Dimension, T>::Addition(
  const Matrix<Dimension, T> &lhs, const Matrix<Dimension, T> &rhs) {
  Matrix<Dimension, T> result;
  for (int row = 0; row < Dimension; ++row) {
    for (int col = 0; col < Dimension; ++col)
      result.elem_[row][col] = lhs.elem_[row][col] + rhs.elem_[row][col];
  }
  return result;
}

template<int Dimension, typename T>
Matrix<Dimension, T> Matrix<Dimension, T>::Subtraction(
    const Matrix<Dimension, T> &lhs, const Matrix<Dimension, T> &rhs) {
  Matrix<Dimension, T> result;
  for (int row = 0; row < Dimension; ++row) {
    for (int col = 0; col < Dimension; ++col)
      result.elem_[row][col] = lhs.elem_[row][col] - rhs.elem_[row][col];
  }
  return result;
}

template <int Dimension, typename T>
Matrix<Dimension, T> Matrix<Dimension, T>::Product(const Matrix& m0,
                                                   const Matrix& m1) {
  Matrix result = Matrix::Zero();
  for (int row = 0; row < Dimension; ++row) {
    for (int col = 0; col < Dimension; ++col) {
      for (int i = 0; i < Dimension; ++i)
        result.elem_[row][col] += m0.elem_[row][i] * m1.elem_[i][col];
    }
  }
  return result;
}

template <int Dimension, typename T>
bool Matrix<Dimension, T>::AreEqual(const Matrix& m0, const Matrix& m1) {
  for (int row = 0; row < Dimension; ++row) {
    for (int col = 0; col < Dimension; ++col) {
      if (m0.elem_[row][col] != m1.elem_[row][col])
        return false;
    }
  }
  return true;
}




typedef Matrix<2, float> Matrix2f;
typedef Matrix<2, double> Matrix2d;
typedef Matrix<3, float> Matrix3f;
typedef Matrix<3, double> Matrix3d;
typedef Matrix<4, float> Matrix4f;
typedef Matrix<4, double> Matrix4d;

}  // namespace math
}  // namespace ion

#endif  // ION_MATH_MATRIX_H_