????????这篇文章主要是讲了矩阵的相乘、旋转、平移、缩放、绕轴翻转、求逆、求转置等运算。
????????矩阵一共写了两套，一个是给2D元素使用的，一套是给3D对象使用的，这里为了减少篇幅，只写4*4的矩阵的运算，原理和3*3矩阵一直，相比之下比3*3矩阵困难，理解了4*4的运算自然也就理解3*3的运算了。
????????
? 矩阵相乘：? 先说什么是行什么是列，横着的就是行，竖着的就是列。可以看一下图片1.1，别弄过了。不然一会计算一起全都弄饭了算起来就全错。? ?计算说的简单点就是每一行乘上每一列，然后把算出来的结果保存到一个新的矩阵对应的n行m列中。其实这个可以从结果出发反着看比较容易能看懂，比如说我要计算mat[0][0]这个mat的第0行第0列怎么算？ 就是让
a[0][0]*b[0][0]+
a[0][1]*b[1][0]+
a[0][2]*b[2][0]+
a[0][3]*b[3][0] 得到的结果就是mat[0][0]想要的结果
? ?程序1.1
Matrix3D Matrix3D::operator*(Matrix3D &mat)
{
Matrix3D temp;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
temp.m[i][j] =
m[i][0] * mat.m[0][j] +
m[i][1] * mat.m[1][j] +
m[i][2] * mat.m[2][j] +
m[i][3] * mat.m[3][j];
}
}
return temp;
}????????
?????????图1.1
矩阵绕X轴旋转：直接套用图1.2 公式就可以：
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?图1.2
但是大家一定会有疑问，公式里不是这样的呀？为什么行列正好反过来了，这是因为OpenGL虽然用的是行矩阵，但是是列主序。相当于存空间信息的时候要以列主序的方式去存。
下文有具体解释什么是行主序，什么是列主序列，这里就不展开讲这个了。
null
https://www.jianshu.com/p/bfc8327eaad3
程序1.2
void Matrix3D::XRotate(float angle)
{
m[1][1] =cos(GL_PI/180*angle) ; m[1][2] =sin(GL_PI/180*angle) ;
m[2][1] =-sin(GL_PI/180*angle); m[2][2] =cos(GL_PI/180*angle) ;
}矩阵绕Y轴旋转：同上直接套图1.3公式，空间信息存成列主序列：
????????????????????????????????????????????????????????????????图1.3
程序1.3
void Matrix3D::YRotate(float angle)
{
m[0][0] =cos(GL_PI/180*angle); m[0][2] =-sin(GL_PI/180*angle) ;
m[2][0] = sin(GL_PI / 180 * angle); m[2][2] =cos(GL_PI/180*angle);
}矩阵绕Z轴旋转：同上直接套图1.4公式，空间信息存成列主序列：
????????????????????????????????????????????????????????????????????????图1.4
程序1.4
void Matrix3D::ZRotate(float angle)
{
m[0][0] = cos(GL_PI / 180 * angle); m[0][1] = sin(GL_PI / 180 * angle);
m[1][0] = -sin(GL_PI / 180 * angle);m[1][1] = cos(GL_PI / 180 * angle);
}矩阵平移：直接往矩阵的设置三个空间信息就可以，这个没难度。
程序1.5
void Matrix3D::Translate(float x, float y, float z)
{
m[3][0] = x;
m[3][1] = y;
m[3][2] = z;
}
矩阵缩放：直接往矩阵的设置三个空间信息就可以，这个没难度。
程序1.6
void Matrix3D::Scale(float x, float y, float z)
{
m[0][0] *= x;
m[1][1] *= y;
m[2][2] *= z;
}矩阵绕XYZ翻转：我们使用的方法是让x,y,z的旋转角度直接*-1 空间就翻转过来了。
程序1.7
void Matrix3D::FlipX()
{
m[1][1] *=-1;
}
void Matrix3D::FlipY()
{
m[0][0] *= -1;
}
void Matrix3D::FlipZ()
{
m[3][3] *= -1;
}代码太多了，还有一些复杂的运算，到时候有时间了再把矩阵求逆和矩阵转置讲一下，还有把摄像机的俯仰角算成矩阵的讲一下。剩下的都是些简单的运算就自己研究下把。先把代码贴出来。
GLMath.h
#pragma once
#define PI 3.141596
class Vector2
{
private:
public:
int PointNum=0;
float x, y;
Vector2(float x, float y) { this->x = x;this->y = y; }
void setXy(float x, float y) { this->x = x; this->y = y; }
Vector2 &operator /= (float n)
{
this->x /= n;
this->y /= n;
return *this;
}
Vector2& operator *= (float n)
{
this->x *= n;
this->y *= n;
return *this;
}
Vector2& operator += (Vector2 &vec)
{
this->x += vec.x;
this->y += vec.y;
return *this;
}
Vector2& operator -= (Vector2 &vec)
{
this->x -= vec.x;
this->y -= vec.y;
return *this;
}
Vector2 operator* (float n)
{
Vector2 temp = *this;
temp.x *= n;
temp.y *= n;
return temp;
}
Vector2 operator/ (float n)
{
Vector2 temp = *this;
temp.x /= n;
temp.y /= n;
return temp;
}
Vector2 operator+ (Vector2 vec)
{
Vector2 temp = *this;
temp.x += vec.x;
temp.y += vec.y;
return temp;
}
Vector2 operator- (Vector2 vec)
{
Vector2 temp = *this;
temp.x -= vec.x;
temp.y -= vec.y;
return temp;
}
void normalize()
{
float len = sqrt(x*x + y*y);
x /= len;
y /= len;
}
Vector2 Normalize()
{
Vector2 temp = *this;
float len = sqrt(temp.x*temp.x + temp.y*temp.y);//单位化
temp.x /= len;
temp.y /= len;
return temp;
}
float length()
{
return sqrt(x*x + y*y);
}
float length2q()
{
return x*x + y*y;
}
float dot(Vector2 &vec)//点乘
{
return x*vec.x +y*vec.y;
}
Vector2 projection(Vector2&vec)//投影
{
Vector2 temp;
temp = vec.Normalize();
Vector2 temp1;
temp1 = temp*dot(temp);
return temp1;
}
Vector2 ortho()//正交
{
Vector2 temp=*this;
temp.x = y;
temp.y = -x;
return temp;
}
float Cross(Vector2 &vec)//叉乘
{
Vector2 a = *this;
return a.x * vec.x - a.y * vec.y;
}
Vector2::Vector2() { x = 0, y = 0; }
Vector2::~Vector2() {}
};
class Vector3D
{
public:
float x, y, z;
Vector3D(float x, float y, float z) {this->x = x; this->y = y;this->z = z;}
void setXYZ(float x, float y, float z){this->x = x; this->y = y; this->z = z;}
void normalize()
{
float len = sqrt(x*x + y*y+z*z);
x /= len;
y /= len;
z /= len;
}
Vector3D Normalize()
{
Vector3D temp=*this;
float len = sqrt(x*x + y*y + z*z);
temp.x /= len;
temp.y /= len;
temp.z /= len;
return temp;
}
float length()
{
return sqrt(x*x + y*y+z*z);
}
float length2q()
{
return x*x + y*y + z*z;
}
float dot(Vector3D &vec)//点乘
{
return x*vec.x +y*vec.y +z*vec.z;
}
Vector3D &operator /= (float n)
{
this->x /= n;
this->y /= n;
this->z /= n;
return *this;
}
Vector3D& operator *= (float n)
{
this->x *= n;
this->y *= n;
this->z *= n;
return *this;
}
Vector3D& operator += (Vector3D &vec)
{
this->x += vec.x;
this->y += vec.y;
this->z += vec.z;
return *this;
}
Vector3D& operator -= (Vector3D &vec)
{
this->x -= vec.x;
this->y -= vec.y;
this->z -= vec.z;
return *this;
}
Vector3D operator* (float n)
{
Vector3D temp = *this;
temp.x *= n;
temp.y *= n;
temp.z *= n;
return temp;
}
Vector3D operator/ (float n)
{
Vector3D temp = *this;
temp.x /= n;
temp.y /= n;
temp.z /= n;
return temp;
}
Vector3D operator+ (Vector3D vec)
{
Vector3D temp = *this;
temp.x += vec.x;
temp.y += vec.y;
temp.z += vec.z;
return temp;
}
Vector3D operator+(float n)
{
Vector3D temp = *this;
temp.x += n;
temp.y += n;
temp.z += n;
return temp;
}
Vector3D operator-()
{
Vector3D vec ;
Vector3D temp = *this;
vec.x =-temp.x;
vec.y=-temp.y;
vec.z = -temp.z;
return vec;
}
Vector3D operator- (Vector3D vec)
{
Vector3D temp = *this;
temp.x -= vec.x;
temp.y -= vec.y;
temp.z -= vec.z;
return temp;
}
bool operator == (Vector3D &vec)
{
if (this->x == vec.x&&this->y == vec.y&&this->z==vec.z)
return true;
else
return false;
}
bool operator >= (Vector3D &vec)
{
if (this->x >=vec.x&&this->y>=vec.y&&this->z>=vec.z)
return true;
else
return false;
}
bool operator <= (Vector3D &vec)
{
if (this->x <= vec.x&&this->y <= vec.y&&this->z<=vec.z)
return true;
else
return false;
}
bool operator %=(Vector3D &vec)
{
if (this->x <= vec.x&&this->y <= vec.y&&this->z >=vec.z)
return true;
else
return false;
}
int operator || (Vector3D &vec) //判断距离差的是正还是负
{
if (vec.x > 0 && vec.z > 0)
return 1;
else if(vec.x < 0 && vec.z < 0)
return -1;
else
return 0;
}
Vector3D projection(Vector3D&vec)//投影
{
Vector3D temp;
temp = vec.Normalize();
Vector3D temp1;
temp1 = temp*dot(temp);
return temp1;
}
Vector3D cross(Vector3D &v)
{
Vector3D temp;
temp.x = y * v.z - z * v.y;
temp.y = z * v.x - x * v.z;
temp.z = x * v.y - y * v.x;
return temp;
}
Vector3D::Vector3D() {}
Vector3D::~Vector3D() {}
};
Vector3D operator*(Vector3D vc1, Vector3D vc2);
struct Vector4D
{
public:
float x, y, z, w;
Vector4D(float x, float y, float z, float w)
{
this->x = x;this->y = y;
this->z = z;this->w = w;
}
void setXYZW(float x, float y, float z, float w)
{
this->x = x;this->y = y;
this->z = z;this->w = w;
}
Vector4D() {}
~Vector4D() {}
};
class Matrix3
{
public:
union {
struct {
float _11, _21, _31,_41; //_11,_12,_13,_14 |
float _12, _22, _32,_42; //_21,_22,_23,_24 |转置后的矩阵
float _13, _23, _33,_43; //_31,_32,_33,_34 |
float _14, _24, _34,_44; //_41,_42,_43,_44 |
}; //openGL用的是行矩阵，但是是列的算法
float mat[16];
float m[4][4];
};
Matrix3()
{
for (int i = 0; i < 4; ++i)
for (int j = 0; j < 4; ++j)
if (i == j)
m[i][j] = 1;
else
m[i][j] = 0;
}
Matrix3 operator*(Matrix3& mat);//矩阵*矩阵
Matrix3 &operator*=(const Matrix3 &mat); //矩阵与矩阵
Vector2 operator*(const Vector2 &v); //矩阵与向量
Matrix3 &Rotate(float angle); //绕Z轴旋转
Matrix3 &Translate(float x, float y,float z); //平移
Matrix3 &Translate(float x, float y);
Matrix3 &Scale(float x, float y); //比例
Matrix3 &FlipX(); //x轴翻转
Matrix3 &FlipY(); //y轴翻转
void Identity()//矩阵单位化
{
m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0;
m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = 0;
m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
}
~Matrix3();
};
Matrix3 operator*(const Matrix3 &mat1, const Matrix3 &mat2); //矩阵与矩阵
Vector2 operator*(const Vector2 &v, const Matrix3 &mat); //矩阵与向量
Matrix3 &Rotate(Matrix3 &mat, float angle); //绕Z轴旋转
Matrix3 &Translate(Matrix3 &mat, float x, float y,float z); //平移
Matrix3 &Translate(Matrix3 &mat, float x, float y);
Matrix3 &Scale(Matrix3 &mat, float x, float y); //比例
Matrix3 &FlipX(Matrix3 &mat); //x轴翻转
Matrix3 &FlipY(Matrix3 &mat); //y轴翻转
void Vec2TransformCoord(Vector2 &newVec, Vector2 &cornerPoint, Matrix3 &world_matrix);
//************3D*******************
class Matrix3D
{
public:
union {
struct {
float _11, _21, _31, _41; //_11,_12,_13,_14 |
float _12, _22, _32, _42; //_21,_22,_23,_24 |转置后的矩阵
float _13, _23, _33, _43; //_31,_32,_33,_34 |
float _14, _24, _34, _44; //_41,_42,_43,_44 |
}; //openGL用的是行矩阵，但是是列的算法
float mat[16];
float m[4][4];
};
Matrix3D()
{
m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0;
m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = 0;
m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
}
Matrix3D operator*(Matrix3D &mat);//矩阵*矩阵
Matrix3D &operator*=(Matrix3D &mat); //矩阵与矩阵
Vector3D operator*(Vector3D &v); //矩阵与向量
Matrix3D MatrixInverse(Matrix3D &m); //逆矩阵
void XRotate(float angle); //绕X轴旋转
void YRotate(float angle); //绕Y轴旋转
void ZRotate(float angle); //绕Z轴旋转
void Translate(float x, float y, float z);//平移
void Scale(float x, float y, float z); //比例
void FlipX(); //x轴翻转
void FlipY(); //y轴翻转
void FlipZ(); //z翻转
void Identity()//矩阵单位化
{
m[0][0] = 1; m[0][1] = 0; m[0][2] = 0; m[0][3] = 0;
m[1][0] = 0; m[1][1] = 1; m[1][2] = 0; m[1][3] = 0;
m[2][0] = 0; m[2][1] = 0; m[2][2] = 1; m[2][3] = 0;
m[3][0] = 0; m[3][1] = 0; m[3][2] = 0; m[3][3] = 1;
}
~Matrix3D(){};
};
Matrix3D MatrixRotationAxis(Vector3D &axis, float angle);
Matrix3D MatrixRotationAxisRad(Vector3D &axis, float radian);
Vector3D Vec3TransformCoord(Matrix3D &m, Vector3D &v);
Vector2 Vec3TransformCoord(Matrix3D &m, Vector2 &v);
Vector3D Vec3TransformNormal(Matrix3D &m, Vector3D &v);
Matrix3D YRotate(float angle); //绕Y轴旋转(全局函数)
Matrix3D MatrixTranspose(Matrix3D &m); //转置矩阵
Matrix3D MatrixInverse(Matrix3D &m); //逆矩阵
Matrix3D FromToRotation(Vector3D from, Vector3D to);
Matrix3D &MatrixRotationYawPitchRoll(float Yaw, float Pitch, float Roll);
Matrix3D &MatrixRotationY(float angle);
struct sColor
{
public:
union {
struct {
float r, g, b, a;
};
float color[4];//r,g,b,a的分量
};
sColor()//默认颜色
{
color[0] = 1.0f;
color[1] = 1.0f;
color[2] = 1.0f;
color[3] = 1.0f;
}
sColor(float r, float g, float b, float a)//外部初始化颜色
{
color[0] = r;
color[1] = g;
color[2] = b;
color[3] = a;
}
float getR() { return color[0]; }
float getG() { return color[1]; }
float getB() { return color[2]; }
float getA() { return color[3]; }
void operator=(DWORD col)
{
color[0] = (col >> 16 & 0xff)*0.00392156862;
color[1] = (col >> 8 & 0xff)*0.00392156862;
color[2] = (col & 0xff)*0.00392156862;
color[3] = (col >> 24)*0.00392156862;
}
void operator+=(float z)
{
r +=z; g +=z; b +=z;
}
bool operator==(float z)
{
if (r == z&&g == z&&b == z)
return true;
return false;
}
bool operator>=(float z)
{
if (r>=z&&g>=z&&b>=z)
return true;
return false;
}
bool operator <= (float z)
{
if (r <= z&&g <= z&&b <= z)
return true;
return false;
}
};
GLMath.cpp
#include "Engine.h"
Vector2 Matrix3::operator*(const Vector2 &v)
{
return Vector2(v.x*m[0][0] + v.y*m[1][0] + m[3][0], v.x*m[0][1] + v.y*m[1][1] + m[3][1]);
}
Matrix3 Matrix3::operator*(Matrix3&mat)
{
Matrix3 temp;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
temp.m[i][j] =
m[i][0] * mat.m[0][j] +
m[i][1] * mat.m[1][j] +
m[i][2] * mat.m[2][j] +
m[i][3] * mat.m[3][j];
}
}
return temp;
}
Matrix3 &Matrix3::operator*=(const Matrix3 &mat)
{
Matrix3 cal=*this;
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
m[i][j] =
cal.m[i][0] * mat.m[0][j] +
cal.m[i][1] * mat.m[1][j] +
cal.m[i][2] * mat.m[2][j] +
cal.m[i][3] * mat.m[3][j];
return *this;
}
Matrix3 &Matrix3::Rotate(float angle) //绕Z轴旋转
{
return ::Rotate(*this, angle);
}
Matrix3 &Matrix3::Translate(float x, float y,float z) //平移
{
return ::Translate(*this, x, y,z);
}
Matrix3 &Matrix3::Translate(float x, float y)
{
return ::Translate(*this, x, y);
}
Matrix3 &Matrix3::Scale(float x, float y) //比例
{
return ::Scale(*this, x, y);
}
Matrix3 &Matrix3::FlipX() //x轴翻转
{
return ::FlipX(*this);
}
Matrix3 &Matrix3::FlipY() //y轴翻转
{
return ::FlipY(*this);
}
void Vec2TransformCoord(Vector2 &newVec, Vector2 &cornerPoint, Matrix3 &world_matrix)
{
Vector2 temp;
temp.x = cornerPoint.x*world_matrix.m[0][0]+ cornerPoint.y*world_matrix.m[0][1]+ world_matrix.m[3][0];
temp.y= cornerPoint.x*world_matrix.m[1][0] + cornerPoint.y*world_matrix.m[1][1]+ world_matrix.m[3][1];
newVec = temp;
}
Matrix3::~Matrix3()
{
}
//以上为类成员函数
//以下为全局函数
Matrix3 operator*(const Matrix3 &mat1, const Matrix3 &mat2)
{
Matrix3 temp;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
temp.m[i][j] =
mat1.m[i][0] * mat2.m[0][j] +
mat1.m[i][1] * mat2.m[1][j] +
mat1.m[i][2] * mat2.m[2][j] +
mat1.m[i][3] * mat2.m[3][j];
}
}
return temp;
}
Vector2 operator*(const Vector2 &v, const Matrix3 &mat)
{
return Vector2(v.x*mat.m[0][0] + v.y*mat.m[1][0] + mat.m[3][0], v.x*mat.m[0][1] + v.y*mat.m[1][1] + mat.m[3][1]);
}
Matrix3 &Rotate(Matrix3 &mat, float angle)
{
mat.m[0][0] = cos(GL_PI / 180 * angle); mat.m[0][1] = sin(GL_PI / 180 * angle);
mat.m[1][0] = -1 * sin(GL_PI / 180 * angle); mat.m[1][1] = cos(GL_PI / 180 * angle);
return mat;
}
Matrix3 &Translate(Matrix3 &mat, float x, float y,float z)
{
mat.m[3][0] = x;
mat.m[3][1] = y;
mat.m[3][2] = z;
return mat;
}
Matrix3 &Translate(Matrix3 &mat, float x, float y)
{
mat.m[3][0] = x;
mat.m[3][1] = y;
return mat;
}
Matrix3 &Scale(Matrix3 &mat, float x, float y)
{
mat.m[0][0] *= x;
mat.m[1][1] *= y;
return mat;
}
Matrix3 &FlipX(Matrix3 &mat) //x轴翻转
{
mat.m[1][1] *= -1;
return mat;
}
Matrix3 &FlipY(Matrix3 &mat) //y轴翻转
{
mat.m[0][0] *= -1;
return mat;
}
//************3D******************
Matrix3D Matrix3D::operator*(Matrix3D &mat)
{
Matrix3D temp;
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
temp.m[i][j] =
m[i][0] * mat.m[0][j] +
m[i][1] * mat.m[1][j] +
m[i][2] * mat.m[2][j] +
m[i][3] * mat.m[3][j];
}
}
return temp;
}
Matrix3D &Matrix3D::operator*=(Matrix3D &mat)
{
Matrix3D cal = *this;
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
m[i][j] =
cal.m[i][0] * mat.m[0][j] +
cal.m[i][1] * mat.m[1][j] +
cal.m[i][2] * mat.m[2][j] +
cal.m[i][3] * mat.m[3][j];
return *this;
}
Vector3D Matrix3D::operator*(Vector3D&v)
{
Vector3D res;
res.x = m[0][0]*v.x+m[1][0]*v.y+m[2][0]*v.z ;
res.y = m[0][1]*v.x+m[1][1]*v.y+m[2][2]*v.z;
res.z = m[0][2]*v.x+m[1][2]*v.y+m[2][3]*v.z;
return res;
}
Vector3D operator*(Vector3D v1, Vector3D v2)
{
Vector3D temp;
temp.x=v1.x*=v2.x;
temp.y=v1.y*=v2.y;
temp.z=v1.z*=v2.z;
return temp;
}
void Matrix3D::XRotate(float angle)
{
m[1][1] =cos(GL_PI/180*angle) ; m[1][2] =sin(GL_PI/180*angle) ;
m[2][1] =-sin(GL_PI/180*angle); m[2][2] =cos(GL_PI/180*angle) ;
}
void Matrix3D::YRotate(float angle)
{
m[0][0] =cos(GL_PI/180*angle); m[0][2] =-sin(GL_PI/180*angle) ;
m[2][0] = sin(GL_PI / 180 * angle); m[2][2] =cos(GL_PI/180*angle);
}
void Matrix3D::ZRotate(float angle)
{
m[0][0] = cos(GL_PI / 180 * angle); m[0][1] = sin(GL_PI / 180 * angle);
m[1][0] = -sin(GL_PI / 180 * angle);m[1][1] = cos(GL_PI / 180 * angle);
}
void Matrix3D::Translate(float x, float y, float z)
{
m[3][0] = x;
m[3][1] = y;
m[3][2] = z;
}
void Matrix3D::Scale(float x, float y, float z)
{
m[0][0] *= x;
m[1][1] *= y;
m[2][2] *= z;
}
void Matrix3D::FlipX()
{
m[1][1] *=-1;
}
void Matrix3D::FlipY()
{
m[0][0] *= -1;
}
void Matrix3D::FlipZ()
{
m[3][3] *= -1;
}
Matrix3D Matrix3D::MatrixInverse(Matrix3D &m)
{
float fTemp[12], fDet;
Matrix3D mTrans = MatrixTranspose(m);
fTemp[0] = mTrans.mat[10] * mTrans.mat[15];
fTemp[1] = mTrans.mat[11] * mTrans.mat[14];
fTemp[2] = mTrans.mat[9] * mTrans.mat[15];
fTemp[3] = mTrans.mat[11] * mTrans.mat[13];
fTemp[4] = mTrans.mat[9] * mTrans.mat[14];
fTemp[5] = mTrans.mat[10] * mTrans.mat[13];
fTemp[6] = mTrans.mat[8] * mTrans.mat[15];
fTemp[7] = mTrans.mat[11] * mTrans.mat[12];
fTemp[8] = mTrans.mat[8] * mTrans.mat[14];
fTemp[9] = mTrans.mat[10] * mTrans.mat[12];
fTemp[10] = mTrans.mat[8] * mTrans.mat[13];
fTemp[11] = mTrans.mat[9] * mTrans.mat[12];
Matrix3D res;
res.mat[0] = fTemp[0] * mTrans.mat[5] + fTemp[3] * mTrans.mat[6] + fTemp[4] * mTrans.mat[7];
res.mat[0] -= fTemp[1] * mTrans.mat[5] + fTemp[2] * mTrans.mat[6] + fTemp[5] * mTrans.mat[7];
res.mat[1] = fTemp[1] * mTrans.mat[4] + fTemp[6] * mTrans.mat[6] + fTemp[9] * mTrans.mat[7];
res.mat[1] -= fTemp[0] * mTrans.mat[4] + fTemp[7] * mTrans.mat[6] + fTemp[8] * mTrans.mat[7];
res.mat[2] = fTemp[2] * mTrans.mat[4] + fTemp[7] * mTrans.mat[5] + fTemp[10] * mTrans.mat[7];
res.mat[2] -= fTemp[3] * mTrans.mat[4] + fTemp[6] * mTrans.mat[5] + fTemp[11] * mTrans.mat[7];
res.mat[3] = fTemp[5] * mTrans.mat[4] + fTemp[8] * mTrans.mat[5] + fTemp[11] * mTrans.mat[6];
res.mat[3] -= fTemp[4] * mTrans.mat[4] + fTemp[9] * mTrans.mat[5] + fTemp[10] * mTrans.mat[6];
res.mat[4] = fTemp[1] * mTrans.mat[1] + fTemp[2] * mTrans.mat[2] + fTemp[5] * mTrans.mat[3];
res.mat[4] -= fTemp[0] * mTrans.mat[1] + fTemp[3] * mTrans.mat[2] + fTemp[4] * mTrans.mat[3];
res.mat[5] = fTemp[0] * mTrans.mat[0] + fTemp[7] * mTrans.mat[2] + fTemp[8] * mTrans.mat[3];
res.mat[5] -= fTemp[1] * mTrans.mat[0] + fTemp[6] * mTrans.mat[2] + fTemp[9] * mTrans.mat[3];
res.mat[6] = fTemp[3] * mTrans.mat[0] + fTemp[6] * mTrans.mat[1] + fTemp[11] * mTrans.mat[3];
res.mat[6] -= fTemp[2] * mTrans.mat[0] + fTemp[7] * mTrans.mat[1] + fTemp[10] * mTrans.mat[3];
res.mat[7] = fTemp[4] * mTrans.mat[0] + fTemp[9] * mTrans.mat[1] + fTemp[10] * mTrans.mat[2];
res.mat[7] -= fTemp[5] * mTrans.mat[0] + fTemp[8] * mTrans.mat[1] + fTemp[11] * mTrans.mat[2];
fTemp[0] = mTrans.mat[2] * mTrans.mat[7];
fTemp[1] = mTrans.mat[3] * mTrans.mat[6];
fTemp[2] = mTrans.mat[1] * mTrans.mat[7];
fTemp[3] = mTrans.mat[3] * mTrans.mat[5];
fTemp[4] = mTrans.mat[1] * mTrans.mat[6];
fTemp[5] = mTrans.mat[2] * mTrans.mat[5];
fTemp[6] = mTrans.mat[0] * mTrans.mat[7];
fTemp[7] = mTrans.mat[3] * mTrans.mat[4];
fTemp[8] = mTrans.mat[0] * mTrans.mat[6];
fTemp[9] = mTrans.mat[2] * mTrans.mat[4];
fTemp[10] = mTrans.mat[0] * mTrans.mat[5];
fTemp[11] = mTrans.mat[1] * mTrans.mat[4];
res.mat[8] = fTemp[0] * mTrans.mat[13] + fTemp[3] * mTrans.mat[14] + fTemp[4] * mTrans.mat[15];
res.mat[8] -= fTemp[1] * mTrans.mat[13] + fTemp[2] * mTrans.mat[14] + fTemp[5] * mTrans.mat[15];
res.mat[9] = fTemp[1] * mTrans.mat[12] + fTemp[6] * mTrans.mat[14] + fTemp[9] * mTrans.mat[15];
res.mat[9] -= fTemp[0] * mTrans.mat[12] + fTemp[7] * mTrans.mat[14] + fTemp[8] * mTrans.mat[15];
res.mat[10] = fTemp[2] * mTrans.mat[12] + fTemp[7] * mTrans.mat[13] + fTemp[10] * mTrans.mat[15];
res.mat[10] -= fTemp[3] * mTrans.mat[12] + fTemp[6] * mTrans.mat[13] + fTemp[11] * mTrans.mat[15];
res.mat[11] = fTemp[5] * mTrans.mat[12] + fTemp[8] * mTrans.mat[13] + fTemp[11] * mTrans.mat[14];
res.mat[11] -= fTemp[4] * mTrans.mat[12] + fTemp[9] * mTrans.mat[13] + fTemp[10] * mTrans.mat[14];
res.mat[12] = fTemp[2] * mTrans.mat[10] + fTemp[5] * mTrans.mat[11] + fTemp[1] * mTrans.mat[9];
res.mat[12] -= fTemp[4] * mTrans.mat[11] + fTemp[0] * mTrans.mat[9] + fTemp[3] * mTrans.mat[10];
res.mat[13] = fTemp[8] * mTrans.mat[11] + fTemp[0] * mTrans.mat[8] + fTemp[7] * mTrans.mat[10];
res.mat[13] -= fTemp[6] * mTrans.mat[10] + fTemp[9] * mTrans.mat[11] + fTemp[1] * mTrans.mat[8];
res.mat[14] = fTemp[6] * mTrans.mat[9] + fTemp[11] * mTrans.mat[11] + fTemp[3] * mTrans.mat[8];
res.mat[14] -= fTemp[10] * mTrans.mat[11] + fTemp[2] * mTrans.mat[8] + fTemp[7] * mTrans.mat[9];
res.mat[15] = fTemp[10] * mTrans.mat[10] + fTemp[4] * mTrans.mat[8] + fTemp[9] * mTrans.mat[9];
res.mat[15] -= fTemp[8] * mTrans.mat[9] + fTemp[11] * mTrans.mat[10] + fTemp[5] * mTrans.mat[8];
fDet = mTrans.mat[0] * res.mat[0] +
mTrans.mat[1] * res.mat[1] +
mTrans.mat[2] * res.mat[2] +
mTrans.mat[3] * res.mat[3];
fDet = 1 / fDet;
res.mat[0] *= fDet;
res.mat[1] *= fDet;
res.mat[2] *= fDet;
res.mat[3] *= fDet;
res.mat[4] *= fDet;
res.mat[5] *= fDet;
res.mat[6] *= fDet;
res.mat[7] *= fDet;
res.mat[8] *= fDet;
res.mat[9] *= fDet;
res.mat[10] *= fDet;
res.mat[11] *= fDet;
res.mat[12] *= fDet;
res.mat[13] *= fDet;
res.mat[14] *= fDet;
res.mat[15] *= fDet;
return res;
}
Vector3D Vec3TransformCoord(Matrix3D &m, Vector3D &v)
{
Vector3D res;
res.x = m.m[0][0] * v.x + m.m[1][0] * v.y + m.m[2][0] * v.z+m.m[3][0];
res.y = m.m[0][1] * v.x + m.m[1][1] * v.y + m.m[2][1] * v.z+m.m[3][1];
res.z = m.m[0][2] * v.x + m.m[1][2] * v.y + m.m[2][2] * v.z+m.m[3][2];
return res;
}
Vector2 Vec3TransformCoord(Matrix3D &worldMat, Vector2 &v)
{
Vector2 temp;
temp.x = v.x*worldMat.m[0][0] + v.y*worldMat.m[1][0] + worldMat.m[3][0];
temp.y = v.x*worldMat.m[0][1] + v.y*worldMat.m[1][1] + worldMat.m[3][1];
return temp;
}
Vector3D Vec3TransformNormal(Matrix3D &m, Vector3D &v)
{
Vector3D res;
res.x = m.m[0][0] * v.x + m.m[1][0] * v.y + m.m[2][0] * v.z;
res.y = m.m[0][1] * v.x + m.m[1][1] * v.y + m.m[2][1] * v.z;
res.z = m.m[0][2] * v.x + m.m[1][2] * v.y + m.m[2][2] * v.z;
return res;
}
Matrix3D MatrixRotationAxis(Vector3D &axis, float angle)
{
float s = sin(angle*PI / 180);
float c = cos(angle*PI / 180);
Vector3D v = axis.Normalize();
Vector3D a = v*(1.0f - c);
Matrix3D res;
res.m[0][0] = a.x*v.x + c; res.m[0][1] = a.y*v.x - v.z*s; res.m[0][2] = a.z*v.x + v.y*s; res.m[0][3] = 0;
res.m[1][0] = a.x*v.y + v.z*s; res.m[1][1] = a.y*v.y + c; res.m[1][2] = a.z*v.y - v.x*s; res.m[1][3] = 0.0f;
res.m[2][0] = a.x*v.z - v.y*s; res.m[2][1] = a.y*v.z + v.x*s; res.m[2][2] = a.z*v.z + c; res.m[2][3] = 0;
res.m[3][0] = 0; res.m[3][1] = 0; res.m[3][2] = 0; res.m[3][3] = 1;
return res;
}
Matrix3D MatrixRotationAxisRad(Vector3D &axis, float radian)
{
float s = sin(radian);
float c = cos(radian);
Vector3D v = axis.Normalize();
Vector3D a = v*(1.0f - c);
Matrix3D res;
res.m[0][0] = a.x*v.x + c; res.m[0][1] = a.y*v.x - v.z*s; res.m[0][2] = a.z*v.x + v.y*s; res.m[0][3] = 0;
res.m[1][0] = a.x*v.y + v.z*s; res.m[1][1] = a.y*v.y + c; res.m[1][2] = a.z*v.y - v.x*s; res.m[1][3] = 0.0f;
res.m[2][0] = a.x*v.z - v.y*s; res.m[2][1] = a.y*v.z + v.x*s; res.m[2][2] = a.z*v.z + c; res.m[2][3] = 0;
res.m[3][0] = 0; res.m[3][1] = 0; res.m[3][2] = 0; res.m[3][3] = 1;
return res;
}
Matrix3D YRotate(float angle)
{
Matrix3D mat;
mat.m[0][0] = cos(GL_PI / 180 * angle); mat.m[0][2] = sin(GL_PI / 180 * angle);
mat.m[2][0] = -sin(GL_PI / 180 * angle); mat.m[2][2] = cos(GL_PI / 180 * angle);
return mat;
}
Matrix3D MatrixTranspose(Matrix3D &m)
{
Matrix3D res;
res.mat[0] = m.mat[0]; res.mat[1] = m.mat[4]; res.mat[2] = m.mat[8]; res.mat[3] = m.mat[12];
res.mat[4] = m.mat[1]; res.mat[5] = m.mat[5]; res.mat[6] = m.mat[9]; res.mat[7] = m.mat[13];
res.mat[8] = m.mat[2]; res.mat[9] = m.mat[6]; res.mat[10] = m.mat[10]; res.mat[11] = m.mat[14];
res.mat[12] = m.mat[3]; res.mat[13] = m.mat[7]; res.mat[14] = m.mat[11]; res.mat[15] = m.mat[15];
return res;
}
Matrix3D MatrixInverse(Matrix3D &m)
{
float fTemp[12], fDet;
Matrix3D mTrans = MatrixTranspose(m);
fTemp[0] = mTrans.mat[10] * mTrans.mat[15];
fTemp[1] = mTrans.mat[11] * mTrans.mat[14];
fTemp[2] = mTrans.mat[9] * mTrans.mat[15];
fTemp[3] = mTrans.mat[11] * mTrans.mat[13];
fTemp[4] = mTrans.mat[9] * mTrans.mat[14];
fTemp[5] = mTrans.mat[10] * mTrans.mat[13];
fTemp[6] = mTrans.mat[8] * mTrans.mat[15];
fTemp[7] = mTrans.mat[11] * mTrans.mat[12];
fTemp[8] = mTrans.mat[8] * mTrans.mat[14];
fTemp[9] = mTrans.mat[10] * mTrans.mat[12];
fTemp[10] = mTrans.mat[8] * mTrans.mat[13];
fTemp[11] = mTrans.mat[9] * mTrans.mat[12];
Matrix3D res;
res.mat[0] = fTemp[0] * mTrans.mat[5] + fTemp[3] * mTrans.mat[6] + fTemp[4] * mTrans.mat[7];
res.mat[0] -= fTemp[1] * mTrans.mat[5] + fTemp[2] * mTrans.mat[6] + fTemp[5] * mTrans.mat[7];
res.mat[1] = fTemp[1] * mTrans.mat[4] + fTemp[6] * mTrans.mat[6] + fTemp[9] * mTrans.mat[7];
res.mat[1] -= fTemp[0] * mTrans.mat[4] + fTemp[7] * mTrans.mat[6] + fTemp[8] * mTrans.mat[7];
res.mat[2] = fTemp[2] * mTrans.mat[4] + fTemp[7] * mTrans.mat[5] + fTemp[10] * mTrans.mat[7];
res.mat[2] -= fTemp[3] * mTrans.mat[4] + fTemp[6] * mTrans.mat[5] + fTemp[11] * mTrans.mat[7];
res.mat[3] = fTemp[5] * mTrans.mat[4] + fTemp[8] * mTrans.mat[5] + fTemp[11] * mTrans.mat[6];
res.mat[3] -= fTemp[4] * mTrans.mat[4] + fTemp[9] * mTrans.mat[5] + fTemp[10] * mTrans.mat[6];
res.mat[4] = fTemp[1] * mTrans.mat[1] + fTemp[2] * mTrans.mat[2] + fTemp[5] * mTrans.mat[3];
res.mat[4] -= fTemp[0] * mTrans.mat[1] + fTemp[3] * mTrans.mat[2] + fTemp[4] * mTrans.mat[3];
res.mat[5] = fTemp[0] * mTrans.mat[0] + fTemp[7] * mTrans.mat[2] + fTemp[8] * mTrans.mat[3];
res.mat[5] -= fTemp[1] * mTrans.mat[0] + fTemp[6] * mTrans.mat[2] + fTemp[9] * mTrans.mat[3];
res.mat[6] = fTemp[3] * mTrans.mat[0] + fTemp[6] * mTrans.mat[1] + fTemp[11] * mTrans.mat[3];
res.mat[6] -= fTemp[2] * mTrans.mat[0] + fTemp[7] * mTrans.mat[1] + fTemp[10] * mTrans.mat[3];
res.mat[7] = fTemp[4] * mTrans.mat[0] + fTemp[9] * mTrans.mat[1] + fTemp[10] * mTrans.mat[2];
res.mat[7] -= fTemp[5] * mTrans.mat[0] + fTemp[8] * mTrans.mat[1] + fTemp[11] * mTrans.mat[2];
fTemp[0] = mTrans.mat[2] * mTrans.mat[7];
fTemp[1] = mTrans.mat[3] * mTrans.mat[6];
fTemp[2] = mTrans.mat[1] * mTrans.mat[7];
fTemp[3] = mTrans.mat[3] * mTrans.mat[5];
fTemp[4] = mTrans.mat[1] * mTrans.mat[6];
fTemp[5] = mTrans.mat[2] * mTrans.mat[5];
fTemp[6] = mTrans.mat[0] * mTrans.mat[7];
fTemp[7] = mTrans.mat[3] * mTrans.mat[4];
fTemp[8] = mTrans.mat[0] * mTrans.mat[6];
fTemp[9] = mTrans.mat[2] * mTrans.mat[4];
fTemp[10] = mTrans.mat[0] * mTrans.mat[5];
fTemp[11] = mTrans.mat[1] * mTrans.mat[4];
res.mat[8] = fTemp[0] * mTrans.mat[13] + fTemp[3] * mTrans.mat[14] + fTemp[4] * mTrans.mat[15];
res.mat[8] -= fTemp[1] * mTrans.mat[13] + fTemp[2] * mTrans.mat[14] + fTemp[5] * mTrans.mat[15];
res.mat[9] = fTemp[1] * mTrans.mat[12] + fTemp[6] * mTrans.mat[14] + fTemp[9] * mTrans.mat[15];
res.mat[9] -= fTemp[0] * mTrans.mat[12] + fTemp[7] * mTrans.mat[14] + fTemp[8] * mTrans.mat[15];
res.mat[10] = fTemp[2] * mTrans.mat[12] + fTemp[7] * mTrans.mat[13] + fTemp[10] * mTrans.mat[15];
res.mat[10] -= fTemp[3] * mTrans.mat[12] + fTemp[6] * mTrans.mat[13] + fTemp[11] * mTrans.mat[15];
res.mat[11] = fTemp[5] * mTrans.mat[12] + fTemp[8] * mTrans.mat[13] + fTemp[11] * mTrans.mat[14];
res.mat[11] -= fTemp[4] * mTrans.mat[12] + fTemp[9] * mTrans.mat[13] + fTemp[10] * mTrans.mat[14];
res.mat[12] = fTemp[2] * mTrans.mat[10] + fTemp[5] * mTrans.mat[11] + fTemp[1] * mTrans.mat[9];
res.mat[12] -= fTemp[4] * mTrans.mat[11] + fTemp[0] * mTrans.mat[9] + fTemp[3] * mTrans.mat[10];
res.mat[13] = fTemp[8] * mTrans.mat[11] + fTemp[0] * mTrans.mat[8] + fTemp[7] * mTrans.mat[10];
res.mat[13] -= fTemp[6] * mTrans.mat[10] + fTemp[9] * mTrans.mat[11] + fTemp[1] * mTrans.mat[8];
res.mat[14] = fTemp[6] * mTrans.mat[9] + fTemp[11] * mTrans.mat[11] + fTemp[3] * mTrans.mat[8];
res.mat[14] -= fTemp[10] * mTrans.mat[11] + fTemp[2] * mTrans.mat[8] + fTemp[7] * mTrans.mat[9];
res.mat[15] = fTemp[10] * mTrans.mat[10] + fTemp[4] * mTrans.mat[8] + fTemp[9] * mTrans.mat[9];
res.mat[15] -= fTemp[8] * mTrans.mat[9] + fTemp[11] * mTrans.mat[10] + fTemp[5] * mTrans.mat[8];
fDet = mTrans.mat[0] * res.mat[0] +
mTrans.mat[1] * res.mat[1] +
mTrans.mat[2] * res.mat[2] +
mTrans.mat[3] * res.mat[3];
fDet = 1 / fDet;
res.mat[0] *= fDet;
res.mat[1] *= fDet;
res.mat[2] *= fDet;
res.mat[3] *= fDet;
res.mat[4] *= fDet;
res.mat[5] *= fDet;
res.mat[6] *= fDet;
res.mat[7] *= fDet;
res.mat[8] *= fDet;
res.mat[9] *= fDet;
res.mat[10] *= fDet;
res.mat[11] *= fDet;
res.mat[12] *= fDet;
res.mat[13] *= fDet;
res.mat[14] *= fDet;
res.mat[15] *= fDet;
return res;
}
Matrix3D FromToRotation(Vector3D from, Vector3D to)
{
from.Normalize();
to.Normalize();
float angle = acos(from.dot(to));
if (angle == 0)
{
return Matrix3D();
}
Vector3D axis = from.cross(to);
axis.Normalize();
return MatrixRotationAxisRad(axis, angle);
}
Matrix3D &MatrixRotationYawPitchRoll(float Yaw, float Pitch, float Roll)
{
Matrix3D res;
float sx = sin(Pitch); float sy = sin(Yaw); float sz = sin(Roll);
float cx = cos(Pitch); float cy = cos(Yaw); float cz = cos(Roll);
res.mat[0] = cz * cy - sx * sy * sz; res.mat[1] = cy * sz + sx * sy * cz; res.mat[2] = -sy * cx; res.mat[3] = 0.0f;
res.mat[4] = -cx * sz; res.mat[5] = cx * cz; res.mat[6] = sx; res.mat[7] = 0.0f;
res.mat[8] = sy * cz + sx * cy * sz; res.mat[9] = sy * sz - sx * cy * cz; res.mat[10] = cx * cy; res.mat[11] = 0.0f;
res.mat[12] = 0.0f; res.mat[13] = 0.0f; res.mat[14] = 0.0f; res.mat[15] = 1.0f;
return res;
}
Matrix3D &MatrixRotationY(float angle)
{
float s = sin(angle);
float c = cos(angle);
Matrix3D res;
res.mat[0] = c; res.mat[1] = 0; res.mat[2] = -s; res.mat[3] = 0.0f;
res.mat[4] = 0; res.mat[5] = 1; res.mat[6] = 0; res.mat[7] = 0.0f;
res.mat[8] = s; res.mat[9] = 0; res.mat[10] = c; res.mat[11] = 0.0f;
res.mat[12] = 0.0f; res.mat[13] = 0.0f; res.mat[14] = 0.0f; res.mat[15] = 1.0f;
return res;
}