stlmat.h

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

  {}

  /**
   * Assignment operator.
   */
  inline Matrix &operator =(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;
    return *this;
  }

  /**
   * Assignment operator.
   */
  inline Matrix &operator =(const double *v)
  {
    m00 = v[0]; m01 = v[1]; m02 = v[2];
    m10 = v[3]; m11 = v[4]; m12 = v[5];
    m20 = v[6]; m21 = v[7]; m22 = v[8];
    return *this;
  }

  /**
   * Assignment operator.
   */
  template <typename T>
  inline Matrix &operator =(const T *v)
  {
    m00 = v[0]; m01 = v[1]; m02 = v[2];
    m10 = v[3]; m11 = v[4]; m12 = v[5];
    m20 = v[6]; m21 = v[7]; m22 = v[8];
    return *this;
  }

  /**
   * Assignment operator.
   */
  inline Matrix &operator =(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];
    return *this;
  }

  /**
   * Assignment operator.
   */
  template <typename T>
  inline Matrix &operator =(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];
    return *this;
  }

  /**
   * Assign a quaternion to the matrix.
   */
  Matrix &operator =(const Quaternion &q);

  /**
   * 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<3> getRow(int n) const
  {
  	return Vector<3>(mat[n][0], mat[n][1], mat[n][2]);
  }

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

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

  /**
   * Set the matrix with given values array.
   */
  inline Matrix &set(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];
    return *this;
  }

  /**
   * Set the matrix with given type compatible values.
   */
  template <typename T>
  inline Matrix &set(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];
    return *this;
  }

  /**
   * Set the matrix with given two dimension values array.
   */
  inline Matrix &set(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];
    return *this;
  }

  /**
   * Set the matrix with given type compatible two dimension values array.
   */
  template <typename T>
  inline Matrix &set(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];
    return *this;
  }

  /**
   * Set the matrix with given quaternion.
   */
  Matrix &set(const Quaternion &q);

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

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

  /**
   * Query the inverse matrix.
   */
  inline Matrix getInverse() const
  {
    Matrix C(
       (m11*m22 - m12*m21), -(m01*m22 - m02*m21),  (m01*m12 - m02*m11),
      -(m10*m22 - m12*m20),  (m00*m22 - m02*m20), -(m00*m12 - m02*m10),
       (m10*m21 - m11*m20), -(m00*m21 - m01*m20),  (m00*m11 - m01*m10));
    double s = 1.0 /(m00 * C.m00 + m01 * C.m10 + m02 * C.m20);
    C *= s;
    return C;
  }

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

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

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

  /**
   * Add another matrix onto this one.
   */
  inline Matrix &operator +=(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;
    return *this;
  }

  /**
   * Subtract another matrix from this one.
   */
  inline Matrix &operator -=(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;
    return *this;
  }

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

  /**
   * Divide this matrix by a scalar.
   */
  inline Matrix &operator /=(double s)
  {
    s = 1.0 / s;
    m00 *= s; m01 *= s; m02 *= s;
    m10 *= s; m11 *= s; m12 *= s;
    m20 *= s; m21 *= s; m22 *= s;
    return *this;
  }

  /**
   * Multiply this matrix with another one.
   */
  inline Matrix &operator *=(const Matrix &m)
  {
    double old_m00 = m00;
    m00 = m00 * m.m00 + m01 * m.m10 + m02 * m.m20;

    double old_m01 = m01;
    m01 = old_m00 * m.m01 + m01 * m.m11 + m02 * m.m21;
    m02 = old_m00 * m.m02 + old_m01 * m.m12 + m02 * m.m22;

    double old_m10 = m10;
    m10 = m10 * m.m00 + m11 * m.m10 + m12 * m.m20;

    double old_m11 = m11;
    m11 = old_m10 * m.m01 + m11 * m.m11 + m12 * m.m21;
    m12 = old_m10 * m.m02 + old_m11 * m.m12 + m12 * m.m22;

    double old_m20 = m20;
    m20 = m20 * m.m00 + m21 * m.m10 + m22 * m.m20;

    double old_m21 = m21;
    m21 = old_m20 * m.m01 + m21 * m.m11 + m22 * m.m21;
    m22 = old_m20 * m.m02 + old_m21 * m.m12 + m22 * m.m22;
    return *this;
  }

  /**
   * Divide by another matrix.
   */
  inline Matrix &operator /=(const Matrix &m)
  {
    (*this) *= m.getInverse();
    return *this;
  }

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

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

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

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

  /**
   * Multiply a matrix with a scalar.
   */
  friend inline Matrix operator *(const Matrix &m, double s)
  {
    return Matrix(
        m.m00 * s, m.m01 * s, m.m02 * s,
        m.m10 * s, m.m11 * s, m.m12 * s,
        m.m20 * s, m.m21 * s, m.m22 * 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.m02,
        s * m.m10, s * m.m11, s * m.m12,
        s * m.m20, s * m.m21, s * m.m22);
  }

  /**
   * 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.m02 * s,
        m.m10 * s, m.m11 * s, m.m12 * s,
        m.m20 * s, m.m21 * s, m.m22 * 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.m02 * m2.m20,
        m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21,
        m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22,
        m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20,
        m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21,
        m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22,
        m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20,
        m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21,
        m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22);
  }

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

  /**
   * 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.m02 == m2.m02) &&
      (m1.m10 == m2.m10) && (m1.m11 == m2.m11) && (m1.m12 == m2.m12) &&
      (m1.m20 == m2.m20) && (m1.m21 == m2.m21) && (m1.m22 == m2.m22);
  }

  /**
   * 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.m02 != m2.m02) ||
      (m1.m10 != m2.m10) || (m1.m11 != m2.m11) || (m1.m12 != m2.m12) ||
      (m1.m20 != m2.m20) || (m1.m21 != m2.m21) || (m1.m22 != m2.m22);
  }

  /**
   * Add a rotation around x axis with given angle in radians at the end of this
   * rotation matrix, this is equal to left multiply a XRotMatrix onto this.
   */
  inline Matrix &addEndRotationX(double angle)
  {
    double c = cos(angle);
    double s = sin(angle);

    double m;
    m = m10;
    m10 =  c * m + s * m20;
    m20 = -s * m + c * m20;
    m = m11;
    m11 =  c * m + s * m21;
    m21 = -s * m + c * m21;
    m = m12;
    m12 =  c * m + s * m22;
    m22 = -s * m + c * m22;

    return *this;
  }

  /**
   * Add a rotation around x axis with given angle in radians at the head of
   * this rotation matrix, this is equal to right multiply a XRotMatrix onto
   * this.
   */
  inline Matrix &addHeadRotationX(double angle)
  {
    double c = cos(angle);
    double s = sin(angle);

    double m;
    m = m01;
    m01 = m * c - s * m02;
    m02 = m * s + c * m02;
    m = m11;
    m11 = c * m - s * m12;
    m12 = s * m + c * m12;
    m = m21;
    m21 = c * m - s * m22;
    m22 = s * m + c * m22;

    return *this;
  }

  /**
   * Add a rotation around y axis with given angle in radians at the end of this
   * rotation matrix, this is equal to left multiply a YRotMatrix onto this.
   */
  inline Matrix &addEndRotationY(double angle)
  {
    double c = cos(angle);
    double s = sin(angle);

    double m;
    m = m00;
    m00 = c * m - s * m20;
    m20 = s * m + c * m20;
    m = m01;
    m01 = m * c - s * m21;
    m21 = m * s + c * m21;
    m = m02;
    m02 = m * c - s * m22;
    m22 = m * s + c * m22;

    return *this;
  }

  /**
   * Add a rotation around y axis with given angle in radians at the head of
   * this rotation matrix, this is equal to right multiply a YRotMatrix onto
   * this.
   */
  inline Matrix &addHeadRotationY(double angle)
  {
    double c = cos(angle);
    double s = sin(angle);

    double m;
    m = m00;
    m00 =  c * m + m02 * s;
    m02 = -s * m + m02 * c;
    m = m10;
    m10 =  c * m + m12 * s;
    m12 = -s * m + m12 * c;
    m = m20;
    m20 =  c * m + m22 * s;
    m22 = -s * m + m22 * c;

    return *this;
  }

  /**
   * Add a rotation around z axis with given angle in radians at the end of this
   * rotation matrix, this is equal to left multiply a YRotMatrix onto this.
   */
  inline Matrix &addEndRotationZ(double angle)
  {
    double c = cos(angle);
    double s = sin(angle);

    double m;
    m = m00;
    m00 =  c * m + s * m10;
    m10 = -s * m + c * m10;
    m = m01;
    m01 =  c * m + s * m11;
    m11 = -s * m + c * m11;
    m = m02;
    m02 =  c * m + s * m12;
    m12 = -s * m + c * m12;

    return *this;
  }

  /**
   * Add a rotation around z axis with given angle in radians at the head of
   * this rotation matrix, this is equal to right multiply a YRotMatrix onto
   * this.
   */
  inline Matrix &addHeadRotationZ(double angle)
  {
    double c = cos(angle);
    double s = sin(angle);

    double m;
    m = m00;
    m00 = c * m - m01 * s;
    m01 = s * m + m01 * c;
    m = m10;
    m10 = c * m - s * m11;
    m11 = s * m + c * m11;
    m = m20;
    m20 = c * m - s * m21;
    m21 = s * m + c * m21;

    return *this;
  }

  /**
   * Query the euler angle in radians with 312 order.
   */
  inline void getEuler312(double &rZ, double &rX, double &rY) const
  {
    if (m12 < -1) rX = -STLC_HPI;
    else if (m12 > 1) rX = STLC_HPI;
    else rX = asin(m12);

    rY = atan2(-m02, m22);
    rZ = atan2(-m10, m11);
  }
};

typedef Matrix<3, 3>      Matrix3;

/**
 * The rotation matrix around x axis.
 */
struct XRotMatrix3 : public Matrix<3, 3>
{
public:
  /**
   * Construct a rotation matrix around x axis with given angle in radians.
   */
  XRotMatrix3(double angle)
  {
    double c = cos(angle);
    double s = sin(angle);
    set(1, 0, 0, 0, c, s, 0, -s, c);
  }
};

/**
 * The rotation matrix around y axis.
 */
struct YRotMatrix3 : public Matrix<3, 3>
{
public:
  /**
   * Construct a rotation matrix around y axis with given angle in radians.
   */
  YRotMatrix3(double angle)
  {
    double c = cos(angle);
    double s = sin(angle);
    set(c, 0, -s, 0, 1, 0, s, 0, c);
  }
};

/**
 * The rotation matrix around z axis.
 */
struct ZRotMatrix3 : public Matrix<3, 3>
{
public:
  /**
   * Construct a rotation matrix around z axis with given angle in radians.
   */
  ZRotMatrix3(double angle)
  {
    double c = cos(angle);
    double s = sin(angle);
    set(c, s, 0, -s, c, 0, 0, 0, 1);
  }
};

}}

#endif /* __STLMATH_MAT_H__ */