stlmat.h

矩阵、向量以及四元数的封装类

//========================================================================================
// Copyright (C) 2009 by Ma Zhi-Hao, Key Lab.
//          		The Academy of Equipment Command & Technology, PLA
//          		HuaiRou, Beijing, 101416, China
// File 		：STLmat.h
// Version	：1.0
// Email  	：[e-mail]
// Address  : 164, P.O.Box 3380, Beijing
//========================================================================================

/** \file
 * Matrix template.
 */

#ifndef __STLMATH_MAT_H__
#define __STLMATH_MAT_H__

#include "STLvec.h"

namespace shooter{ namespace math{

/** \name Error code
 * Matrix operation error code.
 */
enum MatrixErrorCode
{
	/// Determinant of the matrix is zero, then no inverse matrix exist.
	ZeroMatrix				= 0,
#if defined(STL_COMPILER_MSVC) && STL_COMPILER_VER <= 1200
	/// The matrix is not square matrix.
	NoSquareMatrix		= 9
#endif
};

/** \class Matrix<R, C>
 * A matrix template class with R rows and C columns.
 */
template <size_t R, size_t C>
struct Matrix
{
protected:
  /// Array components.
  union
  {
    /// 2D array.
    double      mat[R][C];
    /// 1D array.
    double      vec[R * C];
  };
  /// Count of the components
  static const size_t count;
  /// Size in bytes of the components.
  static const size_t 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 to 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 to the values array used to set the matrix.@return
   */
  template <typename T>
  Matrix(const T *m)
  {
    for (size_t 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[R][C])
  {
    memcpy(vec, m[0], size);
  }

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

  /**
   * Copy constructor, construct a matrix with another one.
   * @param m Reference of the 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 type compatible values array used to set the matrix.
   * @return Reference of this matrix.
   */
  template <typename T>
  inline Matrix &operator =(const T *m)
  {
    for (size_t 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[R][C])
  {
    memcpy(vec, m[0], size);
    return *this;
  }

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

  /**
   * Assignment operator, set this matrix with another one.
   * @param m Reference of the 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 the 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 the 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 values array.
   * @param m Pointer of the type compatible values array used to set the matrix.
   * @return Reference of this matrix.
   */
  template <typename T>
  inline Matrix &set(const T *m)
  {
    for (size_t 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 used to set this matrix.
   * @return Reference of this matrix.
   */
  inline Matrix &set(const double m[R][C])
  {
    memcpy(vec, m[0], size);
    return *this;
  }

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

  /** \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 at the queried row in the matrix.
   */
  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 at the queried row in the matrix.
   */
  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's components.
   */
  inline Vector<C> getRow(int n) const
  {
    return Vector<C>(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's components.
   */
  inline Vector<R> getColumn(int n) const
  {
    Vector<R> v;
    for (size_t i = 0; i < R; i ++)
      v[i] = mat[i][n];
    return v;
  }

  /**
   * Query the matrix's components as an array.
   * @return Address of the first component in the matrix.
   */
  inline const double * const get() const
  {
  	return vec;
  }

  /**
   * Get the transpose Matrix of this one.
   * @return The transposed matrix of this one.
   */
  inline Matrix<C, R> getTranspose() const
  {
    Matrix<C, R> m;
    for (size_t i = 0; i < R; i ++)
      for (size_t j = 0; j < C; j ++)
        m[j][i] = mat[i][j];
    return m;
  }
  /** @} */

  /** \name Operators
   * @{ */
  /**
   * Add another matrix onto this one.
   */
  inline Matrix &operator +=(const Matrix &m)
  {
    for (size_t 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 (size_t i = 0; i < count; i ++)
      vec[i] -= m.vec[i];
    return *this;
  }

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

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

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

  /**
   * Unary - operator.
   */
  inline Matrix operator -() const
  {
    Matrix m;
    for (size_t 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 (size_t 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 (size_t 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 (size_t 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 (size_t i = 0; i < count; i ++)
      m.vec[i] = s * m1.vec[i];
    return m;
  }

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

  /**
   * Multiply two matrices.
   */
  template <size_t K>
  friend inline Matrix operator *(const Matrix<R, K> &m1, const Matrix<K, C> &m2)
  {
    Matrix m;
    for (size_t i = 0; i < R; i ++)
      for (size_t j = 0; j < C; j ++)
      {
        m.mat[i][j] = 0;
        for (size_t k = 0; k < K; k ++)
          m.mat[i][j] += m1[i][k] * m2[k][j];
      }
    return m;
  }

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

  /**
   * Check if the two matrices are equal.
   */
  friend inline bool operator ==(const Matrix &m1, const Matrix &m2)
  {
    for (size_t 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 (size_t i = 0; i < count; i ++)
      if (m1.vec[i] != m2.vec[i])
        return true;
    return false;
  }
  /** @} */

#if defined(STL_COMPILER_MSVC) && STL_COMPILER_VER <= 1200
  /**
   * Make this matrix identity.
   * @return Reference of this matrix.
   */
  inline Matrix &identity() throw(MatrixErrorCode)
  {
  	if (R != C)
  		throw NoSquareMatrix;
    memset(vec, 0, size);
    for (unsigned int i = 0; i < N; i ++)
      mat[i][i] = 1.0;
    return *this;
  }

  /**
   * Transpose this matrix.
   * @return Reference of this matrix.
   */
  inline Matrix &transpose()
  {
  	if (R != C)
  		throw NoSquareMatrix;
    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(int)
  {
  	if (R != C)
  		throw NoSquareMatrix;
    Matrix m(mat);
    m.invert();
    return m;
  }

  /**
   * Invert this matrix.
   * @return Reference of this one.
   */
  inline void invert() throw(int)
  {
  	if (R != C)
  		throw NoSquareMatrix;
    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 0;
      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
  {
  	if (R != C)
  		throw NoSquareMatrix;
    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];