public static Matrix4B4 operator *(Matrix4B4 lhs, Matrix4B4 rhs)
{
Matrix4B4 RET = new Matrix4B4(new Vector4(), new Vector4(), new Vector4(), new Vector4()); //Create a new matrix 4 by 4
RET.values[0, 0] = lhs.values[0, 0] * rhs.values[0, 0] + lhs.values[0, 1] * rhs.values[1, 0] + lhs.values[0, 2] * rhs.values[2, 0] + lhs.values[0, 3] * rhs.values[3, 0]; //Rows multiplied by Columns Row is first Number Columns is second number
RET.values[0, 1] = lhs.values[0, 0] * rhs.values[0, 1] + lhs.values[0, 1] * rhs.values[1, 1] + lhs.values[0, 2] * rhs.values[2, 1] + lhs.values[0, 3] * rhs.values[3, 1]; //Rows multiplied by Columns Row is first Number Columns is second number
RET.values[0, 2] = lhs.values[0, 0] * rhs.values[0, 2] + lhs.values[0, 1] * rhs.values[1, 2] + lhs.values[0, 2] * rhs.values[2, 2] + lhs.values[0, 3] * rhs.values[3, 2]; //Rows multiplied by Columns Row is first Number Columns is second number
RET.values[0, 3] = lhs.values[0, 0] * rhs.values[0, 3] + lhs.values[0, 1] * rhs.values[1, 3] + lhs.values[0, 2] * rhs.values[2, 3] + lhs.values[0, 3] * rhs.values[3, 3]; ///Rows multiplied by Columns Row is first Number Columns is second number
RET.values[1, 0] = lhs.values[1, 0] * rhs.values[0, 0] + lhs.values[1, 1] * rhs.values[1, 0] + lhs.values[1, 2] * rhs.values[2, 0] + lhs.values[1, 3] * rhs.values[3, 0]; //Rows multiplied by Columns Row is first Number Columns is second number
RET.values[1, 1] = lhs.values[1, 0] * rhs.values[0, 1] + lhs.values[1, 1] * rhs.values[1, 1] + lhs.values[1, 2] * rhs.values[2, 1] + lhs.values[1, 3] * rhs.values[3, 1]; //Rows multiplied by Columns Row is first Number Columns is second number
RET.values[1, 2] = lhs.values[1, 0] * rhs.values[0, 2] + lhs.values[1, 1] * rhs.values[1, 2] + lhs.values[1, 2] * rhs.values[2, 2] + lhs.values[1, 3] * rhs.values[3, 2]; //Rows multiplied by Columns Row is first Number Columns is second number
RET.values[1, 3] = lhs.values[1, 0] * rhs.values[0, 3] + lhs.values[1, 1] * rhs.values[1, 3] + lhs.values[1, 2] * rhs.values[2, 3] + lhs.values[1, 3] * rhs.values[3, 3]; ///Rows multiplied by Columns Row is first Number Columns is second number
RET.values[2, 0] = lhs.values[2, 0] * rhs.values[0, 0] + lhs.values[2, 1] * rhs.values[1, 0] + lhs.values[2, 2] * rhs.values[2, 0] + lhs.values[2, 3] * rhs.values[3, 0]; //Rows multiplied by Columns Row is first Number Columns is second number
RET.values[2, 1] = lhs.values[2, 0] * rhs.values[0, 1] + lhs.values[2, 1] * rhs.values[1, 1] + lhs.values[2, 2] * rhs.values[2, 1] + lhs.values[2, 3] * rhs.values[3, 1]; //Rows multiplied by Columns Row is first Number Columns is second number
RET.values[2, 2] = lhs.values[2, 0] * rhs.values[0, 2] + lhs.values[2, 1] * rhs.values[1, 2] + lhs.values[2, 2] * rhs.values[2, 2] + lhs.values[2, 3] * rhs.values[3, 2]; //Rows multiplied by Columns Row is first Number Columns is second number
RET.values[2, 3] = lhs.values[2, 0] * rhs.values[0, 3] + lhs.values[2, 1] * rhs.values[1, 3] + lhs.values[2, 2] * rhs.values[2, 3] + lhs.values[2, 3] * rhs.values[3, 3]; //Rows Into Columns Row is first Number Columns is second number
RET.values[3, 0] = lhs.values[3, 0] * rhs.values[0, 0] + lhs.values[3, 1] * rhs.values[1, 0] + lhs.values[3, 2] * rhs.values[2, 0] + lhs.values[3, 3] * rhs.values[3, 0]; //Rows multiplied by Columns Row is first Number Columns is second number
RET.values[3, 1] = lhs.values[3, 0] * rhs.values[0, 1] + lhs.values[3, 1] * rhs.values[1, 1] + lhs.values[3, 2] * rhs.values[2, 1] + lhs.values[3, 3] * rhs.values[3, 1]; //Rows multiplied by Columns Row is first Number Columns is second number
RET.values[3, 2] = lhs.values[3, 0] * rhs.values[0, 2] + lhs.values[3, 1] * rhs.values[1, 2] + lhs.values[3, 2] * rhs.values[2, 2] + lhs.values[3, 3] * rhs.values[3, 2]; //Rows multiplied by Columns Row is first Number Columns is second number
RET.values[3, 3] = lhs.values[3, 0] * rhs.values[0, 3] + lhs.values[3, 1] * rhs.values[1, 3] + lhs.values[3, 2] * rhs.values[2, 3] + lhs.values[3, 3] * rhs.values[3, 3]; //Rows multiplied by Columns Row is first Number Columns is second number
return(RET); //Create RET //Create RET //Return RET
}
public Matrix4B4 TranslationInverse()
{
Matrix4B4 rv = Identiy; //Create identity matrix
rv.values[0, 3] = -values[0, 3]; //Set values of 0, 3 to be inverse of 0,3
rv.values[1, 3] = -values[1, 3]; //Set values of 0, 3 to be inverse of 1,3
rv.values[2, 3] = -values[2, 3]; //Set values of 0, 3 to be inverse of 2,3
return(rv); //Return RV
}
public Quat MatToQuat(Matrix4B4 m)
{
Quat RET = new Quat();
//Rotating Objects Using Quaternions (Bobic, 1998)
//Moefied to work with my maths
float tr, s;
float[] q = new float[4];
int i, j, k;
int[] nxt = new int[3];
nxt[0] = 1;
nxt[1] = 2;
nxt[2] = 0;
tr = m.values[0, 0] + m.values[1, 1] + m.values[2, 2];
// check the diagonal
if (tr > 0.0)
{
s = Mathf.Sqrt(tr + 1.0f);
RET.w = s / 2.0f;
s = 0.5f / s;
RET.x = (m.values[1, 2] - m.values[2, 1]) * s;
RET.y = (m.values[2, 0] - m.values[0, 2]) * s;
RET.z = (m.values[0, 1] - m.values[1, 0]) * s;
return(RET); //Create RET
}
else
{
// diagonal is negative
i = 0;
if (m.values[1, 1] > m.values[0, 0])
{
i = 1;
}
if (m.values[2, 2] > m.values[i, i])
{
i = 2;
}
j = nxt[i];
k = nxt[j];
s = Mathf.Sqrt((m.values[i, i] - (m.values[j, j] + m.values[k, k])) + 1.0f);
q[i] = s * 0.5f;
if (s != 0.0)
{
s = 0.5f / s;
}
q[3] = (m.values[j, k] - m.values[k, j]) * s;
q[j] = (m.values[i, j] + m.values[j, i]) * s;
q[k] = (m.values[i, k] + m.values[k, i]) * s;
RET.x = q[0];
RET.y = q[1];
RET.z = q[2];
RET.w = q[3];
return(RET); //Create RET
}
}
public Matrix4B4 ScaleInverse()
{
Matrix4B4 rv = Identiy; //Create identity matrix
rv.values[0, 0] = 1.0f / values[0, 0]; //set 0,0 to be 1 divided by value [0,0]
rv.values[1, 1] = 1.0f / values[1, 1]; //set 1,1 to be 1 divided by value [1,1]
rv.values[2, 2] = 1.0f / values[2, 2]; //set 2,2 to be 1 divided by value [2,2]
return(rv); //Return RV
}
public Matrix4B4 InvertTR()
{
Matrix4B4 rv = Identiy;
for (int i = 0; i < 4; i++)
{
for (int j = i + 1; j < 4; j++)
{
rv.values[i, j] = values[j, i]; //Transpose matrix
}
}
rv.SetColumn(3, -(rv * GetColum(3))); //Set the colum to inverse of rv * column 4
return(rv); //return RV
}
public static MyVector3 QuatToEuler(Quat q1)
{
// Rotation Matrix To Euler Angles(Mallick, 2016)
if (float.IsNaN(q1.w))
{
q1.w = 0; //set w to be 0
}
if (float.IsNaN(q1.x))
{
q1.x = 0; //set x to be 0
}
if (float.IsNaN(q1.y))
{
q1.y = 0; //set y to be 0
}
if (float.IsNaN(q1.z))
{
q1.z = 0; //set z to be 0
}
Matrix4B4 m = Matrix4B4.QuatToMatrix(q1); //Convert quat to matrix
m = m.Transpose;
float sy = Mathf.Sqrt(m.values[0, 0] * m.values[0, 0] + m.values[1, 0] * m.values[1, 0]); //Sqrt of 0,0 * 0,0 + 1,0 * 1,0
bool singular = sy < 1e-6; //Check if sy is less then 1e-6
float x, y, z; //Create x y and z
if (!singular)
{
x = Mathf.Atan2(m.values[2, 1], m.values[2, 2]); //Set x to be atan2 of 2,1 and 2,2
y = Mathf.Atan2(-m.values[2, 0], sy); //Set y to be atan2 of -2,0 and sy
z = Mathf.Atan2(m.values[1, 0], m.values[0, 0]); //Set z to be atan2 of 1,0 and 0,0
}
else
{
x = Mathf.Atan2(-m.values[1, 2], m.values[1, 1]); //Set x to be atan2 of 1,2 and 1,1
y = Mathf.Atan2(-m.values[2, 0], sy); //SEt y to be atan2 of - 2,0 and sy
z = 0; //Set z to be 0
}
MyVector3 RET = new MyVector3(x, y, z); //Set ret to be x y z
RET *= Mathf.Rad2Deg; //Convert RET to degres
return(RET); //Create RET
}
public static Matrix4B4 QuatToMatrix(Quat quat)
{
//Rotating Objects Using Quaternions (Bobic, 1998)
//Modefied to fit my maths
Matrix4B4 RET = Identiy; //Create Identiy Matrix
float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; //Creates coefficent values
// calculate coefficients
x2 = quat.x + quat.x; //set X2 To be x * x
y2 = quat.y + quat.y; //set Y2 To be Y * Y
z2 = quat.z + quat.z; //set Z2 To be Z * Z
xx = quat.x * x2; //Multiply x by X2
xy = quat.x * y2; //Multiply x by Y2
xz = quat.x * z2; //Multiply x by Z2
yy = quat.y * y2; //Multiply y by Y2
yz = quat.y * z2; //Multiply y by Z2
zz = quat.z * z2; //Multiply z by Z2
wx = quat.w * x2; //Multiply w by X2
wy = quat.w * y2; //Multiply w by Y2
wz = quat.w * z2; //Multiply w by Z2
RET.values[0, 0] = 1.0f - (yy + zz); //Set 0, 0 to be 1 - (yy + zz)
RET.values[1, 0] = xy - wz; //Set 1, 0 to be xy - wz
RET.values[2, 0] = xz + wy; //Set 2, 0 to be xz wy
RET.values[3, 0] = 0.0f; //Set 3, 0 to be 0.0f
RET.values[0, 1] = xy + wz; //Set 0, 1 to be Xy + Wz
RET.values[1, 1] = 1.0f - (xx + zz); //Set 1, 1 to be 1 - (xx + zz)
RET.values[2, 1] = yz - wx; //Set 2, 1 to be yz - wx
RET.values[3, 1] = 0.0f; //Set 3,1 to be 0
RET.values[0, 2] = xz - wy; //Set 0, 2 to be xz - wy
RET.values[1, 2] = yz + wx; //Set 1, 2 to be yz + wx
RET.values[2, 2] = 1.0f - (xx + yy); //Set 2,2 to be 1 - (xx + yy)
RET.values[3, 2] = 0.0f; //Set 3, 2 to be 0
RET.values[0, 3] = 0; //Set 0, 3 to be 0
RET.values[1, 3] = 0; //Set 1, 3 to be 0
RET.values[2, 3] = 0; //Set 2, 3 to be 0
RET.values[3, 3] = 1; //Set 3, 3 to be 1
return(RET); //Create RET
}
void Update()
{
Vector3[] TransformedVertices = new Vector3[ModelSpaceVertices.Length]; //Creates Transformed Vertices And sets array length to stop memory leak issues
//Scale MAtrix created by setting scale diagonaly down a matrix
Matrix4B4 scaleMatrix = new Matrix4B4(
new MyVector3(Scale.x, 0, 0),
new MyVector3(0, Scale.y, 0),
new MyVector3(0, 0, Scale.z),
MyVector3.zero
);
//Translation Matrix, created by using an identity matrix for first 3 rows then set last row to be translation
Matrix4B4 translationMatrix = new Matrix4B4(
new MyVector3(1, 0, 0),
new MyVector3(0, 1, 0),
new MyVector3(0, 0, 1),
new MyVector3(Translation.x, Translation.y, Translation.z)
);
//Rotatio
MyVector3 RotationInRadians = new MyVector3(); //Creates Rotation in radians
//Convert Rotation to Radians
RotationInRadians.x = VectorMaths.Deg2Rad(Rotation.x); //Converts Rotation x to Radians
RotationInRadians.y = VectorMaths.Deg2Rad(Rotation.y); //Converts Rotation y to Radians
RotationInRadians.z = VectorMaths.Deg2Rad(Rotation.z); //Converts Rotation z to Radians
QuatRotation = Quat.EulerToQuat(RotationInRadians); //Convert Euler Rotation To Quat Rotation
RotationMatrix = Matrix4B4.QuatToMatrix(QuatRotation); //Convert Quat rotation to matrix
Matrix4B4 M = translationMatrix * RotationMatrix * scaleMatrix; //Transformation matrix set my multiplying matrcies in TRS ordeer
//Loop through Model points and transform them from local to world
for (int i = 0; i < TransformedVertices.Length; i++)
{
TransformedVertices[i] = M * ModelSpaceVertices[i]; //Transform vertices by multiplying matrix by Model spacee verticies
}
//Goes through all model space vertices to create model bounds
for (int i = 0; i < ModelSpaceVertices.Length; i++)
{
//If Model space vertice is less then model min extent then set model min extent to be model space verticies repeats for x y and z
if (ModelSpaceVertices[i].x < ModelMinExtent.x)
{
ModelMinExtent.x = ModelSpaceVertices[i].x; //Set Model Min Extent x to model space vertices x
}
if (ModelSpaceVertices[i].y < ModelMinExtent.y)
{
ModelMinExtent.y = ModelSpaceVertices[i].y; //Set Model Min Extent y to model space vertices y
}
if (ModelSpaceVertices[i].z < ModelMinExtent.z)
{
ModelMinExtent.z = ModelSpaceVertices[i].z; //Set Model Min Extent z to model space vertices z
}
//If Model space vertice is more then model max extent then set model max extent to be model space verticies repeats for x y and z
if (ModelSpaceVertices[i].x > ModelMaxExtent.x)
{
ModelMaxExtent.x = ModelSpaceVertices[i].x; //Set Model Max Extent x to model space vertices x
}
if (ModelSpaceVertices[i].y > ModelMaxExtent.y)
{
ModelMaxExtent.y = ModelSpaceVertices[i].y; //Set Model Max Extent y to model space vertices y
}
if (ModelSpaceVertices[i].z > ModelMaxExtent.z)
{
ModelMaxExtent.z = ModelSpaceVertices[i].z; //Set Model Max Extent z to model space vertices z
}
}
//Unity Mesh Filter
MeshFilter MF = GetComponent <MeshFilter>(); //Gets mesh filter
MF.mesh.vertices = TransformedVertices; //Sets vertices to be the transformed vertices
MF.mesh.RecalculateNormals(); //Calls Recalculate Normal
MF.mesh.RecalculateBounds(); //Calls Recalculate bounds
}
