Matrix.h// 矩阵类

//Matrix.h

 

// 矩阵类

//

 

#pragma once

 

#include "Vector.h"

 

//

// 4x4齐次矩阵类

//

class Matrix

{

public:

 /* 行优先存储，列矩阵乘法

 [ m[0][0] m[0][1] m[0][2] ] {x}

 | m[1][0] m[1][1] m[1][2] | * {y}

 | m[2][0] m[2][1] m[2][2] | {z}

 索引顺序： m[row][col]

 */

 union

 {

  float m[3][3];

  float _m[12];

 };

public:

 inline Matrix()

 {}

 inline Matrix(

  float m00, float m01, float m02,

  float m10, float m11, float m12,

  float m20, float m21, float m22

  )

 {

  m[0][0] = m00;

  m[0][1] = m01;

  m[0][2] = m02;

  m[1][0] = m10;

  m[1][1] = m11;

  m[1][2] = m12;

  m[2][0] = m20;

  m[2][1] = m21;

  m[2][2] = m22;

 }

 /**

 *索引矩阵的一行

 */

 inline float* operator [](int row)

 {

  return m[row];

 }

 /**

 *当前矩阵左乘另一个矩阵

 */

 inline Matrix operator*(const Matrix& rhm)

 {

  Matrix r;

  r.m[0][0] = m[0][0] * rhm.m[0][0] + m[0][1] * rhm.m[1][0] + m[0][2] * rhm.m[2][0];

  r.m[0][1] = m[0][0] * rhm.m[0][1] + m[0][1] * rhm.m[1][1] + m[0][2] * rhm.m[2][1];

  r.m[0][2] = m[0][0] * rhm.m[0][2] + m[0][1] * rhm.m[1][2] + m[0][2] * rhm.m[2][2];

 

  r.m[1][0] = m[1][0] * rhm.m[0][0] + m[1][1] * rhm.m[1][0] + m[1][2] * rhm.m[2][0];

  r.m[1][1] = m[1][0] * rhm.m[0][1] + m[1][1] * rhm.m[1][1] + m[1][2] * rhm.m[2][1];

  r.m[1][2] = m[1][0] * rhm.m[0][2] + m[1][1] * rhm.m[1][2] + m[1][2] * rhm.m[2][2];

 

  r.m[2][0] = m[2][0] * rhm.m[0][0] + m[2][1] * rhm.m[1][0] + m[2][2] * rhm.m[2][0];

  r.m[2][1] = m[2][0] * rhm.m[0][1] + m[2][1] * rhm.m[1][1] + m[2][2] * rhm.m[2][1];

  r.m[2][2] = m[2][0] * rhm.m[0][2] + m[2][1] * rhm.m[1][2] + m[2][2] * rhm.m[2][2];

  return r;

 }

 /**

 * m4x4 * (rhv , 0.0)

 * 矩阵左乘向量

 */

 inline Vector3 operator*(const Vector3& rhv)

 {

  Vector3 r;

  r.x = (m[0][0] * rhv.x + m[0][1] * rhv.y + m[0][2] * rhv.z);

  r.y = (m[1][0] * rhv.x + m[1][1] * rhv.y + m[1][2] * rhv.z);

  r.z = (m[2][0] * rhv.x + m[2][1] * rhv.y + m[2][2] * rhv.z);

  return r;

 }

 

 inline void operator=(const Matrix& mat)

 {

  m[0][0] = mat.m[0][0]; m[0][1] = mat.m[0][1]; m[0][2] = mat.m[0][2];

  m[1][0] = mat.m[1][0]; m[1][1] = mat.m[1][1]; m[1][2] = mat.m[1][2];

  m[2][0] = mat.m[2][0]; m[2][1] = mat.m[2][1]; m[2][2] = mat.m[2][2];

 }

 /**

 * 设为单位矩阵

 */

 inline void SetIdentity()

 {

  m[0][0] = 1; m[0][1] = 0; m[0][2] = 0;

  m[1][0] = 0; m[1][1] = 1; m[1][2] = 0;

  m[2][0] = 0; m[2][1] = 0; m[2][2] = 1;

 }

 /**

 *设为零矩阵

 */

 inline void SetZero()

 {

  m[0][0] = 0; m[0][1] = 0; m[0][2] = 0;

  m[1][0] = 0; m[1][1] = 0; m[1][2] = 0;

  m[2][0] = 0; m[2][1] = 0; m[2][2] = 0;

 }

 /**

 *矩阵转置

 */

 inline Matrix Transpose(void) const

 {

  return Matrix( m[0][0], m[1][0], m[2][0],

      m[0][1], m[1][1], m[2][1],

      m[0][2], m[1][2], m[2][2]

      );

 }

 /**

 * 计算矩阵的逆

 */

 Matrix Inverse() const

 {

  float m00 = m[0][0], m01 = m[0][1], m02 = m[0][2], m03 = 0;

  float m10 = m[1][0], m11 = m[1][1], m12 = m[1][2], m13 = 0;

  float m20 = m[2][0], m21 = m[2][1], m22 = m[2][2], m23 = 0;

  float m30 = 0, m31 = 0, m32 = 0, m33 = 1;

 

  float v0 = m20 * m31 - m21 * m30;

  float v1 = m20 * m32 - m22 * m30;

  float v2 = m20 * m33 - m23 * m30;

  float v3 = m21 * m32 - m22 * m31;

  float v4 = m21 * m33 - m23 * m31;

  float v5 = m22 * m33 - m23 * m32;

 

  float t00 = +(v5 * m11 - v4 * m12 + v3 * m13);

  float t10 = -(v5 * m10 - v2 * m12 + v1 * m13);

  float t20 = +(v4 * m10 - v2 * m11 + v0 * m13);

  float t30 = -(v3 * m10 - v1 * m11 + v0 * m12);

 

  float invDet = 1 / (t00 * m00 + t10 * m01 + t20 * m02 + t30 * m03);

 

  float d00 = t00 * invDet;

  float d10 = t10 * invDet;

  float d20 = t20 * invDet;

  float d30 = t30 * invDet;

 

  float d01 = -(v5 * m01 - v4 * m02 + v3 * m03) * invDet;

  float d11 = +(v5 * m00 - v2 * m02 + v1 * m03) * invDet;

  float d21 = -(v4 * m00 - v2 * m01 + v0 * m03) * invDet;

  float d31 = +(v3 * m00 - v1 * m01 + v0 * m02) * invDet;

 

  v0 = m10 * m31 - m11 * m30;

  v1 = m10 * m32 - m12 * m30;

  v2 = m10 * m33 - m13 * m30;

  v3 = m11 * m32 - m12 * m31;

  v4 = m11 * m33 - m13 * m31;

  v5 = m12 * m33 - m13 * m32;

 

  float d02 = +(v5 * m01 - v4 * m02 + v3 * m03) * invDet;

  float d12 = -(v5 * m00 - v2 * m02 + v1 * m03) * invDet;

  float d22 = +(v4 * m00 - v2 * m01 + v0 * m03) * invDet;

  float d32 = -(v3 * m00 - v1 * m01 + v0 * m02) * invDet;

 

  v0 = m21 * m10 - m20 * m11;

  v1 = m22 * m10 - m20 * m12;

  v2 = m23 * m10 - m20 * m13;

  v3 = m22 * m11 - m21 * m12;

  v4 = m23 * m11 - m21 * m13;

  v5 = m23 * m12 - m22 * m13;

 

  float d03 = -(v5 * m01 - v4 * m02 + v3 * m03) * invDet;

  float d13 = +(v5 * m00 - v2 * m02 + v1 * m03) * invDet;

  float d23 = -(v4 * m00 - v2 * m01 + v0 * m03) * invDet;

  float d33 = +(v3 * m00 - v1 * m01 + v0 * m02) * invDet;

 

  return Matrix(

   d00, d01, d02,

   d10, d11, d12,

   d20, d21, d22);

 }

 

 friend std::ostream& operator << (std::ostream& os, Matrix& mat)

 {

  os << "[ " << mat.m[0][0] << " , " << mat.m[0][1] << " , " << mat.m[0][2] << " ]" << std::endl;

  os << "[ " << mat.m[1][0] << " , " << mat.m[1][1] << " , " << mat.m[1][2] << " ]" << std::endl;

  os << "[ " << mat.m[2][0] << " , " << mat.m[2][1] << " , " << mat.m[2][2] << " ]" << std::endl;

  return os;

 }

};