|
|
|
@@ -1,7 +1,6 @@
|
|
|
|
|
using CSNetwork.GPDataType;
|
|
|
|
|
using System;
|
|
|
|
|
using UnityEngine;
|
|
|
|
|
using static A3DMATRIX3;
|
|
|
|
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
|
|
|
//
|
|
|
|
@@ -362,8 +361,8 @@ public class A3DMATRIX4
|
|
|
|
|
}
|
|
|
|
|
set
|
|
|
|
|
{
|
|
|
|
|
if (value.GetLength(0) != 3 || value.GetLength(1) != 3)
|
|
|
|
|
throw new ArgumentException("Matrix must be 3x3");
|
|
|
|
|
if (value.GetLength(0) != 4 || value.GetLength(1) != 4)
|
|
|
|
|
throw new ArgumentException("Matrix must be 4x4");
|
|
|
|
|
_11 = value[0, 0]; _12 = value[0, 1]; _13 = value[0, 2]; _14 = value[0, 3];
|
|
|
|
|
_21 = value[1, 0]; _22 = value[1, 1]; _23 = value[1, 2]; _14 = value[1, 3];
|
|
|
|
|
_31 = value[2, 0]; _32 = value[2, 1]; _33 = value[2, 2]; _14 = value[2, 3];
|
|
|
|
@@ -380,305 +379,394 @@ public class A3DMATRIX4
|
|
|
|
|
public void SetCol(int i, A3DVECTOR3 v) { m[0,i]=v.x; m[1,i]=v.y; m[2,i]=v.z; }
|
|
|
|
|
|
|
|
|
|
// * operator
|
|
|
|
|
public static A3DVECTOR3 operator * (A3DVECTOR3 v, A3DMATRIX4 mat)
|
|
|
|
|
{
|
|
|
|
|
A3DVECTOR3 vRet;
|
|
|
|
|
D3DXVec3TransformCoord((D3DXVECTOR3)vRet, (D3DXVECTOR3)v, (D3DXMATRIX)mat);
|
|
|
|
|
return vRet;
|
|
|
|
|
public static A3DVECTOR3 operator *(A3DVECTOR3 v, A3DMATRIX4 mat)
|
|
|
|
|
{
|
|
|
|
|
A3DVECTOR3 vRet = new A3DVECTOR3(v.x * mat._11 + v.y * mat._21 + v.z * mat._31 + mat._41,
|
|
|
|
|
v.x * mat._12 + v.y * mat._22 + v.z * mat._32 + mat._42,
|
|
|
|
|
v.x * mat._13 + v.y * mat._23 + v.z * mat._33 + mat._43);
|
|
|
|
|
float w = 1.0f / (v.x * mat._14 + v.y * mat._24 + v.z * mat._34 + mat._44);
|
|
|
|
|
return vRet *= w;
|
|
|
|
|
}
|
|
|
|
|
public static A3DVECTOR3 operator *(A3DMATRIX4 mat, A3DVECTOR3 v)
|
|
|
|
|
{
|
|
|
|
|
A3DVECTOR3 vRet = new A3DVECTOR3(v.x * mat._11 + v.y * mat._21 + v.z * mat._31 + mat._41,
|
|
|
|
|
v.x * mat._12 + v.y * mat._22 + v.z * mat._32 + mat._42,
|
|
|
|
|
v.x * mat._13 + v.y * mat._23 + v.z * mat._33 + mat._43);
|
|
|
|
|
float w = 1.0f / (v.x * mat._14 + v.y * mat._24 + v.z * mat._34 + mat._44);
|
|
|
|
|
return vRet *= w;
|
|
|
|
|
}
|
|
|
|
|
public static A3DVECTOR3 operator * (A3DMATRIX4 mat, A3DVECTOR3 v);
|
|
|
|
|
public static A3DVECTOR4 operator * (A3DVECTOR4 v, A3DMATRIX4 mat);
|
|
|
|
|
public static A3DVECTOR4 operator * (A3DMATRIX4 mat, A3DVECTOR4 v);
|
|
|
|
|
public static A3DMATRIX4 operator * (A3DMATRIX4 mat1, A3DMATRIX4 mat2);
|
|
|
|
|
|
|
|
|
|
// Scale matrix
|
|
|
|
|
public static A3DMATRIX4 operator * (const A3DMATRIX4& mat, float s);
|
|
|
|
|
public static A3DMATRIX4 operator * (float s, const A3DMATRIX4& mat) { return mat * s; }
|
|
|
|
|
public static A3DMATRIX4 operator / (const A3DMATRIX4& mat, float s) { return mat * (1.0f / s); }
|
|
|
|
|
|
|
|
|
|
public static A3DVECTOR4 operator *(A3DVECTOR4 v, A3DMATRIX4 mat)
|
|
|
|
|
{
|
|
|
|
|
return new A3DVECTOR4(v.x * mat._11 + v.y * mat._21 + v.z * mat._31 + v.w * mat._41,
|
|
|
|
|
v.x * mat._12 + v.y * mat._22 + v.z * mat._32 + v.w * mat._42,
|
|
|
|
|
v.x * mat._13 + v.y * mat._23 + v.z * mat._33 + v.w * mat._43,
|
|
|
|
|
v.x * mat._14 + v.y * mat._24 + v.z * mat._34 + v.w * mat._44);
|
|
|
|
|
}
|
|
|
|
|
public static A3DVECTOR4 operator *(A3DMATRIX4 mat, A3DVECTOR4 v)
|
|
|
|
|
{
|
|
|
|
|
return new A3DVECTOR4(v.x * mat._11 + v.y * mat._21 + v.z * mat._31 + v.w * mat._41,
|
|
|
|
|
v.x * mat._12 + v.y * mat._22 + v.z * mat._32 + v.w * mat._42,
|
|
|
|
|
v.x * mat._13 + v.y * mat._23 + v.z * mat._33 + v.w * mat._43,
|
|
|
|
|
v.x * mat._14 + v.y * mat._24 + v.z * mat._34 + v.w * mat._44);
|
|
|
|
|
}
|
|
|
|
|
public static A3DMATRIX4 operator *(A3DMATRIX4 mat1, A3DMATRIX4 mat2)
|
|
|
|
|
{
|
|
|
|
|
A3DMATRIX4 matRet = new A3DMATRIX4();
|
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j = 0; j < 4; j++)
|
|
|
|
|
{
|
|
|
|
|
for (int k = 0; k < 4; k++)
|
|
|
|
|
matRet.m[i,j] += mat1.m[i,k] * mat2.m[k,j];
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return matRet;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Scale matrix
|
|
|
|
|
public static A3DMATRIX4 operator *(A3DMATRIX4 mat, float s)
|
|
|
|
|
{
|
|
|
|
|
A3DMATRIX4 matRet = new A3DMATRIX4();
|
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j = 0; j < 4; j++)
|
|
|
|
|
matRet.m[i,j] = mat.m[i,j] * s;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return matRet;
|
|
|
|
|
}
|
|
|
|
|
public static A3DMATRIX4 operator *(float s, A3DMATRIX4 mat) { return mat* s; }
|
|
|
|
|
public static A3DMATRIX4 operator /(A3DMATRIX4 mat, float s) { return mat * (1.0f / s); }
|
|
|
|
|
|
|
|
|
|
// *= operator
|
|
|
|
|
public static A3DMATRIX4 & operator *= (const A3DMATRIX4& mat)
|
|
|
|
|
{
|
|
|
|
|
*this = *this * mat;
|
|
|
|
|
return *this;
|
|
|
|
|
}
|
|
|
|
|
public static A3DMATRIX4 & operator *= (float s);
|
|
|
|
|
// /= operator
|
|
|
|
|
public static A3DMATRIX4 & operator /= (float s) { return *this *= (1.0f / s); }
|
|
|
|
|
public A3DMATRIX4 MultiplyAssign (A3DMATRIX4 mat)
|
|
|
|
|
{
|
|
|
|
|
A3DMATRIX4 temp = this * mat;
|
|
|
|
|
Array.Copy(temp.m, this.m, temp.m.Length);
|
|
|
|
|
return this;
|
|
|
|
|
}
|
|
|
|
|
public A3DMATRIX4 MultiplyAssign (float s)
|
|
|
|
|
{
|
|
|
|
|
A3DMATRIX4 temp = this;
|
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j = 0; j < 4; j++)
|
|
|
|
|
m[i,j] *= s;
|
|
|
|
|
}
|
|
|
|
|
Array.Copy(temp.m, this.m, temp.m.Length);
|
|
|
|
|
return this;
|
|
|
|
|
}
|
|
|
|
|
// /= operator
|
|
|
|
|
public A3DMATRIX4 DivisionAssign (float s) { return MultiplyAssign(1.0f / s); }
|
|
|
|
|
|
|
|
|
|
// == operator
|
|
|
|
|
public static bool operator == (A3DMATRIX4 mat1, A3DMATRIX4 mat2)
|
|
|
|
|
{
|
|
|
|
|
for (int i=0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j=0; j < 4; j++)
|
|
|
|
|
{
|
|
|
|
|
if (mat1.m[i][j] != mat2.m[i][j])
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
public static bool operator ==(A3DMATRIX4 mat1, A3DMATRIX4 mat2)
|
|
|
|
|
{
|
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j = 0; j < 4; j++)
|
|
|
|
|
{
|
|
|
|
|
if (mat1.m[i, j] != mat2.m[i, j])
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// != operator
|
|
|
|
|
public static bool operator != (A3DMATRIX4 mat1, A3DMATRIX4 mat2)
|
|
|
|
|
{
|
|
|
|
|
for (int i=0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j=0; j < 4; j++)
|
|
|
|
|
{
|
|
|
|
|
if (mat1.m[i][j] != mat2.m[i][j])
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
public static bool operator !=(A3DMATRIX4 mat1, A3DMATRIX4 mat2)
|
|
|
|
|
{
|
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j = 0; j < 4; j++)
|
|
|
|
|
{
|
|
|
|
|
if (mat1.m[i, j] != mat2.m[i, j])
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// + operator
|
|
|
|
|
public static A3DMATRIX4 operator + (A3DMATRIX4 mat1, A3DMATRIX4 mat2);
|
|
|
|
|
// - operator
|
|
|
|
|
public static A3DMATRIX4 operator - (A3DMATRIX4 mat1, A3DMATRIX4 mat2);
|
|
|
|
|
// += operator
|
|
|
|
|
public static A3DMATRIX4 & operator += (A3DMATRIX4 mat);
|
|
|
|
|
// -= operator
|
|
|
|
|
public staticA3DMATRIX4 & operator -= (A3DMATRIX4 mat);
|
|
|
|
|
// + operator
|
|
|
|
|
public static A3DMATRIX4 operator +(A3DMATRIX4 mat1, A3DMATRIX4 mat2)
|
|
|
|
|
{
|
|
|
|
|
A3DMATRIX4 matRet = new A3DMATRIX4();
|
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j = 0; j < 4; j++)
|
|
|
|
|
matRet.m[i,j] = mat1.m[i,j] + mat2.m[i,j];
|
|
|
|
|
}
|
|
|
|
|
return matRet;
|
|
|
|
|
}
|
|
|
|
|
// - operator
|
|
|
|
|
public static A3DMATRIX4 operator -(A3DMATRIX4 mat1, A3DMATRIX4 mat2)
|
|
|
|
|
{
|
|
|
|
|
A3DMATRIX4 matRet = new A3DMATRIX4();
|
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j = 0; j < 4; j++)
|
|
|
|
|
matRet.m[i,j] = mat1.m[i,j] - mat2.m[i,j];
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Clear all elements to zero
|
|
|
|
|
void Clear();
|
|
|
|
|
// Set matrix to identity matrix
|
|
|
|
|
void Identity();
|
|
|
|
|
// Transpose matrix
|
|
|
|
|
void Transpose();
|
|
|
|
|
// Get transpose matrix of this matrix
|
|
|
|
|
A3DMATRIX4 GetTranspose() const;
|
|
|
|
|
// Inverse matrix
|
|
|
|
|
return matRet;
|
|
|
|
|
}
|
|
|
|
|
// += operator
|
|
|
|
|
public A3DMATRIX4 AdditionAssign (A3DMATRIX4 mat)
|
|
|
|
|
{
|
|
|
|
|
A3DMATRIX4 temp = this;
|
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j = 0; j < 4; j++)
|
|
|
|
|
m[i,j] += mat.m[i,j];
|
|
|
|
|
}
|
|
|
|
|
Array.Copy(temp.m, this.m, temp.m.Length);
|
|
|
|
|
return this;
|
|
|
|
|
}
|
|
|
|
|
// -= operator
|
|
|
|
|
public A3DMATRIX4 SubtractionAssign (A3DMATRIX4 mat)
|
|
|
|
|
{
|
|
|
|
|
A3DMATRIX4 temp = this;
|
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j = 0; j < 4; j++)
|
|
|
|
|
m[i,j] -= mat.m[i,j];
|
|
|
|
|
}
|
|
|
|
|
Array.Copy(temp.m, this.m, temp.m.Length);
|
|
|
|
|
return this;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Clear all elements to zero
|
|
|
|
|
public void Clear()
|
|
|
|
|
{
|
|
|
|
|
_11 = _12 = _13 = _14 = 0.0f;
|
|
|
|
|
_21 = _22 = _23 = _24 = 0.0f;
|
|
|
|
|
_31 = _32 = _33 = _34 = 0.0f;
|
|
|
|
|
_41 = _42 = _43 = _44 = 0.0f;
|
|
|
|
|
}
|
|
|
|
|
// Set matrix to identity matrix
|
|
|
|
|
public void Identity()
|
|
|
|
|
{
|
|
|
|
|
_12 = _13 = _14 = 0.0f;
|
|
|
|
|
_21 = _23 = _24 = 0.0f;
|
|
|
|
|
_31 = _32 = _34 = 0.0f;
|
|
|
|
|
_41 = _42 = _43 = 0.0f;
|
|
|
|
|
_11 = _22 = _33 = _44 = 1.0f;
|
|
|
|
|
}
|
|
|
|
|
// Transpose matrix
|
|
|
|
|
public void Transpose()
|
|
|
|
|
{
|
|
|
|
|
float t = 0f;
|
|
|
|
|
t = _12; _12 = _21; _21 = t;
|
|
|
|
|
t = _13; _13 = _31; _31 = t;
|
|
|
|
|
t = _14; _14 = _41; _41 = t;
|
|
|
|
|
t = _23; _23 = _32; _32 = t;
|
|
|
|
|
t = _24; _24 = _42; _42 = t;
|
|
|
|
|
t = _34; _34 = _43; _43 = t;
|
|
|
|
|
}
|
|
|
|
|
// Get transpose matrix of this matrix
|
|
|
|
|
public A3DMATRIX4 GetTranspose()
|
|
|
|
|
{
|
|
|
|
|
A3DMATRIX4 matRet = new A3DMATRIX4();
|
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j = 0; j < 4; j++)
|
|
|
|
|
matRet.m[i,j] = m[j,i];
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return matRet;
|
|
|
|
|
}
|
|
|
|
|
// Inverse matrix
|
|
|
|
|
// void Inverse() { *this = GetInverse(); }
|
|
|
|
|
// Get inverse matrix of this matrix
|
|
|
|
|
// Get inverse matrix of this matrix
|
|
|
|
|
// A3DMATRIX4 GetInverse() const;
|
|
|
|
|
// Inverse matrix (used only by transform matrix)
|
|
|
|
|
void InverseTM() { *this = GetInverseTM(); }
|
|
|
|
|
// Get inverse matrix (used only by transform matrix)
|
|
|
|
|
A3DMATRIX4 GetInverseTM() const;
|
|
|
|
|
// Get determinant of this matrix
|
|
|
|
|
float Determinant() const;
|
|
|
|
|
// Inverse matrix (used only by transform matrix)
|
|
|
|
|
public void InverseTM()
|
|
|
|
|
{
|
|
|
|
|
A3DMATRIX4 temp = GetInverseTM();
|
|
|
|
|
Array.Copy(temp.m, this.m, temp.m.Length);
|
|
|
|
|
}
|
|
|
|
|
// Get inverse matrix (used only by transform matrix)
|
|
|
|
|
public A3DMATRIX4 GetInverseTM()
|
|
|
|
|
{
|
|
|
|
|
float fDet = 1.0f / Det3x3(_11, _12, _13, _21, _22, _23, _31, _32, _33);
|
|
|
|
|
|
|
|
|
|
// Build matrix to be translation and rotation matrix
|
|
|
|
|
void Translate(float x, float y, float z);
|
|
|
|
|
void RotateX(float fRad);
|
|
|
|
|
void RotateY(float fRad);
|
|
|
|
|
void RotateZ(float fRad);
|
|
|
|
|
void RotateAxis(const A3DVECTOR3& v, float fRad);
|
|
|
|
|
void RotateAxis(const A3DVECTOR3& vPos, const A3DVECTOR3& vAxis, float fRad);
|
|
|
|
|
void Scale(float sx, float sy, float sz);
|
|
|
|
|
A3DMATRIX4 mat = new A3DMATRIX4();
|
|
|
|
|
|
|
|
|
|
protected: // Attributes
|
|
|
|
|
mat._11 = fDet * Det3x3(_22, _23, _24, _32, _33, _34, _42, _43, _44);
|
|
|
|
|
mat._12 = -fDet * Det3x3(_12, _13, _14, _32, _33, _34, _42, _43, _44);
|
|
|
|
|
mat._13 = fDet * Det3x3(_12, _13, _14, _22, _23, _24, _42, _43, _44);
|
|
|
|
|
mat._14 = -fDet * Det3x3(_12, _13, _14, _22, _23, _24, _32, _33, _34);
|
|
|
|
|
|
|
|
|
|
// Calcualte determinant of a 3x3 matrix
|
|
|
|
|
float Det3x3(float a11, float a12, float a13, float a21, float a22, float a23,
|
|
|
|
|
float a31, float a32, float a33) const
|
|
|
|
|
{
|
|
|
|
|
return a11 * a22 * a33 + a21 * a32 * a13 + a31 * a12 * a23 -
|
|
|
|
|
a13 * a22 * a31 - a23 * a32 * a11 - a33 * a12 * a21;
|
|
|
|
|
}
|
|
|
|
|
mat._21 = -fDet * Det3x3(_21, _23, _24, _31, _33, _34, _41, _43, _44);
|
|
|
|
|
mat._22 = fDet * Det3x3(_11, _13, _14, _31, _33, _34, _41, _43, _44);
|
|
|
|
|
mat._23 = -fDet * Det3x3(_11, _13, _14, _21, _23, _24, _41, _43, _44);
|
|
|
|
|
mat._24 = fDet * Det3x3(_11, _13, _14, _21, _23, _24, _31, _33, _34);
|
|
|
|
|
|
|
|
|
|
protected: // Operations
|
|
|
|
|
mat._31 = fDet * Det3x3(_21, _22, _24, _31, _32, _34, _41, _42, _44);
|
|
|
|
|
mat._32 = -fDet * Det3x3(_11, _12, _14, _31, _32, _34, _41, _42, _44);
|
|
|
|
|
mat._33 = fDet * Det3x3(_11, _12, _14, _21, _22, _24, _41, _42, _44);
|
|
|
|
|
mat._34 = -fDet * Det3x3(_11, _12, _14, _21, _22, _24, _31, _32, _34);
|
|
|
|
|
|
|
|
|
|
mat._41 = -fDet * Det3x3(_21, _22, _23, _31, _32, _33, _41, _42, _43);
|
|
|
|
|
mat._42 = fDet * Det3x3(_11, _12, _13, _31, _32, _33, _41, _42, _43);
|
|
|
|
|
mat._43 = -fDet * Det3x3(_11, _12, _13, _21, _22, _23, _41, _42, _43);
|
|
|
|
|
mat._44 = fDet * Det3x3(_11, _12, _13, _21, _22, _23, _31, _32, _33);
|
|
|
|
|
|
|
|
|
|
return mat;
|
|
|
|
|
}
|
|
|
|
|
// Get determinant of this matrix
|
|
|
|
|
public float Determinant()
|
|
|
|
|
{
|
|
|
|
|
float fDet = 0f;
|
|
|
|
|
fDet = _11 * _22 * _33 * _44;
|
|
|
|
|
fDet += _21 * _32 * _43 * _14;
|
|
|
|
|
fDet += _31 * _42 * _13 * _24;
|
|
|
|
|
fDet += _41 * _12 * _23 * _34;
|
|
|
|
|
fDet -= _41 * _32 * _23 * _14;
|
|
|
|
|
fDet -= _42 * _33 * _24 * _11;
|
|
|
|
|
fDet -= _43 * _34 * _21 * _12;
|
|
|
|
|
fDet -= _44 * _31 * _22 * _13;
|
|
|
|
|
return fDet;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Build matrix to be translation and rotation matrix
|
|
|
|
|
public void Translate(float x, float y, float z)
|
|
|
|
|
{
|
|
|
|
|
Identity();
|
|
|
|
|
_41 = x;
|
|
|
|
|
_42 = y;
|
|
|
|
|
_43 = z;
|
|
|
|
|
}
|
|
|
|
|
public void RotateX(float fRad)
|
|
|
|
|
{
|
|
|
|
|
Identity();
|
|
|
|
|
m[2,2] = m[1,1] = Mathf.Cos(fRad);
|
|
|
|
|
m[1,2] = Mathf.Sin(fRad);
|
|
|
|
|
m[2,1] = -m[1,2];
|
|
|
|
|
}
|
|
|
|
|
public void RotateY(float fRad)
|
|
|
|
|
{
|
|
|
|
|
Identity();
|
|
|
|
|
m[2,2] = m[0,0] = Mathf.Cos(fRad);
|
|
|
|
|
m[2,0] = Mathf.Sin(fRad);
|
|
|
|
|
m[0,2] = -m[2,0];
|
|
|
|
|
}
|
|
|
|
|
public void RotateZ(float fRad)
|
|
|
|
|
{
|
|
|
|
|
Identity();
|
|
|
|
|
m[1,1] = m[0,0] = Mathf.Cos(fRad);
|
|
|
|
|
m[0,1] = Mathf.Sin(fRad);
|
|
|
|
|
m[1,0] = -m[0,1];
|
|
|
|
|
}
|
|
|
|
|
public void RotateAxis(A3DVECTOR3 v, float fRad)
|
|
|
|
|
{
|
|
|
|
|
float fCos = Mathf.Cos(fRad);
|
|
|
|
|
float fSin = Mathf.Sin(fRad);
|
|
|
|
|
|
|
|
|
|
_11 = (v.x * v.x) * (1.0f - fCos) + fCos;
|
|
|
|
|
_21 = (v.x * v.y) * (1.0f - fCos) - (v.z * fSin);
|
|
|
|
|
_31 = (v.x * v.x) * (1.0f - fCos) + (v.y * fSin);
|
|
|
|
|
|
|
|
|
|
_12 = (v.y * v.y) * (1.0f - fCos) + (v.z * fSin);
|
|
|
|
|
_22 = (v.y * v.y) * (1.0f - fCos) + fCos;
|
|
|
|
|
_32 = (v.y * v.z) * (1.0f - fCos) - (v.x * fSin);
|
|
|
|
|
|
|
|
|
|
_13 = (v.z * v.x) * (1.0f - fCos) - (v.y * fSin);
|
|
|
|
|
_23 = (v.z * v.y) * (1.0f - fCos) + (v.x * fSin);
|
|
|
|
|
_33 = (v.z * v.z) * (1.0f - fCos) + fCos;
|
|
|
|
|
|
|
|
|
|
_14 = _24 = _34 = 0.0f;
|
|
|
|
|
_41 = _42 = _43 = 0.0f;
|
|
|
|
|
_44 = 1.0f;
|
|
|
|
|
}
|
|
|
|
|
public void RotateAxis(A3DVECTOR3 vPos, A3DVECTOR3 vAxis, float fRad)
|
|
|
|
|
{
|
|
|
|
|
Translate(-vPos.x, -vPos.y, -vPos.z);
|
|
|
|
|
|
|
|
|
|
A3DMATRIX4 mat = new A3DMATRIX4();
|
|
|
|
|
mat.RotateAxis(vAxis, fRad);
|
|
|
|
|
MultiplyAssign(mat);
|
|
|
|
|
mat.Translate(vPos.x, vPos.y, vPos.z);
|
|
|
|
|
MultiplyAssign(mat);
|
|
|
|
|
}
|
|
|
|
|
public void Scale(float sx, float sy, float sz)
|
|
|
|
|
{
|
|
|
|
|
Clear();
|
|
|
|
|
_11 = sx;
|
|
|
|
|
_22 = sy;
|
|
|
|
|
_33 = sz;
|
|
|
|
|
_44 = 1.0f;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Calcualte determinant of a 3x3 matrix
|
|
|
|
|
protected float Det3x3(float a11, float a12, float a13, float a21, float a22, float a23,
|
|
|
|
|
float a31, float a32, float a33)
|
|
|
|
|
{
|
|
|
|
|
return a11 * a22 * a33 + a21 * a32 * a13 + a31 * a12 * a23 -
|
|
|
|
|
a13 * a22 * a31 - a23 * a32 * a11 - a33 * a12 * a21;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
};///////////////////////////////////////////////////////////////////////////
|
|
|
|
|
}
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
|
|
|
//
|
|
|
|
|
// class A3DMATRIX4
|
|
|
|
|
// class A3DVECTOR4
|
|
|
|
|
//
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
|
|
|
|
|
|
|
|
class A3DMATRIX4
|
|
|
|
|
public class A3DVECTOR4
|
|
|
|
|
{
|
|
|
|
|
public: // Types
|
|
|
|
|
public float x;
|
|
|
|
|
public float y;
|
|
|
|
|
public float z;
|
|
|
|
|
public float w;
|
|
|
|
|
|
|
|
|
|
// Construct flag
|
|
|
|
|
enum CONSTRUCT
|
|
|
|
|
{
|
|
|
|
|
IDENTITY = 0, // Construct a identity matrix
|
|
|
|
|
};
|
|
|
|
|
public float[] m
|
|
|
|
|
{
|
|
|
|
|
get => new float[] { x, y, z, w };
|
|
|
|
|
set
|
|
|
|
|
{
|
|
|
|
|
if (value == null || value.Length < 4)
|
|
|
|
|
throw new ArgumentException("Array must have at least 4 elements.");
|
|
|
|
|
x = value[0];
|
|
|
|
|
y = value[1];
|
|
|
|
|
z = value[2];
|
|
|
|
|
w = value[3];
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public: // Constructions and Destructions
|
|
|
|
|
// Constructors and Destructors
|
|
|
|
|
|
|
|
|
|
A3DMATRIX4() {}
|
|
|
|
|
public A3DVECTOR4() { }
|
|
|
|
|
public A3DVECTOR4(float _x, float _y, float _z, float _w) { x = _x; y = _y; z = _z; w = _w; }
|
|
|
|
|
public A3DVECTOR4(float f) { x = y = z = w = f; }
|
|
|
|
|
public A3DVECTOR4(float[] f)
|
|
|
|
|
{
|
|
|
|
|
if (f == null || f.Length < 4)
|
|
|
|
|
throw new ArgumentException("Array must have at least 4 elements.");
|
|
|
|
|
x = f[0]; y = f[1]; z = f[2]; w = f[3];
|
|
|
|
|
}
|
|
|
|
|
public A3DVECTOR4(A3DVECTOR4 v)
|
|
|
|
|
{
|
|
|
|
|
x = v.x; y = v.y; z = v.z; w = v.w;
|
|
|
|
|
}
|
|
|
|
|
public A3DVECTOR4(A3DVECTOR3 v)
|
|
|
|
|
{
|
|
|
|
|
x = v.x; y = v.y; z = v.z; w = 1.0f;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
A3DMATRIX4(float* v)
|
|
|
|
|
{
|
|
|
|
|
for (int i=0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j=0; j < 4; j++)
|
|
|
|
|
m[i][j] = v[i * 4 + j];
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
// Operations
|
|
|
|
|
|
|
|
|
|
A3DMATRIX4(const A3DMATRIX4& mat)
|
|
|
|
|
{
|
|
|
|
|
for (int i=0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j=0; j < 4; j++)
|
|
|
|
|
m[i][j] = mat.m[i][j];
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
A3DMATRIX4(CONSTRUCT c)
|
|
|
|
|
{
|
|
|
|
|
_12 = _13 = _14 = 0.0f;
|
|
|
|
|
_21 = _23 = _24 = 0.0f;
|
|
|
|
|
_31 = _32 = _34 = 0.0f;
|
|
|
|
|
_41 = _42 = _43 = 0.0f;
|
|
|
|
|
_11 = _22 = _33 = _44 = 1.0f;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
A3DMATRIX4( float fEntry00, float fEntry01, float fEntry02, float fEntry03,
|
|
|
|
|
float fEntry10, float fEntry11, float fEntry12, float fEntry13,
|
|
|
|
|
float fEntry20, float fEntry21, float fEntry22, float fEntry23,
|
|
|
|
|
float fEntry30, float fEntry31, float fEntry32, float fEntry33)
|
|
|
|
|
{
|
|
|
|
|
m[0][0] = fEntry00;
|
|
|
|
|
m[0][1] = fEntry01;
|
|
|
|
|
m[0][2] = fEntry02;
|
|
|
|
|
m[0][3] = fEntry03;
|
|
|
|
|
|
|
|
|
|
m[1][0] = fEntry10;
|
|
|
|
|
m[1][1] = fEntry11;
|
|
|
|
|
m[1][2] = fEntry12;
|
|
|
|
|
m[1][3] = fEntry13;
|
|
|
|
|
|
|
|
|
|
m[2][0] = fEntry20;
|
|
|
|
|
m[2][1] = fEntry21;
|
|
|
|
|
m[2][2] = fEntry22;
|
|
|
|
|
m[2][3] = fEntry23;
|
|
|
|
|
|
|
|
|
|
m[3][0] = fEntry30;
|
|
|
|
|
m[3][1] = fEntry31;
|
|
|
|
|
m[3][2] = fEntry32;
|
|
|
|
|
m[3][3] = fEntry33;
|
|
|
|
|
}
|
|
|
|
|
// = operator
|
|
|
|
|
public A3DVECTOR4 assignment(A3DVECTOR4 v)
|
|
|
|
|
{
|
|
|
|
|
x = v.x; y = v.y; z = v.z; w = v.w; return this; }
|
|
|
|
|
public A3DVECTOR4 assignment(A3DVECTOR3 v) { x = v.x; y = v.y; z = v.z; w = 1.0f; return this; }
|
|
|
|
|
|
|
|
|
|
public: // Attributes
|
|
|
|
|
|
|
|
|
|
union
|
|
|
|
|
{
|
|
|
|
|
struct
|
|
|
|
|
{
|
|
|
|
|
float _11, _12, _13, _14;
|
|
|
|
|
float _21, _22, _23, _24;
|
|
|
|
|
float _31, _32, _33, _34;
|
|
|
|
|
float _41, _42, _43, _44;
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
float m[4][4];
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
public: // Operaitons
|
|
|
|
|
|
|
|
|
|
// Get row and column
|
|
|
|
|
A3DVECTOR3 GetRow(int i) const { return A3DVECTOR3(m[i][0], m[i][1], m[i][2]); }
|
|
|
|
|
A3DVECTOR3 GetCol(int i) const { return A3DVECTOR3(m[0][i], m[1][i], m[2][i]); }
|
|
|
|
|
// Set row and column
|
|
|
|
|
void SetRow(int i, const A3DVECTOR3& v) { m[i][0]=v.x; m[i][1]=v.y; m[i][2]=v.z; }
|
|
|
|
|
void SetCol(int i, const A3DVECTOR3& v) { m[0][i]=v.x; m[1][i]=v.y; m[2][i]=v.z; }
|
|
|
|
|
|
|
|
|
|
// * operator
|
|
|
|
|
friend A3DVECTOR3 operator * (const A3DVECTOR3& v, const A3DMATRIX4& mat);
|
|
|
|
|
friend A3DVECTOR3 operator * (const A3DMATRIX4& mat, const A3DVECTOR3& v);
|
|
|
|
|
friend A3DVECTOR4 operator * (const A3DVECTOR4& v, const A3DMATRIX4& mat);
|
|
|
|
|
friend A3DVECTOR4 operator * (const A3DMATRIX4& mat, const A3DVECTOR4& v);
|
|
|
|
|
friend A3DMATRIX4 operator * (const A3DMATRIX4& mat1, const A3DMATRIX4& mat2);
|
|
|
|
|
|
|
|
|
|
// Scale matrix
|
|
|
|
|
friend A3DMATRIX4 operator * (const A3DMATRIX4& mat, float s);
|
|
|
|
|
friend A3DMATRIX4 operator * (float s, const A3DMATRIX4& mat) { return mat * s; }
|
|
|
|
|
friend A3DMATRIX4 operator / (const A3DMATRIX4& mat, float s) { return mat * (1.0f / s); }
|
|
|
|
|
|
|
|
|
|
// *= operator
|
|
|
|
|
A3DMATRIX4& operator *= (const A3DMATRIX4& mat)
|
|
|
|
|
{
|
|
|
|
|
*this = *this * mat;
|
|
|
|
|
return *this;
|
|
|
|
|
}
|
|
|
|
|
A3DMATRIX4& operator *= (float s);
|
|
|
|
|
// /= operator
|
|
|
|
|
A3DMATRIX4& operator /= (float s) { return *this *= (1.0f / s); }
|
|
|
|
|
|
|
|
|
|
// == operator
|
|
|
|
|
friend bool operator == (const A3DMATRIX4& mat1, const A3DMATRIX4& mat2)
|
|
|
|
|
{
|
|
|
|
|
for (int i=0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j=0; j < 4; j++)
|
|
|
|
|
{
|
|
|
|
|
if (mat1.m[i][j] != mat2.m[i][j])
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// != operator
|
|
|
|
|
friend bool operator != (const A3DMATRIX4& mat1, const A3DMATRIX4& mat2)
|
|
|
|
|
{
|
|
|
|
|
for (int i=0; i < 4; i++)
|
|
|
|
|
{
|
|
|
|
|
for (int j=0; j < 4; j++)
|
|
|
|
|
{
|
|
|
|
|
if (mat1.m[i][j] != mat2.m[i][j])
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// + operator
|
|
|
|
|
friend A3DMATRIX4 operator + (const A3DMATRIX4& mat1, const A3DMATRIX4& mat2);
|
|
|
|
|
// - operator
|
|
|
|
|
friend A3DMATRIX4 operator - (const A3DMATRIX4& mat1, const A3DMATRIX4& mat2);
|
|
|
|
|
// += operator
|
|
|
|
|
A3DMATRIX4& operator += (const A3DMATRIX4& mat);
|
|
|
|
|
// -= operator
|
|
|
|
|
A3DMATRIX4& operator -= (const A3DMATRIX4& mat);
|
|
|
|
|
|
|
|
|
|
// Clear all elements to zero
|
|
|
|
|
void Clear();
|
|
|
|
|
// Set matrix to identity matrix
|
|
|
|
|
void Identity();
|
|
|
|
|
// Transpose matrix
|
|
|
|
|
void Transpose();
|
|
|
|
|
// Get transpose matrix of this matrix
|
|
|
|
|
A3DMATRIX4 GetTranspose() const;
|
|
|
|
|
// Inverse matrix
|
|
|
|
|
// void Inverse() { *this = GetInverse(); }
|
|
|
|
|
// Get inverse matrix of this matrix
|
|
|
|
|
// A3DMATRIX4 GetInverse() const;
|
|
|
|
|
// Inverse matrix (used only by transform matrix)
|
|
|
|
|
void InverseTM() { *this = GetInverseTM(); }
|
|
|
|
|
// Get inverse matrix (used only by transform matrix)
|
|
|
|
|
A3DMATRIX4 GetInverseTM() const;
|
|
|
|
|
// Get determinant of this matrix
|
|
|
|
|
float Determinant() const;
|
|
|
|
|
|
|
|
|
|
// Build matrix to be translation and rotation matrix
|
|
|
|
|
void Translate(float x, float y, float z);
|
|
|
|
|
void RotateX(float fRad);
|
|
|
|
|
void RotateY(float fRad);
|
|
|
|
|
void RotateZ(float fRad);
|
|
|
|
|
void RotateAxis(const A3DVECTOR3& v, float fRad);
|
|
|
|
|
void RotateAxis(const A3DVECTOR3& vPos, const A3DVECTOR3& vAxis, float fRad);
|
|
|
|
|
void Scale(float sx, float sy, float sz);
|
|
|
|
|
|
|
|
|
|
protected: // Attributes
|
|
|
|
|
|
|
|
|
|
// Calcualte determinant of a 3x3 matrix
|
|
|
|
|
float Det3x3(float a11, float a12, float a13, float a21, float a22, float a23,
|
|
|
|
|
float a31, float a32, float a33) const
|
|
|
|
|
{
|
|
|
|
|
return a11 * a22 * a33 + a21 * a32 * a13 + a31 * a12 * a23 -
|
|
|
|
|
a13 * a22 * a31 - a23 * a32 * a11 - a33 * a12 * a21;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
protected: // Operations
|
|
|
|
|
|
|
|
|
|
public void Set(float _x, float _y, float _z, float _w) { x = _x; y = _y; z = _z; w = _w; }
|
|
|
|
|
public void Clear() { x = y = z = w = 0.0f; }
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|