stlmat.h

来自「矩阵、向量以及四元数的封装类」· C头文件 代码 · 共 2,636 行 · 第 1/4 页

   * Divide a matrix by a scalar.
   */
  friend inline Matrix operator /(const Matrix &m1, double s)
  {
    s = 1.0 / s;
    Matrix m;
    for (unsigned int i = 0; i < count; i ++)
      m.vec[i] = m1.vec[i] * s;
    return m;
  }

  /**
   * Multiply two matrices.
   */
  template <int K>
  friend inline Matrix operator *(const Matrix<N, K> &mL, const Matrix<K, N> &mR)
  {
    Matrix m;
    for (int i = 0; i < N; i ++)
      for (int j = 0; j < N; j ++)
      {
        m.mat[i][j] = 0;
        for (int k = 0; k < K; k ++)
          m.mat[i][j] += (mL[i][k] * mR[k][j]);
      }
    return m;
  }

  /**
   * Divide a matrix by another one.
   */
  friend inline Matrix operator /(const Matrix &mL, const Matrix &mR) throw(int)
  {
    return mL * mR.getInverse();
  }

  /**
   * Multiply a matrix with a column vector.
   */
  friend inline Vector<N> operator *(const Matrix &m, const Vector<N> &v)
  {
    Vector<N> ret;
    for (int i = 0; i < N; i ++)
    {
      ret[i] = 0;
      for (int j = 0; j < N; j ++)
        ret[i] += m.mat[i][j] * v[j];
    }
    return ret;
  }

  /**
   * Check if the two matrices are equal.
   */
  friend inline bool operator ==(const Matrix &m1, const Matrix &m2)
  {
    for (unsigned int i = 0; i < count; i ++)
      if (m1.vec[i] != m2.vec[i])
        return false;
    return true;
  }

  /**
   * Check if the two matrices are not equal.
   */
  friend inline bool operator !=(const Matrix &m1, const Matrix &m2)
  {
    for (unsigned int i = 0; i < count; i ++)
      if (m1.vec[i] != m2.vec[i])
        return true;
    return false;
  }
  /** @} */
};

template <size_t N>
const unsigned int Matrix<N, N>::count = N * N;
template <size_t N>
const unsigned int Matrix<N, N>::size = sizeof(double) * N * N;

#endif

/** \class Matrix<2, 2>
 * A 2D matrix template class.
 */
template <>
struct Matrix<2, 2>
{
public:
  /// Matrix components.
  union
  {
    struct
    {
      /// First row
      double    m00, m01;
      /// Second row
      double    m10, m11;
    };
    /// Matrix components vector.
    double      vec[4];
    /// Matrix components vector with 2 dim.
    double      mat[2][2];
  };

public:
  /**
   * Construct a new matrix. The matrix is initialized as identity.
   */
  Matrix() :
  	m00(1), m01(0), m10(0), m11(1)
  {}

  /**
   * Construct a new Matrix with given values.
   */
  Matrix(double m00, double m01, double m10, double m11) :
  	m00(m00), m01(m01), m10(m10), m11(m11)
  {}

  /**
   * Construct a new Matrix with given values.
   */
  Matrix(const double *m) :
  	m00(m[0]), m01(m[1]), m10(m[2]), m11(m[3])
  {}

  /**
   * Construct a new Matrix with given values.
   */
  template <typename T>
  Matrix(const T *m) :
    m00(m[0]), m01(m[1]), m10(m[2]), m11(m[3])
  {}

  /**
   * Construct a new Matrix with given values.
   */
  Matrix(const double m[2][2]) :
  	m00(m[0][0]), m01(m[0][1]), m10(m[1][0]), m11(m[1][1])
  {}

  /**
   * Construct a new Matrix with given values.
   */
  template <typename T>
  Matrix(const T m[2][2]) :
    m00(m[0][0]), m01(m[0][1]), m10(m[1][0]), m11(m[1][1])
  {}

  /**
   * Copy constructor.
   */
  Matrix(const Matrix &m) :
  	m00(m.m00), m01(m.m01), m10(m.m10), m11(m.m11)
  {}

  /**
   * Assignment operator.
   */
  inline Matrix &operator =(const Matrix &m)
  {
    m00 = m.m00; m01 = m.m01;
    m10 = m.m10; m11 = m.m11;
    return *this;
  }

  /**
   * Assignment operator.
   */
  inline Matrix &operator =(const double *v)
  {
    m00 = v[0]; m01 = v[1];
    m10 = v[2]; m11 = v[3];
    return *this;
  }

  /**
   * Assignment operator.
   */
  template <typename T>
  inline Matrix &operator =(const T *v)
  {
    m00 = v[0]; m01 = v[1];
    m10 = v[2]; m11 = v[3];
    return *this;
  }

  /**
   * Assignment operator.
   */
  inline Matrix &operator =(const double m[2][2])
  {
    m00 = m[0][0]; m01 = m[0][1];
    m10 = m[1][0]; m11 = m[1][1];
    return *this;
  }

  /**
   * Assignment operator.
   */
  template <typename T>
  inline Matrix &operator =(const T m[2][2])
  {
    m00 = m[0][0]; m01 = m[0][1];
    m10 = m[1][0]; m11 = m[1][1];
    return *this;
  }

  /**
   * Query the n'th row of the Matrix.
   */
  inline double *operator [](int n)
  {
  	return mat[n];
  }

  /**
   * Query the n'th row of the Matrix.
   */
  inline const double *operator[](int n) const
  {
  	return mat[n];
  }

  /**
   * Get the specified row as a vector.
   */
  inline Vector<2> getRow(int n) const
  {
  	return Vector<2>(mat[n][0], mat[n][1]);
  }

  /**
   * Get the specified column as a vector.
   */
  inline Vector<2> getColumn(int n) const
  {
  	return Vector<2>(mat[0][n], mat[1][n]);
  }

  /**
   * Query the matrix's components as an array.
   */
  inline const double * const get() const
  {
  	return vec;
  }

  /**
   * Get the transpose Matrix of this one.
   */
  inline Matrix getTranspose() const
  {
  	return Matrix(m00, m10, m01, m11);
  }

  /**
   * Set the Matrix with given values.
   */
  inline Matrix &set(double m00, double m01, double m10, double m11)
  {
    this->m00 = m00;
    this->m01 = m01;
    this->m10 = m10;
    this->m11 = m11;
    return *this;
  }

  /**
   * Set the matrix with another one.
   */
  inline Matrix &set(const Matrix &m)
  {
    m00 = m.m00; m01 = m.m01;
    m10 = m.m10; m11 = m.m11;
    return *this;
  }

  /**
   * Set the matrix with given values array.
   */
  inline Matrix &set(const double *m)
  {
    m00 = m[0]; m01 = m[1];
    m10 = m[2]; m11 = m[3];
    return *this;
  }

  /**
   * Set the matrix with given type compatible value array.
   */
  template <typename T>
  inline Matrix &set(const T *m)
  {
    m00 = m[0]; m01 = m[1];
    m10 = m[2]; m11 = m[3];
    return *this;
  }

  /**
   * Set the matrix with given two dimension array.
   */
  inline Matrix &set(const double m[2][2])
  {
    m00 = m[0][0]; m01 = m[0][1];
    m10 = m[1][0]; m11 = m[1][1];
    return *this;
  }

  /**
   * Set the matrix with given type compatible two dimension values array.
   */
  template <typename T>
  inline Matrix &set(const T m[2][2])
  {
    m00 = m[0][0]; m01 = m[0][1];
    m10 = m[1][0]; m11 = m[1][1];
    return *this;
  }

  /**
   * Transpose this matrix.
   */
  inline Matrix &transpose()
  {
    double swap = m01;
    m01 = m10;
    m10 = swap;
    return *this;
  }

  /**
   * Query the inverse matrix.
   */
  inline Matrix getInverse() const
  {
    double inv_det = 1.0 / (m00 * m11 - m01 * m10);
    return Matrix(
        m11 * inv_det, -m01 * inv_det,
        -m10 * inv_det, m00 * inv_det);
  }

  /**
   * Invert this matrix.
   */
  inline Matrix &invert()
  {
  	*this = getInverse();
  	return *this;
  }

  /**
   * Query the determinant of this matrix.
   */
  inline double getDeterminant() const
  {
  	return m00 * m11 - m01 * m10;
  }

  /**
   * Identity this matrix.
   */
  inline Matrix &identity()
  {
    m00 = m11 = 1;
    m01 = m10 = 0;
    return *this;
  }

  /**
   * Add another matrix onto this one.
   */
  inline Matrix &operator +=(const Matrix &m)
  {
    m00 += m.m00;
    m01 += m.m01;
    m10 += m.m10;
    m11 += m.m11;
    return *this;
  }

  /**
   * Subtract another matrix from this one.
   */
  inline Matrix &operator -=(const Matrix &m)
  {
    m00 -= m.m00;
    m01 -= m.m01;
    m10 -= m.m10;
    m11 -= m.m11;
    return *this;
  }

  /**
   * Multiply this matrix with a scalar.
   */
  inline Matrix &operator *=(double s)
  {
    m00 *= s;
    m01 *= s;
    m10 *= s;
    m11 *= s;
    return *this;
  }

  /**
   * Devide this matrix by a scalar.
   */
  inline Matrix &operator /=(double s)
  {
    s = 1.0 / s;
    m00 *= s;
    m01 *= s;
    m10 *= s;
    m11 *= s;
    return *this;
  }

  /**
   * Multiply this matrix with another one.
   */
  inline Matrix &operator *=(const Matrix &m)
  {
    Matrix r(*this);
    m00 = r.m00 * m.m00 + r.m01 * m.m10;
    m01 = r.m00 * m.m01 + r.m01 * m.m11;
    m10 = r.m10 * m.m00 + r.m11 * m.m10;
    m11 = r.m10 * m.m01 + r.m11 * m.m11;
    return *this;
  }

  /**
   * Divide by another matrix.
   */
  inline Matrix &operator /=(const Matrix &m)
  {
    Matrix r(this->getInverse());
    m00 = r.m00 * m.m00 + r.m01 * m.m10;
    m01 = r.m00 * m.m01 + r.m01 * m.m11;
    m10 = r.m10 * m.m00 + r.m11 * m.m10;
    m11 = r.m10 * m.m01 + r.m11 * m.m11;
    return *this;
  }

  /**
   * Unary + operator.
   */
  inline Matrix operator +() const
  {
  	return *this;
  }

  /**
   * Unary - operator.
   */
  inline Matrix operator -() const
  {
  	return Matrix(-m00, -m01, -m10, -m11);
  }

  /**
   * Add two matrices.
   */
  friend inline Matrix operator +(const Matrix &m1, const Matrix &m2)
  {
    return Matrix(
        m1.m00 + m2.m00, m1.m01 + m2.m01,
        m1.m10 + m2.m10, m1.m11 + m2.m11);
  }

  /**
   * Subtract two matrices.
   */
  friend inline Matrix operator -(const Matrix &m1, const Matrix &m2)
  {
    return Matrix(
        m1.m00 - m2.m00, m1.m01 - m2.m01,
        m1.m10 - m2.m10, m1.m11 - m2.m11);
  }

  /**
   * Multiply a matrix with a scalar.
   */
  friend inline Matrix operator *(const Matrix &m, double s)
  {
  	return Matrix(m.m00 * s, m.m01 * s, m.m10 * s, m.m11 * s);
  }

  /**
   * Multiply a scalar with a matrix.
   */
  friend inline Matrix operator *(double s, const Matrix &m)
  {
    return Matrix(
        s * m.m00, s * m.m01,
        s * m.m10, s * m.m11);
  }

  /**
   * Divide a matrix by a scalar.
   */
  friend inline Matrix operator /(const Matrix &m, double s)
  {
    s = 1.0 / s;
    return Matrix(
        m.m00 * s, m.m01 * s,
        m.m10 * s, m.m11 * s);
  }

  /**
   * Multiply two matrices.
   */
  friend inline Matrix operator *(const Matrix &m1, const Matrix &m2)
  {
    return Matrix(
        m1.m00 * m2.m00 + m1.m01 * m2.m10,
        m1.m00 * m2.m01 + m1.m01 * m2.m11,
        m1.m10 * m2.m00 + m1.m11 * m2.m10,
        m1.m10 * m2.m01 + m1.m11 * m2.m11);
  }

  /**
   * Multiply a matrix with a column vector.
   */
  friend inline Vector<2> operator *(const Matrix &m, const Vector<2> &v)
  {
    return Vector<2>(
        m.m00 * v.x + m.m01 * v.y,
        m.m10 * v.x + m.m11 * v.y);
  }

  /**
   * Divide a Matrix by another one.
   */
  friend inline Matrix operator /(const Matrix &m1, const Matrix &m2)
  {
  	return m1 * m2.getInverse();
  }

  /**
   * Check if the two matrices are equal.
   */
  friend inline bool operator ==(const Matrix &m1, const Matrix &m2)
  {
    return (m1.m00 == m2.m00) && (m1.m01 == m2.m01) &&
      (m1.m10 == m2.m10) && (m1.m11 == m2.m11);
  }

  /**
   * Check if the two matrices are not equal.
   */
  friend inline bool operator !=(const Matrix &m1, const Matrix &m2)
  {
    return (m1.m00 != m2.m00) || (m1.m01 != m2.m01) ||
      (m1.m10 != m2.m10) || (m1.m11 != m2.m11);
  }
};

typedef Matrix<2, 2>    Matrix2;

struct Quaternion;

/** \class Matrix<3, 3>
 * A 3D matrix.
 */
template <>
struct Matrix<3, 3>
{
public:
  /// Matrix components.
  union
  {
    struct
    {
      /// First row
      double    m00, m01, m02;
      /// Second row
      double    m10, m11, m12;
      /// Third row
      double    m20, m21, m22;
    };
    /// Matrix components vector.
    double      vec[9];
    /// Matrix components vector with 2 dim.
    double      mat[3][3];
  };

public:
  /**
   * Construct a new matrix.
   */
  Matrix() :
    m00(1), m01(0), m02(0),
    m10(0), m11(1), m12(0),
    m20(0), m21(0), m22(1)
  {}

  /**
   * Construct a new matrix with given values.
   */
  Matrix(double m00, double m01, double m02,
         double m10, double m11, double m12,
         double m20, double m21, double m22) :
            m00(m00), m01(m01), m02(m02),
            m10(m10), m11(m11), m12(m12),
            m20(m20), m21(m21), m22(m22)
  {}

  /**
   * Construct a new matrix with given values.
   */
  Matrix(const double *m) :
    m00(m[0]), m01(m[1]), m02(m[2]),
    m10(m[3]), m11(m[4]), m12(m[5]),
    m20(m[6]), m21(m[7]), m22(m[8])
  {}

  /**
   * Construct a new matrix with given values.
   */
  template <typename T>
  Matrix(const T *m) :
    m00(m[0]), m01(m[1]), m02(m[2]),
    m10(m[3]), m11(m[4]), m12(m[5]),
    m20(m[6]), m21(m[7]), m22(m[8])
  {}

  /**
   * Construct a new matrix with given values.
   */
  Matrix(const double m[3][3]) :
    m00(m[0][0]), m01(m[0][1]), m02(m[0][2]),
    m10(m[1][0]), m11(m[1][1]), m12(m[1][2]),
    m20(m[2][0]), m21(m[2][1]), m22(m[2][2])
  {}

  /**
   * Construct a new matrix with given values.
   */
  template <typename T>
  Matrix(const T m[3][3]) :
    m00(m[0][0]), m01(m[0][1]), m02(m[0][2]),
    m10(m[1][0]), m11(m[1][1]), m12(m[1][2]),
    m20(m[2][0]), m21(m[2][1]), m22(m[2][2])
  {}

  /**
   * Construct a matrix by quaternion @a q
   */
  Matrix(const Quaternion &q);

  /**
   * Copy constructor.
   */
  Matrix(const Matrix &m) :
    m00(m.m00), m01(m.m01), m02(m.m02),
    m10(m.m10), m11(m.m11), m12(m.m12),
    m20(m.m20), m21(m.m21), m22(m.m22)