Tungdv
848 lines
25 KiB
C#
848 lines
25 KiB
C#
using CSNetwork.GPDataType;
|
|
using System;
|
|
using UnityEngine;
|
|
|
|
///////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// class A3DMATRIX3
|
|
//
|
|
///////////////////////////////////////////////////////////////////////////
|
|
|
|
public struct A3DMATRIX3
|
|
{
|
|
// Construct flag
|
|
public enum CONSTRUCT
|
|
{
|
|
IDENTITY = 0, // Construct a identity matrix
|
|
CLEARED, // Construct a cleared matrix
|
|
}
|
|
|
|
float _11, _12, _13;
|
|
float _21, _22, _23;
|
|
float _31, _32, _33;
|
|
|
|
public float[] m;
|
|
//{
|
|
// get
|
|
// {
|
|
// return new float[9] {
|
|
// _11, _12, _13,
|
|
// _21, _22, _23,
|
|
// _31, _32, _33
|
|
// };
|
|
// }
|
|
// set
|
|
// {
|
|
// if (value.Length != 9 )
|
|
// throw new ArgumentException("Matrix must be 9");
|
|
// _11 = value[0]; _12 = value[1]; _13 = value[2];
|
|
// _21 = value[3]; _22 = value[lenght]; _23 = value[5];
|
|
// _31 = value[6]; _32 = value[7]; _33 = value[8];
|
|
// }
|
|
//}
|
|
|
|
//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;
|
|
}
|
|
m = new float[9]{
|
|
_11, _12, _13,
|
|
_21, _22, _23,
|
|
_31, _32, _33
|
|
};
|
|
}
|
|
|
|
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];
|
|
|
|
m = new float[9]{
|
|
_11, _12, _13,
|
|
_21, _22, _23,
|
|
_31, _32, _33
|
|
};
|
|
}
|
|
|
|
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;
|
|
|
|
m = new float[9]{
|
|
_11, _12, _13,
|
|
_21, _22, _23,
|
|
_31, _32, _33
|
|
};
|
|
}
|
|
|
|
public A3DMATRIX3(float fEntry00, float fEntry01, float fEntry02,
|
|
float fEntry10, float fEntry11, float fEntry12,
|
|
float fEntry20, float fEntry21, float fEntry22)
|
|
{
|
|
_11 = fEntry00;
|
|
_12 = fEntry01;
|
|
_13 = fEntry02;
|
|
_21 = fEntry10;
|
|
_22 = fEntry11;
|
|
_23 = fEntry12;
|
|
_31 = fEntry20;
|
|
_32 = fEntry21;
|
|
_33 = fEntry22;
|
|
|
|
m = new float[9]{
|
|
_11, _12, _13,
|
|
_21, _22, _23,
|
|
_31, _32, _33
|
|
};
|
|
}
|
|
// Operations
|
|
|
|
public A3DVECTOR3 GetRow(int i) { int row = i * 3; return new A3DVECTOR3(m[row + 0], m[row + 1], m[row + 2]); }
|
|
public A3DVECTOR3 GetCol(int i) { return new A3DVECTOR3(m[i + (0 * 3)], m[i + (1 * 3)], m[i + (2 * 3)]); }
|
|
|
|
// * 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();
|
|
int lenght = 3;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
for (int j = 0; j < 3; j++)
|
|
{
|
|
for (int k = 0; k < 3; k++)
|
|
matRet.m[(i * lenght) + j] += mat1.m[(i * lenght) + k] * mat2.m[(k * lenght) + 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[1]; m[1] = m[3]; m[3] = t;
|
|
t = m[2]; m[2] = m[6]; m[6] = t;
|
|
t = m[5]; m[5] = m[7]; m[7] = 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[4] = m[0] = (float)Math.Cos(fRad);
|
|
m[1] = (float)Math.Cos(fRad);
|
|
m[3] = -m[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 struct A3DMATRIX4
|
|
{
|
|
// Construct flag
|
|
public enum CONSTRUCT
|
|
{
|
|
IDENTITY = 0, // Construct a identity matrix
|
|
}
|
|
|
|
float _11, _12, _13, _14;
|
|
float _21, _22, _23, _24;
|
|
float _31, _32, _33, _34;
|
|
float _41, _42, _43, _44;
|
|
|
|
public float[] m;
|
|
//{
|
|
// get
|
|
// {
|
|
// return new float[16] {
|
|
// _11, _12, _13, _14,
|
|
// _21, _22, _23, _24,
|
|
// _31, _32, _33, _34,
|
|
// _41, _42, _43, _44
|
|
// };
|
|
// }
|
|
// set
|
|
// {
|
|
// if (value.Length != 16)
|
|
// throw new ArgumentException("Matrix must be range 16");
|
|
// _11 = value[0]; _12 = value[1]; _13 = value[2]; _14 = value[3];
|
|
// _21 = value[4]; _22 = value[5]; _23 = value[6]; _24 = value[7];
|
|
// _31 = value[8]; _32 = value[9]; _33 = value[10]; _34 = value[11];
|
|
// _41 = value[12]; _42 = value[13]; _43 = value[14]; _44 = value[15];
|
|
// }
|
|
//}
|
|
|
|
// Constructions and Destructions
|
|
|
|
public A3DMATRIX4(float[] v)
|
|
{
|
|
_11 = v[0]; _12 = v[1]; _13 = v[2]; _14 = v[3];
|
|
_21 = v[4]; _22 = v[5]; _23 = v[6]; _24 = v[7];
|
|
_31 = v[8]; _32 = v[9]; _33 = v[10]; _34 = v[11];
|
|
_41 = v[12]; _42 = v[13]; _43 = v[14]; _44 = v[15];
|
|
m = new float[16] {
|
|
_11, _12, _13, _14,
|
|
_21, _22, _23, _24,
|
|
_31, _32, _33, _34,
|
|
_41, _42, _43, _44
|
|
};
|
|
// for (int i=0; i < 4; i++)
|
|
//{
|
|
// for (int j=0; j < 4; j++)
|
|
// m[i * 4 + j] = v[i * 4 + j];
|
|
//}
|
|
}
|
|
|
|
public A3DMATRIX4(A3DMATRIX4 mat)
|
|
{
|
|
_11 = mat.m[0]; _12 = mat.m[1]; _13 = mat.m[2]; _14 = mat.m[3];
|
|
_21 = mat.m[4]; _22 = mat.m[5]; _23 = mat.m[6]; _24 = mat.m[7];
|
|
_31 = mat.m[8]; _32 = mat.m[9]; _33 = mat.m[10]; _34 = mat.m[11];
|
|
_41 = mat.m[12]; _42 = mat.m[13]; _43 = mat.m[14]; _44 = mat.m[15];
|
|
m = new float[16] {
|
|
_11, _12, _13, _14,
|
|
_21, _22, _23, _24,
|
|
_31, _32, _33, _34,
|
|
_41, _42, _43, _44
|
|
};
|
|
}
|
|
|
|
public 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;
|
|
m = new float[16] {
|
|
_11, _12, _13, _14,
|
|
_21, _22, _23, _24,
|
|
_31, _32, _33, _34,
|
|
_41, _42, _43, _44
|
|
};
|
|
}
|
|
|
|
public 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)
|
|
{
|
|
_11 = fEntry00;
|
|
_12 = fEntry01;
|
|
_13 = fEntry02;
|
|
_14 = fEntry03;
|
|
|
|
_21 = fEntry10;
|
|
_22 = fEntry11;
|
|
_23 = fEntry12;
|
|
_24 = fEntry13;
|
|
|
|
_31 = fEntry20;
|
|
_32 = fEntry21;
|
|
_33 = fEntry22;
|
|
_34 = fEntry23;
|
|
|
|
_41 = fEntry30;
|
|
_42 = fEntry31;
|
|
_43 = fEntry32;
|
|
_44 = fEntry33;
|
|
|
|
m = new float[16] {
|
|
_11, _12, _13, _14,
|
|
_21, _22, _23, _24,
|
|
_31, _32, _33, _34,
|
|
_41, _42, _43, _44
|
|
};
|
|
}
|
|
// Operaitons
|
|
|
|
// Get row and column
|
|
public A3DVECTOR3 GetRow(int i) { int row = i * 4; return new A3DVECTOR3(m[row + 0], m[row + 1], m[row + 2]); }
|
|
public A3DVECTOR3 GetCol(int i) { return new A3DVECTOR3(m[(0 * 4) + i], m[(1 * 4) + i], m[(2 * 4) + i]); }
|
|
// Set row and column
|
|
public void SetRow(int i, A3DVECTOR3 v) { int row = i * 4; m[row + 0] = v.x; m[row + 1] = v.y; m[row + 2] = v.z; }
|
|
public void SetCol(int i, A3DVECTOR3 v) { m[(0 * 3) + i] = v.x; m[(1 * 3) + i] = v.y; m[(2 * 3) + 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.m[0] * mat._11 + v.m[1] * mat._21 + v.m[2] * mat._31 + v.m[3] * mat._41,
|
|
v.m[0] * mat._12 + v.m[1] * mat._22 + v.m[2] * mat._32 + v.m[3] * mat._42,
|
|
v.m[0] * mat._13 + v.m[1] * mat._23 + v.m[2] * mat._33 + v.m[0] * mat._43,
|
|
v.m[0] * mat._14 + v.m[1] * mat._24 + v.m[2] * mat._34 + v.m[3] * mat._44);
|
|
}
|
|
public static A3DVECTOR4 operator *(A3DMATRIX4 mat, A3DVECTOR4 v)
|
|
{
|
|
return new A3DVECTOR4(v.m[0] * mat._11 + v.m[1] * mat._21 + v.m[2] * mat._31 + v.m[3] * mat._41,
|
|
v.m[0] * mat._12 + v.m[1] * mat._22 + v.m[2] * mat._32 + v.m[3] * mat._42,
|
|
v.m[0] * mat._13 + v.m[1] * mat._23 + v.m[2] * mat._33 + v.m[3] * mat._43,
|
|
v.m[0] * mat._14 + v.m[1] * mat._24 + v.m[2] * mat._34 + v.m[3] * mat._44);
|
|
}
|
|
public static A3DMATRIX4 operator *(A3DMATRIX4 mat1, A3DMATRIX4 mat2)
|
|
{
|
|
A3DMATRIX4 matRet = new A3DMATRIX4();
|
|
int lenght = 4;
|
|
for (int i = 0; i < lenght; i++)
|
|
{
|
|
for (int j = 0; j < lenght; j++)
|
|
{
|
|
for (int k = 0; k < lenght; k++)
|
|
//matRet.m[i,j] += mat1.m[i,k] * mat2.m[k,j];
|
|
matRet.m[(i * lenght) + j] += mat1.m[(i * lenght) + k] * mat2.m[(k * lenght) + j];
|
|
|
|
}
|
|
}
|
|
return matRet;
|
|
}
|
|
|
|
// Scale matrix
|
|
public static A3DMATRIX4 operator *(A3DMATRIX4 mat, float s)
|
|
{
|
|
A3DMATRIX4 matRet = new A3DMATRIX4();
|
|
int lenght = 4;
|
|
for (int i = 0; i < lenght; i++)
|
|
{
|
|
for (int j = 0; j < lenght; j++)
|
|
matRet.m[(i * lenght) + j] = mat.m[(i * lenght) + 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;
|
|
int lenght = 4;
|
|
for (int i = 0; i < lenght; i++)
|
|
{
|
|
for (int j = 0; j < lenght; j++)
|
|
m[(i * lenght) + 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)
|
|
{
|
|
int lenght = 4;
|
|
for (int i = 0; i < lenght; i++)
|
|
{
|
|
for (int j = 0; j < lenght; j++)
|
|
{
|
|
if (mat1.m[(i * lenght) + j] != mat2.m[(i * lenght) + j])
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// != operator
|
|
public static bool operator !=(A3DMATRIX4 mat1, A3DMATRIX4 mat2)
|
|
{
|
|
int lenght = 4;
|
|
for (int i = 0; i < lenght; i++)
|
|
{
|
|
for (int j = 0; j < lenght; j++)
|
|
{
|
|
if (mat1.m[(i * lenght) + j] != mat2.m[(i * lenght) + j])
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// + operator
|
|
public static A3DMATRIX4 operator +(A3DMATRIX4 mat1, A3DMATRIX4 mat2)
|
|
{
|
|
A3DMATRIX4 matRet = new A3DMATRIX4();
|
|
int lenght = 4;
|
|
for (int i = 0; i < lenght; i++)
|
|
{
|
|
for (int j = 0; j < lenght; j++)
|
|
matRet.m[(i * lenght) + j] = mat1.m[(i * lenght) + j] + mat2.m[(i * lenght) + j];
|
|
}
|
|
return matRet;
|
|
}
|
|
// - operator
|
|
public static A3DMATRIX4 operator -(A3DMATRIX4 mat1, A3DMATRIX4 mat2)
|
|
{
|
|
int lenght = 4;
|
|
A3DMATRIX4 matRet = new A3DMATRIX4();
|
|
for (int i = 0; i < lenght; i++)
|
|
{
|
|
for (int j = 0; j < lenght; j++)
|
|
matRet.m[(i * lenght) + j] = mat1.m[(i * lenght) + j] - mat2.m[(i * lenght) + j];
|
|
}
|
|
|
|
return matRet;
|
|
}
|
|
// += operator
|
|
public A3DMATRIX4 AdditionAssign(A3DMATRIX4 mat)
|
|
{
|
|
int lenght = 4;
|
|
A3DMATRIX4 temp = this;
|
|
for (int i = 0; i < lenght; i++)
|
|
{
|
|
for (int j = 0; j < lenght; j++)
|
|
m[(i * lenght) + j] += mat.m[(i * lenght) + j];
|
|
}
|
|
Array.Copy(temp.m, this.m, temp.m.Length);
|
|
return this;
|
|
}
|
|
// -= operator
|
|
public A3DMATRIX4 SubtractionAssign(A3DMATRIX4 mat)
|
|
{
|
|
int lenght = 4;
|
|
A3DMATRIX4 temp = this;
|
|
for (int i = 0; i < lenght; i++)
|
|
{
|
|
for (int j = 0; j < lenght; j++)
|
|
m[(i * lenght) + j] -= mat.m[(i * lenght) + 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()
|
|
{
|
|
int lenght = 4;
|
|
A3DMATRIX4 matRet = new A3DMATRIX4();
|
|
for (int i = 0; i < lenght; i++)
|
|
{
|
|
for (int j = 0; j < lenght; j++)
|
|
matRet.m[(i * lenght) + j] = m[(j * lenght) + 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[10] = m[5] = Mathf.Cos(fRad);
|
|
m[6] = Mathf.Sin(fRad);
|
|
m[9] = -m[6];
|
|
}
|
|
public void RotateY(float fRad)
|
|
{
|
|
Identity();
|
|
m[10] = m[0] = Mathf.Cos(fRad);
|
|
m[8] = Mathf.Sin(fRad);
|
|
m[2] = -m[8];
|
|
}
|
|
public void RotateZ(float fRad)
|
|
{
|
|
Identity();
|
|
m[5] = m[0] = Mathf.Cos(fRad);
|
|
m[1] = Mathf.Sin(fRad);
|
|
m[4] = -m[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
|
|
public 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 struct A3DVECTOR4
|
|
{
|
|
float x;
|
|
float y;
|
|
float z;
|
|
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(float _x, float _y, float _z, float _w)
|
|
{
|
|
x = _x;
|
|
y = _y;
|
|
z = _z;
|
|
w = _w;
|
|
m = new float[] { x, y, z, w };
|
|
}
|
|
public A3DVECTOR4(float f)
|
|
{
|
|
x = y = z = w = f;
|
|
m = new float[] { x, y, z, w };
|
|
}
|
|
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];
|
|
m = new float[] { x, y, z, w };
|
|
}
|
|
public A3DVECTOR4(A3DVECTOR4 v)
|
|
{
|
|
x = v.x; y = v.y; z = v.z; w = v.w;
|
|
m = new float[] { x, y, z, w };
|
|
}
|
|
public A3DVECTOR4(A3DVECTOR3 v)
|
|
{
|
|
x = v.x; y = v.y; z = v.z; w = 1.0f;
|
|
m = new float[] { x, y, z, w };
|
|
}
|
|
|
|
// 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; }
|
|
};
|
|
|
|
|