tung.do@monkey.edu.vn
773 lines
22 KiB
C#
773 lines
22 KiB
C#
using CSNetwork.GPDataType;
|
|
using System;
|
|
using UnityEngine;
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// class A3DMATRIX3
|
|
//
|
|
///////////////////////////////////////////////////////////////////////////
|
|
|
|
public class A3DMATRIX3
|
|
{
|
|
// Construct flag
|
|
public enum CONSTRUCT
|
|
{
|
|
IDENTITY = 0, // Construct a identity matrix
|
|
CLEARED, // Construct a cleared matrix
|
|
}
|
|
|
|
public float _11, _12, _13;
|
|
public float _21, _22, _23;
|
|
public float _31, _32, _33;
|
|
|
|
public float[,] m
|
|
{
|
|
get
|
|
{
|
|
return new float[3, 3] {
|
|
{ _11, _12, _13 },
|
|
{ _21, _22, _23 },
|
|
{ _31, _32, _33 }
|
|
};
|
|
}
|
|
set
|
|
{
|
|
if (value.GetLength(0) != 3 || value.GetLength(1) != 3)
|
|
throw new ArgumentException("Matrix must be 3x3");
|
|
_11 = value[0, 0]; _12 = value[0, 1]; _13 = value[0, 2];
|
|
_21 = value[1, 0]; _22 = value[1, 1]; _23 = value[1, 2];
|
|
_31 = value[2, 0]; _32 = value[2, 1]; _33 = value[2, 2];
|
|
}
|
|
}
|
|
|
|
public A3DMATRIX3() { }
|
|
|
|
public A3DMATRIX3(CONSTRUCT c)
|
|
{
|
|
if (c == CONSTRUCT.IDENTITY)
|
|
{
|
|
_12 = _13 = 0.0f;
|
|
_21 = _23 = 0.0f;
|
|
_31 = _32 = 0.0f;
|
|
_11 = _22 = _33 = 1.0f;
|
|
}
|
|
else if (c == CONSTRUCT.CLEARED)
|
|
{
|
|
_11 = _12 = _13 = 0.0f;
|
|
_21 = _22 = _23 = 0.0f;
|
|
_31 = _32 = _33 = 0.0f;
|
|
}
|
|
}
|
|
|
|
public A3DMATRIX3(float[,] arr)
|
|
{
|
|
_11 = arr[0, 0]; _12 = arr[0, 1]; _13 = arr[0, 2];
|
|
_21 = arr[1, 0]; _22 = arr[1, 1]; _23 = arr[1, 2];
|
|
_31 = arr[2, 0]; _32 = arr[2, 1]; _33 = arr[2, 2];
|
|
}
|
|
|
|
public A3DMATRIX3(A3DMATRIX3 rkMatrix)
|
|
{
|
|
_11 = rkMatrix._11; _12 = rkMatrix._12; _13 = rkMatrix._13;
|
|
_21 = rkMatrix._21; _22 = rkMatrix._22; _23 = rkMatrix._23;
|
|
_31 = rkMatrix._31; _32 = rkMatrix._32; _33 = rkMatrix._33;
|
|
}
|
|
|
|
public A3DMATRIX3(float fEntry00, float fEntry01, float fEntry02,
|
|
float fEntry10, float fEntry11, float fEntry12,
|
|
float fEntry20, float fEntry21, float fEntry22)
|
|
|
|
{
|
|
m[0, 0] = fEntry00;
|
|
m[0, 1] = fEntry01;
|
|
m[0, 2] = fEntry02;
|
|
m[1, 0] = fEntry10;
|
|
m[1, 1] = fEntry11;
|
|
m[1, 2] = fEntry12;
|
|
m[2, 0] = fEntry20;
|
|
m[2, 1] = fEntry21;
|
|
m[2, 2] = fEntry22;
|
|
}
|
|
// Operations
|
|
|
|
public A3DVECTOR3 GetRow(int i) { return new A3DVECTOR3(m[i, 0], m[i, 1], m[i, 2]); }
|
|
public A3DVECTOR3 GetCol(int i) { return new A3DVECTOR3(m[0, i], m[1, i], m[2, i]); }
|
|
|
|
// * operator
|
|
public static A3DVECTOR3 operator *(A3DVECTOR3 v, A3DMATRIX3 mat)
|
|
{
|
|
return new A3DVECTOR3(v.x * mat._11 + v.y * mat._21 + v.z * mat._31,
|
|
v.x * mat._12 + v.y * mat._22 + v.z * mat._32,
|
|
v.x * mat._13 + v.y * mat._23 + v.z * mat._33);
|
|
}
|
|
public static A3DVECTOR3 operator *(A3DMATRIX3 mat, A3DVECTOR3 v)
|
|
{
|
|
return new A3DVECTOR3(v.x * mat._11 + v.y * mat._21 + v.z * mat._31,
|
|
v.x * mat._12 + v.y * mat._22 + v.z * mat._32,
|
|
v.x * mat._13 + v.y * mat._23 + v.z * mat._33);
|
|
}
|
|
public static A3DMATRIX3 operator *(A3DMATRIX3 mat1, A3DMATRIX3 mat2)
|
|
{
|
|
A3DMATRIX3 matRet = new A3DMATRIX3();
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
for (int j = 0; j < 3; j++)
|
|
{
|
|
for (int k = 0; k < 3; k++)
|
|
matRet.m[i, j] += mat1.m[i, k] * mat2.m[k, j];
|
|
}
|
|
}
|
|
return matRet;
|
|
}
|
|
|
|
// *= operator
|
|
public A3DMATRIX3 MultiplyAssign(A3DMATRIX3 mat)
|
|
{
|
|
A3DMATRIX3 temp = this * mat;
|
|
Array.Copy(temp.m, this.m, temp.m.Length);
|
|
return this;
|
|
}
|
|
|
|
void Transpose()
|
|
{
|
|
float t;
|
|
t = m[0, 1]; m[0, 1] = m[1, 0]; m[1, 0] = t;
|
|
t = m[0, 2]; m[0, 2] = m[2, 0]; m[2, 0] = t;
|
|
t = m[1, 2]; m[1, 2] = m[2, 1]; m[2, 1] = t;
|
|
}
|
|
|
|
// Clear all elements to zero
|
|
void Clear()
|
|
{
|
|
_11 = _12 = _13 = 0.0f;
|
|
_21 = _22 = _23 = 0.0f;
|
|
_31 = _32 = _33 = 0.0f;
|
|
}
|
|
// Set matrix to identity matrix
|
|
void Identity()
|
|
{
|
|
_12 = _13 = 0.0f;
|
|
_21 = _23 = 0.0f;
|
|
_31 = _32 = 0.0f;
|
|
_11 = _22 = _33 = 1.0f;
|
|
}
|
|
|
|
// Build matrix to be translation and rotation matrix
|
|
void Translate(float x, float y)
|
|
{
|
|
Identity();
|
|
_31 = x;
|
|
_32 = y;
|
|
}
|
|
void Rotate(float fRad)
|
|
{
|
|
Identity();
|
|
m[1, 1] = m[0, 0] = (float)Math.Cos(fRad);
|
|
m[0, 1] = (float)Math.Cos(fRad);
|
|
m[1, 0] = -m[0, 1];
|
|
}
|
|
}
|
|
|
|
public struct D3DXVECTOR3
|
|
{
|
|
public float X;
|
|
public float Y;
|
|
public float Z;
|
|
|
|
// Constructors
|
|
public D3DXVECTOR3(float x, float y, float z)
|
|
{
|
|
X = x;
|
|
Y = y;
|
|
Z = z;
|
|
}
|
|
|
|
public D3DXVECTOR3(float[] v)
|
|
{
|
|
if (v == null || v.Length < 3) throw new ArgumentException("Array must have at least 3 elements.");
|
|
X = v[0];
|
|
Y = v[1];
|
|
Z = v[2];
|
|
}
|
|
|
|
public D3DXVECTOR3(D3DXVECTOR3 v)
|
|
{
|
|
X = v.X;
|
|
Y = v.Y;
|
|
Z = v.Z;
|
|
}
|
|
|
|
// Unary operators
|
|
public static D3DXVECTOR3 operator +(D3DXVECTOR3 v) => v;
|
|
public static D3DXVECTOR3 operator -(D3DXVECTOR3 v) => new D3DXVECTOR3(-v.X, -v.Y, -v.Z);
|
|
|
|
// Binary operators
|
|
public static D3DXVECTOR3 operator +(D3DXVECTOR3 a, D3DXVECTOR3 b) =>
|
|
new D3DXVECTOR3(a.X + b.X, a.Y + b.Y, a.Z + b.Z);
|
|
|
|
public static D3DXVECTOR3 operator -(D3DXVECTOR3 a, D3DXVECTOR3 b) =>
|
|
new D3DXVECTOR3(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
|
|
|
|
public static D3DXVECTOR3 operator *(D3DXVECTOR3 v, float s) =>
|
|
new D3DXVECTOR3(v.X * s, v.Y * s, v.Z * s);
|
|
|
|
public static D3DXVECTOR3 operator *(float s, D3DXVECTOR3 v) => v * s;
|
|
|
|
public static D3DXVECTOR3 operator /(D3DXVECTOR3 v, float s)
|
|
{
|
|
if (s == 0f) throw new DivideByZeroException();
|
|
return new D3DXVECTOR3(v.X / s, v.Y / s, v.Z / s);
|
|
}
|
|
|
|
// Assignment-style operators
|
|
public static D3DXVECTOR3 Add(ref D3DXVECTOR3 a, D3DXVECTOR3 b)
|
|
{
|
|
a.X += b.X; a.Y += b.Y; a.Z += b.Z;
|
|
return a;
|
|
}
|
|
|
|
public static D3DXVECTOR3 Subtract(ref D3DXVECTOR3 a, D3DXVECTOR3 b)
|
|
{
|
|
a.X -= b.X; a.Y -= b.Y; a.Z -= b.Z;
|
|
return a;
|
|
}
|
|
|
|
public static D3DXVECTOR3 Multiply(ref D3DXVECTOR3 v, float s)
|
|
{
|
|
v.X *= s; v.Y *= s; v.Z *= s;
|
|
return v;
|
|
}
|
|
|
|
public static D3DXVECTOR3 Divide(ref D3DXVECTOR3 v, float s)
|
|
{
|
|
if (s == 0f) throw new DivideByZeroException();
|
|
v.X /= s; v.Y /= s; v.Z /= s;
|
|
return v;
|
|
}
|
|
|
|
// Comparison operators
|
|
public static bool operator ==(D3DXVECTOR3 a, D3DXVECTOR3 b) =>
|
|
a.X == b.X && a.Y == b.Y && a.Z == b.Z;
|
|
|
|
public static bool operator !=(D3DXVECTOR3 a, D3DXVECTOR3 b) =>
|
|
!(a == b);
|
|
|
|
public override bool Equals(object obj)
|
|
{
|
|
if (obj is D3DXVECTOR3 v) return this == v;
|
|
return false;
|
|
}
|
|
|
|
public override int GetHashCode() => HashCode.Combine(X, Y, Z);
|
|
|
|
// Magnitude and Normalize
|
|
public float Magnitude() => (float)Math.Sqrt(X * X + Y * Y + Z * Z);
|
|
|
|
public D3DXVECTOR3 Normalize()
|
|
{
|
|
float mag = Magnitude();
|
|
if (Math.Abs(mag) < 1e-12f) return new D3DXVECTOR3(0f, 0f, 0f);
|
|
return this / mag;
|
|
}
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// class A3DMATRIX4
|
|
//
|
|
///////////////////////////////////////////////////////////////////////////
|
|
|
|
public class A3DMATRIX4
|
|
{
|
|
// Construct flag
|
|
public enum CONSTRUCT
|
|
{
|
|
IDENTITY = 0, // Construct a identity matrix
|
|
}
|
|
|
|
public float _11, _12, _13, _14;
|
|
public float _21, _22, _23, _24;
|
|
public float _31, _32, _33, _34;
|
|
public float _41, _42, _43, _44;
|
|
|
|
// Constructions and Destructions
|
|
|
|
public A3DMATRIX4() {}
|
|
|
|
public A3DMATRIX4(float[] v)
|
|
{
|
|
for (int i=0; i < 4; i++)
|
|
{
|
|
for (int j=0; j < 4; j++)
|
|
m[i,j] = v[i * 4 + j];
|
|
}
|
|
}
|
|
|
|
public A3DMATRIX4(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;
|
|
}
|
|
public float[,] m
|
|
{
|
|
get
|
|
{
|
|
return new float[4, 4] {
|
|
{ _11, _12, _13, _14 },
|
|
{ _21, _22, _23, _24 },
|
|
{ _31, _32, _33, _34 },
|
|
{_41, _42, _43, _44 }
|
|
};
|
|
}
|
|
set
|
|
{
|
|
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];
|
|
_41 = value[3, 0]; _32 = value[3, 1]; _33 = value[3, 2]; _14 = value[3, 3];
|
|
}
|
|
}
|
|
// Operaitons
|
|
|
|
// Get row and column
|
|
public A3DVECTOR3 GetRow(int i) { return new A3DVECTOR3(m[i,0], m[i,1], m[i,2]); }
|
|
public A3DVECTOR3 GetCol(int i) { return new A3DVECTOR3(m[0,i], m[1,i], m[2,i]); }
|
|
// Set row and column
|
|
public void SetRow(int i, A3DVECTOR3 v) { m[i,0]=v.x; m[i,1]=v.y; m[i,2]=v.z; }
|
|
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 = 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 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 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;
|
|
}
|
|
|
|
// != 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;
|
|
}
|
|
|
|
// + 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];
|
|
}
|
|
|
|
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
|
|
// A3DMATRIX4 GetInverse() 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);
|
|
|
|
A3DMATRIX4 mat = new A3DMATRIX4();
|
|
|
|
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);
|
|
|
|
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);
|
|
|
|
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 A3DVECTOR4
|
|
//
|
|
///////////////////////////////////////////////////////////////////////////
|
|
|
|
public class A3DVECTOR4
|
|
{
|
|
public float x;
|
|
public float y;
|
|
public float z;
|
|
public float w;
|
|
|
|
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];
|
|
}
|
|
}
|
|
|
|
// Constructors and Destructors
|
|
|
|
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;
|
|
}
|
|
|
|
// Operations
|
|
|
|
// = 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 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; }
|
|
};
|
|
|
|
|