public void MultiplyRev(Matrix4 firstMatrix)
{
double n_a1,
n_a2,
n_a3,
n_a4,
n_b1,
n_b2,
n_b3,
n_b4,
n_c1,
n_c2,
n_c3,
n_c4,
n_d1,
n_d2,
n_d3,
n_d4;
n_a1 = firstMatrix.A1 * A1 + firstMatrix.A2 * B1 + firstMatrix.A3 * C1 + firstMatrix.A4 * D1;
n_a2 = firstMatrix.A1 * A2 + firstMatrix.A2 * B2 + firstMatrix.A3 * C2 + firstMatrix.A4 * D2;
n_a3 = firstMatrix.A1 * A3 + firstMatrix.A2 * B3 + firstMatrix.A3 * C3 + firstMatrix.A4 * D3;
n_a4 = firstMatrix.A1 * A4 + firstMatrix.A2 * B4 + firstMatrix.A3 * C4 + firstMatrix.A4 * D4;
n_b1 = firstMatrix.B1 * A1 + firstMatrix.B2 * B1 + firstMatrix.B3 * C1 + firstMatrix.B4 * D1;
n_b2 = firstMatrix.B1 * A2 + firstMatrix.B2 * B2 + firstMatrix.B3 * C2 + firstMatrix.B4 * D2;
n_b3 = firstMatrix.B1 * A3 + firstMatrix.B2 * B3 + firstMatrix.B3 * C3 + firstMatrix.B4 * D3;
n_b4 = firstMatrix.B1 * A4 + firstMatrix.B2 * B4 + firstMatrix.B3 * C4 + firstMatrix.B4 * D4;
n_c1 = firstMatrix.C1 * A1 + firstMatrix.C2 * B1 + firstMatrix.C3 * C1 + firstMatrix.C4 * D1;
n_c2 = firstMatrix.C1 * A2 + firstMatrix.C2 * B2 + firstMatrix.C3 * C2 + firstMatrix.C4 * D2;
n_c3 = firstMatrix.C1 * A3 + firstMatrix.C2 * B3 + firstMatrix.C3 * C3 + firstMatrix.C4 * D3;
n_c4 = firstMatrix.C1 * A4 + firstMatrix.C2 * B4 + firstMatrix.C3 * C4 + firstMatrix.C4 * D4;
n_d1 = firstMatrix.D1 * A1 + firstMatrix.D2 * B1 + firstMatrix.D3 * C1 + firstMatrix.D4 * D1;
n_d2 = firstMatrix.D1 * A2 + firstMatrix.D2 * B2 + firstMatrix.D3 * C2 + firstMatrix.D4 * D2;
n_d3 = firstMatrix.D1 * A3 + firstMatrix.D2 * B3 + firstMatrix.D3 * C3 + firstMatrix.D4 * D3;
n_d4 = firstMatrix.D1 * A4 + firstMatrix.D2 * B4 + firstMatrix.D3 * C4 + firstMatrix.D4 * D4;
A1 = n_a1;
A2 = n_a2;
A3 = n_a3;
A4 = n_a4;
B1 = n_b1;
B2 = n_b2;
B3 = n_b3;
B4 = n_b4;
C1 = n_c1;
C2 = n_c2;
C3 = n_c3;
C4 = n_c4;
D1 = n_d1;
D2 = n_d2;
D3 = n_d3;
D4 = n_d4;
}
public void Multiply(Matrix4 sM)
{
double n_a1,
n_a2,
n_a3,
n_a4,
n_b1,
n_b2,
n_b3,
n_b4,
n_c1,
n_c2,
n_c3,
n_c4,
n_d1,
n_d2,
n_d3,
n_d4;
n_a1 = A1 * sM.A1 + A2 * sM.B1 + A3 * sM.C1 + A4 * sM.D1;
n_a2 = A1 * sM.A2 + A2 * sM.B2 + A3 * sM.C2 + A4 * sM.D2;
n_a3 = A1 * sM.A3 + A2 * sM.B3 + A3 * sM.C3 + A4 * sM.D3;
n_a4 = A1 * sM.A4 + A2 * sM.B4 + A3 * sM.C4 + A4 * sM.D4;
n_b1 = B1 * sM.A1 + B2 * sM.B1 + B3 * sM.C1 + B4 * sM.D1;
n_b2 = B1 * sM.A2 + B2 * sM.B2 + B3 * sM.C2 + B4 * sM.D2;
n_b3 = B1 * sM.A3 + B2 * sM.B3 + B3 * sM.C3 + B4 * sM.D3;
n_b4 = B1 * sM.A4 + B2 * sM.B4 + B3 * sM.C4 + B4 * sM.D4;
n_c1 = C1 * sM.A1 + C2 * sM.B1 + C3 * sM.C1 + C4 * sM.D1;
n_c2 = C1 * sM.A2 + C2 * sM.B2 + C3 * sM.C2 + C4 * sM.D2;
n_c3 = C1 * sM.A3 + C2 * sM.B3 + C3 * sM.C3 + C4 * sM.D3;
n_c4 = C1 * sM.A4 + C2 * sM.B4 + C3 * sM.C4 + C4 * sM.D4;
n_d1 = D1 * sM.A1 + D2 * sM.B1 + D3 * sM.C1 + D4 * sM.D1;
n_d2 = D1 * sM.A2 + D2 * sM.B2 + D3 * sM.C2 + D4 * sM.D2;
n_d3 = D1 * sM.A3 + D2 * sM.B3 + D3 * sM.C3 + D4 * sM.D3;
n_d4 = D1 * sM.A4 + D2 * sM.B4 + D3 * sM.C4 + D4 * sM.D4;
A1 = n_a1;
A2 = n_a2;
A3 = n_a3;
A4 = n_a4;
B1 = n_b1;
B2 = n_b2;
B3 = n_b3;
B4 = n_b4;
C1 = n_c1;
C2 = n_c2;
C3 = n_c3;
C4 = n_c4;
D1 = n_d1;
D2 = n_d2;
D3 = n_d3;
D4 = n_d4;
}
// apply addition values to Matrix using multiply
// addition to the "this" matrix
// "this matrix" = addtion matrix + "this" matrix
public void Add(double dX, double dY, double dZ)
{
var addMatrix = new Matrix4(dX, dY, dZ);
addMatrix.Multiply(this);
A1 = addMatrix.A1;
A2 = addMatrix.A2;
A3 = addMatrix.A3;
A4 = addMatrix.A4;
B1 = addMatrix.B1;
B2 = addMatrix.B2;
B3 = addMatrix.B3;
B4 = addMatrix.B4;
C1 = addMatrix.C1;
C2 = addMatrix.C2;
C3 = addMatrix.C3;
C4 = addMatrix.C4;
D1 = addMatrix.D1;
D2 = addMatrix.D2;
D3 = addMatrix.D3;
D4 = addMatrix.D4;
}
public void CopyValueFrom(Matrix4 source)
{
A1 = source.A1;
A2 = source.A2;
A3 = source.A3;
A4 = source.A4;
B1 = source.B1;
B2 = source.B2;
B3 = source.B3;
B4 = source.B4;
C1 = source.C1;
C2 = source.C2;
C3 = source.C3;
C4 = source.C4;
D1 = source.D1;
D2 = source.D2;
D3 = source.D3;
D4 = source.D4;
}
public static Matrix4 Rotate(double cosx, double sinx, double cosy, double siny, double cosz, double sinz)
{
var xRotate = new Matrix4(
1, 0, 0, 0,
0, cosx, -sinx, 0,
0, sinx, cosx, 0,
0, 0, 0, 1
);
var yRotate = new Matrix4(
cosy, 0, siny, 0,
0, 1, 0, 0,
-siny, 0, cosy, 0,
0, 0, 0, 1);
var zRotate = new Matrix4(
cosz, -sinz, 0, 0,
sinz, cosz, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
xRotate.Multiply(yRotate);
xRotate.Multiply(zRotate);
return xRotate;
}