stlmat.h

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

          m.mat[k][i] = m.mat[is[k]][i];
          m.mat[is[k]][i] = swap;
        }
      }
      if (js[k] != k)
      {
        f = -f;
        for (i = 0; i < N; ++i)
        {
          swap = m.mat[i][k];
          m.mat[i][k] = m.mat[i][js[k]];
          m.mat[i][js[k]] = swap;
        }
      }

      fDet *= m.mat[k][k];
      m.mat[k][k] = 1.0 / m.mat[k][k];
      for (j = 0; j < N; j++)
      {
        if (j != k)
          m.mat[k][j] *= m.mat[k][k];
      }
      for (i = 0; i < N; i++)
      {
        if (i != k)
        {
          for (j = 0; j < N; j++)
          {
            if (j != k)
              m.mat[i][j] = m.mat[i][j] - m.mat[i][k] * m.mat[k][j];
          }
        }
      }
      for (i = 0; i < N; i++)
      {
        if (i != k)
          m.mat[i][k] *= -m.mat[k][k];
      }
    }
    return fDet * f;
  }

  /**
   * Divide a matrix by another one.
   */
  friend inline Matrix operator /(const Matrix &mL, const Matrix &mR) throw(int)
  {
  	if (R != C)
  		throw NoSquareMatrix;
    return mL * mR.getInverse();
  }
#endif
};

template <size_t R, size_t C>
const size_t Matrix<R, C>::count = R * C;
template <size_t R, size_t C>
const size_t Matrix<R, C>::size = sizeof(double) * R * C;

#if !defined(STL_COMPILER_MSVC) || STL_COMPILER_VER > 1200
/** \class Matrix<N, N>
 * A N x N matrix template class.
 */
template <size_t N>
struct Matrix<N, N>
{
private:
  /// Array components.
  union
  {
    /// 2D array.
    double      mat[N][N];
    /// 1D array.
    double      vec[N * N];
  };
  /// Count of the components
  static const unsigned int count;
  /// Size in bytes of the components.
  static const unsigned int size;

public:
  /** \name Construct
   * @{ */
  /**
   * Default constructor.
   * @remarks The matrix is not initialized in this case.
   */
  Matrix()
  {}

  /**
   * Construct a matrix with given values array.
   * @param m Pointer of the values array used to set the matrix.
   */
  Matrix(const double *m)
  {
    memcpy(vec, m, size);
  }

  /**
   * Construct a matrix with given type compatible values array.
   * @param m Pointer of the values array used to set the matrix.
   */
  template <typename T>
  Matrix(const T *m)
  {
    for (unsigned int i = 0; i < count; i ++)
      vec[i] = m[i];
  }

  /**
   * Construct a matrix with given 2D values array.
   * @param m A 2D values array.
   */
  Matrix(const double m[N][N])
  {
    memcpy(vec, m[0], size);
  }

  /**
   * Construct a matrix with given type compatible 2D values array.
   * @param m A 2D type compatible values array.
   */
  template <typename T>
  Matrix(const T m[N][N])
  {
    for (unsigned int i = 0; i < N; i ++)
      for (unsigned int j = 0; j < N; j ++)
        mat[i][j] = m[i][j];
  }

  /**
   * Copy constructor, construct a matrix with another one.
   * @param m Another matrix used to set the new one.
   */
  Matrix(const Matrix &m)
  {
    memcpy(vec, m.vec, size);
  }
  /** @} */

  /** \name Assignment
   * @{ */
  /**
   * Assignment operator, set the matrix with a values array.
   * @param m Pointer of the values array used to set the matrix.
   * @return Reference of this matrix.
   */
  inline Matrix &operator =(const double *m)
  {
    memcpy(vec, m, size);
    return *this;
  }

  /**
   * Assignment operator, set the matrix with a type compatible values array.
   * @param m Pointer of the values array used to set the matrix.
   * @return Reference of this matrix.
   */
  template <typename T>
  inline Matrix &operator =(const T *m)
  {
    for (unsigned int i = 0; i < count; i ++)
      vec[i] = m[i];
    return *this;
  }

  /**
   * Assignment operator, set the matrix with a 2D values array.
   * @param m A 2D values array.
   * @return Reference of this matrix.
   */
  inline Matrix &operator =(const double m[N][N])
  {
    memcpy(vec, m[0], size);
    return *this;
  }

  /**
   * Assignment operator, set the matrix with a 2D type compatible values array.
   * @param m A 2D values array.
   * @return Reference of this matrix.
   */
  template <typename T>
  inline Matrix &operator =(const T m[N][N])
  {
    for (unsigned int i = 0; i < N; i ++)
      for (unsigned int j = 0; j < N; j ++)
        mat[i][j] = m[i][j];
    return *this;
  }

  /**
   * Assignment operator, set the matrix with another one.
   * @param m Reference of another matrix used to set this one.
   * @return Reference of this matrix.
   */
  inline Matrix &operator =(const Matrix &m)
  {
    memcpy(vec, m.vec, size);
    return *this;
  }


  /**
   * Set the matrix with another one.
   * @param m Reference of another matrix used to set this one.
   * @return Reference of this matrix.
   */
  inline Matrix &set(const Matrix &m)
  {
    memcpy(vec, m.vec, size);
    return *this;
  }

  /**
   * Set the matrix with given values array.
   * @param m Pointer of the values array used to set this matrix.
   * @return Reference of this matrix.
   */
  inline Matrix &set(const double *m)
  {
    memcpy(vec, m, size);
    return *this;
  }

  /**
   * Set the matrix with given type compatible value array.
   * @param m Pointer of the values array used to set the matrix.
   * @return Reference of this matrix.
   */
  template <typename T>
  inline Matrix &set(const T *m)
  {
    for (unsigned int i = 0; i < count; i ++)
      vec[i] = m[i];
    return *this;
  }

  /**
   * Set the matrix with given two dimension array.
   * @param m A 2D values array.
   * @return Reference of this matrix.
   */
  inline Matrix &set(const double m[N][N])
  {
    memcpy(vec, m[0], size);
    return *this;
  }

  /**
   * Set the matrix with given type compatible two dimension values array.
   * @param m A 2D values array.
   * @return Reference of this matrix.
   */
  template <typename T>
  inline Matrix &set(const T m[N][N])
  {
    for (unsigned int i = 0; i < N; i ++)
      for (unsigned int j = 0; j < N; j ++)
        mat[i][j] = m[i][j];
    return *this;
  }

  /**
   * Make this matrix identity.
   * @return Reference of this matrix.
   */
  inline Matrix &identity()
  {
    memset(vec, 0, size);
    for (unsigned int i = 0; i < N; i ++)
      mat[i][i] = 1.0;
    return *this;
  }
  /** @} */

public:
	/** \name Visit
	 * @{ */
  /**
   * Query the n'th row of the Matrix.
   * @param n Zero based index of the row wanted.
   * @return Address of the first component in the given row.
   */
	inline double *operator [](int n)
  {
    return mat[n];
  }

  /**
   * Query the n'th row of the Matrix.
   * @param n Zero based index of the row wanted.
   * @return Address of the first component in the given row.
   */
	inline const double *operator[](int n) const
  {
    return mat[n];
  }

  /**
   * Get the specified row as a vector.
   * @param n Zero based index of the row wanted.
   * @return A vector holding on the queried row.
   */
	inline Vector<N> getRow(int n) const
  {
    return Vector<N>(mat[n]);
  }

  /**
   * Get the specified column as a vector.
   * @param n Zero based index of the column wanted.
   * @return A vector holding on the queried column.
   */
	inline Vector<N> getColumn(int n) const
  {
    Vector<N> v;
    for (unsigned int i = 0; i < N; i ++)
      v[i] = mat[i][n];
    return v;
  }
  /** @} */

  /**
   * Get the transpose Matrix of this one.
   * @return A matrix as the transpose of this one.
   */
	inline Matrix<N, N> getTranspose() const
  {
    Matrix<N, N> m;
    for (unsigned int i = 0; i < N; i ++)
      for (unsigned int j = 0; j < N; j ++)
        m[j][i] = mat[i][j];
    return m;
  }

  /**
   * Transpose this matrix.
   * @return Reference of this matrix.
   */
  inline Matrix &transpose()
  {
    double swap;
    for (int i = 0; i < N; i ++)
      for (int j = 0; j < i; j ++)
      {
        swap = mat[i][j];
        mat[i][j] = mat[j][i];
        mat[j][i] = swap;
      }
    return *this;
  }

  /**
   * Query the inverse matrix.
   * @return A matrix as the inverse matrix of this one.
   */
  inline Matrix getInverse() const throw(MatrixErrorCode)
  {
    Matrix m(mat);
    m.invert();
    return m;
  }

  /**
   * Invert this matrix.
   * @return Reference of this one.
   */
  inline void invert() throw(MatrixErrorCode)
  {
    int is[N];
    int js[N];
    int i, j, k;
    double swap;
    for (k = 0; k < N; ++k)
    {
      double fMax = 0.0;
      is[k] = js[k] = k;
      for (i = k; i < N; ++i)
      {
        for (j = k; j < N; ++j)
        {
          if (fabs(mat[i][j]) > fMax)
          {
            fMax = fabs(mat[i][j]);
            is[k] = i;
            js[k] = j;
          }
        }
      }
      if (fMax < 1e-12)
        throw ZeroMatrix;
      if (is[k] != k)
      {
        for (i = 0; i < N; ++i)
        {
          swap = mat[k][i];
          mat[k][i] = mat[is[k]][i];
          mat[is[k]][i] = swap;
        }
      }
      if (js[k] != k)
      {
        for (i = 0; i < N; ++i)
        {
          swap = mat[i][k];
          mat[i][k] = mat[i][js[k]];
          mat[i][js[k]] = swap;
        }
      }
      mat[k][k] = 1.0 / mat[k][k];
      for (j = 0; j < N; ++j)
      {
        if (j != k)
          mat[k][j] *= mat[k][k];
      }
      for (i = 0; i < N; ++i)
      {
        if (i != k)
        {
          for (j = 0; j < N; ++j)
          {
            if (j != k)
              mat[i][j] = mat[i][j] - mat[i][k] * mat[k][j];
          }
        }
      }
      for (i = 0; i < N; ++i)
      {
        if (i != k)
          mat[i][k] *= -mat[k][k];
      }
    }
    for (k = N - 1; (int)k >= 0; k--)
    {
      if (js[k] != k)
      {
        for (i = 0; i < N; ++i)
        {
          swap = mat[k][i];
          mat[k][i] = mat[js[k]][i];
          mat[js[k]][i] = swap;
        }
      }
      if (is[k] != k)
      {
        for (i = 0; i < N; ++i)
        {
          swap = mat[i][k];
          mat[i][k] = mat[i][is[k]];
          mat[i][is[k]] = swap;
        }
      }
    }

    return *this;
  }

  /**
   * Query the determinant of this matrix.
   * @return Determinant of this matrix.
   */
  inline double getDeterminant() const
  {
    Matrix m(mat);
    int is[N], js[N];
    int i, j, k;
    double fDet = 1.0, swap;
    int f = 1;
    for (k = 0; k < N; ++k)
    {
      double fMax = 0.0;
      for (i = k; i < N; i++)
      {
        for (j = k; j < N; j++)
        {
          if (fabs(m.mat[i][j]) > fMax)
          {
            fMax = fabs(m.mat[i][j]);
            is[k] = i;
            js[k] = j;
          }
        }
      }
      if (fabs(fMax) < 1e-12)
        return 0;
      if (is[k] != k)
      {
        f = -f;
        for (i = 0; i < N; ++i)
        {
          swap = m.mat[k][i];
          m.mat[k][i] = m.mat[is[k]][i];
          m.mat[is[k]][i] = swap;
        }
      }
      if (js[k] != k)
      {
        f = -f;
        for (i = 0; i < N; ++i)
        {
          swap = m.mat[i][k];
          m.mat[i][k] = m.mat[i][js[k]];
          m.mat[i][js[k]] = swap;
        }
      }

      fDet *= m.mat[k][k];
      m.mat[k][k] = 1.0 / m.mat[k][k];
      for (j = 0; j < N; j++)
      {
        if (j != k)
          m.mat[k][j] *= m.mat[k][k];
      }
      for (i = 0; i < N; i++)
      {
        if (i != k)
        {
          for (j = 0; j < N; j++)
          {
            if (j != k)
              m.mat[i][j] = m.mat[i][j] - m.mat[i][k] * m.mat[k][j];
          }
        }
      }
      for (i = 0; i < N; i++)
      {
        if (i != k)
          m.mat[i][k] *= -m.mat[k][k];
      }
    }
    return fDet * f;
  }

  /** \name Operators
   * @{ */
  /**
   * Add another matrix onto this one.
   */
  inline Matrix &operator +=(const Matrix &m)
  {
    for (unsigned int i = 0; i < count; i ++)
      vec[i] += m.vec[i];
    return *this;
  }

  /**
   * Subtract another matrix from this one.
   */
  inline Matrix &operator -=(const Matrix &m)
  {
    for (unsigned int i = 0; i < count; i ++)
      vec[i] -= m.vec[i];
    return *this;
  }

  /**
   * Multiply this matrix with a scalar.
   */
  inline Matrix &operator *=(double s)
  {
    for (unsigned int i = 0; i < count; i ++)
      vec[i] *= s;
    return *this;
  }

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

  /**
   * Multiply this matrix with another one.
   */
  inline Matrix &operator *=(const Matrix &m)
  {
    set((*this) * m);
    return *this;
  }

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

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

  /**
   * Unary - operator.
   */
  inline Matrix operator -() const
  {
    Matrix m;
    for (unsigned int i = 0; i < count; i ++)
      m.vec[i] = -vec[i];
    return m;
  }

  /**
   * Add two matrices.
   */
  friend inline Matrix operator +(const Matrix &m1, const Matrix &m2)
  {
    Matrix m;
    for (unsigned int i = 0; i < count; i ++)
      m.vec[i] = m1.vec[i] + m2.vec[i];
    return m;
  }

  /**
   * Subtract two matrices.
   */
  friend inline Matrix operator -(const Matrix &m1, const Matrix &m2)
  {
    Matrix m;
    for (unsigned int i = 0; i < count; i ++)
      m.vec[i] = m1.vec[i] - m2.vec[i];
    return m;
  }

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

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

  /**