Matrix.h

2301_77830392

于 2023-05-05 23:53:48 发布

阅读量129

点赞数

文章标签：算法 c++ 机器学习

本文链接：https://blog.csdn.net/2301_77830392/article/details/130517621

版权

//
// 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;
   }
};