daily update on 20230309 by nitload youmu
This commit is contained in:
Nitload
@@ -6,6 +6,615 @@ namespace StgSharp.Math
|
||||
{
|
||||
public static unsafe partial class VectorCalc
|
||||
{
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="colum"></param>
|
||||
/// <param name="vec"></param>
|
||||
/// <returns></returns>
|
||||
public static Matrix2x2 multiple(this Colum2D colum, Vec2D vec)
|
||||
{
|
||||
return new Matrix2x2(
|
||||
colum.X * vec.X, colum.X * vec.Y,
|
||||
colum.Y * vec.X, colum.Y * vec.Y
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="vec1"></param>
|
||||
/// <param name="vec2"></param>
|
||||
/// <returns></returns>
|
||||
public static float multiple(this Vec2D vec1, Vec2D vec2)
|
||||
{
|
||||
return vec1.X * vec2.X + vec1.Y * vec2.Y;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="vec"></param>
|
||||
/// <param name="colum"></param>
|
||||
/// <returns></returns>
|
||||
public static float multiple(this Vec2D vec, Colum2D colum)
|
||||
{
|
||||
return vec.X * colum.X + vec.Y * colum.Y;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="vec"></param>
|
||||
/// <param name="matrix"></param>
|
||||
/// <returns></returns>
|
||||
public static unsafe Vec2D multiple(this Vec2D vec, Matrix2x2 matrix)
|
||||
{
|
||||
return new Vec2D(
|
||||
vec.X * matrix.row1.X + vec.Y * matrix.row2.X,
|
||||
vec.X * matrix.row1.Y + vec.Y * matrix.row2.Y
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="matrix"></param>
|
||||
/// <param name="colum"></param>
|
||||
/// <returns></returns>
|
||||
public static unsafe Colum2D multiple(this Matrix2x2 matrix, Colum2D colum)
|
||||
{
|
||||
float* p1 = &matrix.row1.X;
|
||||
float* p2 = &colum.X;
|
||||
|
||||
return new Colum2D(
|
||||
*(p1 + 0) * *(p2 + 0) + *(p1 + 1) * *(p2 + 1),
|
||||
*(p1 + 2) * *(p2 + 0) + *(p1 + 3) * *(p2 + 1)
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="matrix1"></param>
|
||||
/// <param name="matrix2"></param>
|
||||
/// <returns></returns>
|
||||
public static unsafe Matrix2x2 multiple(this Matrix2x2 matrix1, Matrix2x2 matrix2)
|
||||
{
|
||||
float* a11 = &matrix1.row1.X;
|
||||
float* a12 = &matrix1.row1.Y;
|
||||
float* a21 = &matrix1.row2.X;
|
||||
float* a22 = &matrix1.row2.Y;
|
||||
|
||||
float* b11 = &matrix2.row1.X;
|
||||
float* b12 = &matrix2.row1.Y;
|
||||
float* b21 = &matrix2.row2.X;
|
||||
float* b22 = &matrix2.row2.Y;
|
||||
|
||||
var s1 = *a21 + *a22;
|
||||
var s2 = s1 - *a11;
|
||||
var s3 = *a11 - *a21;
|
||||
var s4 = *a12 - s2;
|
||||
|
||||
Console.WriteLine($"{s1}\t{s2}\t{s3}\t{s4}\n");
|
||||
|
||||
var t1 = *b12 - *b11;
|
||||
var t2 = *b22 - t1;
|
||||
var t3 = *b22 - *b12;
|
||||
var t4 = t2 - *b21;
|
||||
|
||||
var m1 = *a11 * *b11;
|
||||
var m2 = *a12 * *b21;
|
||||
var m3 = s4 * *b22;
|
||||
var m4 = *a22 * t4;
|
||||
var m5 = s1 * t1;
|
||||
var m6 = s2 * t2;
|
||||
var m7 = s3 * t3;
|
||||
|
||||
var u1 = m1 + m2;
|
||||
var u2 = m1 + m6;
|
||||
var u3 = u2 + m7;
|
||||
var u4 = u2 + m5;
|
||||
var u5 = u4 + m3;
|
||||
var u6 = u3 - m4;
|
||||
var u7 = u3 + m5;
|
||||
|
||||
return new Matrix2x2(u1, u5, u6, u7);
|
||||
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="matrix1"></param>
|
||||
/// <param name="matrix2"></param>
|
||||
/// <returns></returns>
|
||||
public static unsafe Matrix2x3 multiple(this Matrix2x2 matrix1, Matrix2x3 matrix2)
|
||||
|
||||
{
|
||||
float* p1 = &matrix1.row1.X;
|
||||
float* p2 = &matrix2.row1.X;
|
||||
|
||||
float a11 = *p1;
|
||||
float a12 = *(p1 + 1);
|
||||
float a21 = *(p1 + 2);
|
||||
float a22 = *(p1 + 3);
|
||||
|
||||
Vec2D b11 = new Vec2D(*(p2 + 0), *(p2 + 1));
|
||||
Vec2D b12 = new Vec2D(*(p2 + 2), 0);
|
||||
Vec2D b21 = new Vec2D(*(p2 + 3), *(p2 + 4));
|
||||
Vec2D b22 = new Vec2D(*(p2 + 5), 0);
|
||||
|
||||
var s1 = a21 + a22;
|
||||
var s2 = s1 - a11;
|
||||
var s3 = a11 - a21;
|
||||
var s4 = a12 - s2;
|
||||
|
||||
var t1 = b12 - b11;
|
||||
var t2 = b22 - t1;
|
||||
var t3 = b22 - b12;
|
||||
var t4 = t2 - b21;
|
||||
|
||||
var m1 = b11 * a11;
|
||||
var m2 = b21 * a12;
|
||||
var m3 = b22 * s4;
|
||||
var m4 = t4 * a22;
|
||||
var m5 = t1 * s1;
|
||||
var m6 = t2 * s2;
|
||||
var m7 = t3 * s3;
|
||||
|
||||
var u1 = m1 + m2;
|
||||
var u2 = m1 + m6;
|
||||
var u3 = u2 + m7;
|
||||
var u4 = u2 + m5;
|
||||
var u5 = u4 + m3;
|
||||
var u6 = u3 - m4;
|
||||
var u7 = u3 + m5;
|
||||
|
||||
return new Matrix2x3(
|
||||
u1.X, u1.Y, u5.X,
|
||||
u6.X, u6.Y, u7.X
|
||||
);
|
||||
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="matrix1"></param>
|
||||
/// <param name="matrix2"></param>
|
||||
/// <returns></returns>
|
||||
public static unsafe Matrix2x4 multiple(this Matrix2x2 matrix1, Matrix2x4 matrix2)
|
||||
{
|
||||
float* p1 = &matrix1.row1.X;
|
||||
float* p2 = &matrix2.row1.X;
|
||||
|
||||
float a11 = *p1;
|
||||
float a12 = *(p1 + 1);
|
||||
float a21 = *(p1 + 2);
|
||||
float a22 = *(p1 + 3);
|
||||
|
||||
Vec2D b11 = new Vec2D(*(p2 + 0), *(p2 + 1));
|
||||
Vec2D b12 = new Vec2D(*(p2 + 2), *(p2 + 3));
|
||||
Vec2D b21 = new Vec2D(*(p2 + 4), *(p2 + 5));
|
||||
Vec2D b22 = new Vec2D(*(p2 + 6), *(p2 + 7));
|
||||
|
||||
var s1 = a21 + a22;
|
||||
var s2 = s1 - a11;
|
||||
var s3 = a11 - a21;
|
||||
var s4 = a12 - s2;
|
||||
|
||||
var t1 = b12 - b11;
|
||||
var t2 = b22 - t1;
|
||||
var t3 = b22 - b12;
|
||||
var t4 = t2 - b21;
|
||||
|
||||
var m1 = b11 * a11;
|
||||
var m2 = b21 * a12;
|
||||
var m3 = b22 * s4;
|
||||
var m4 = t4 * a22;
|
||||
var m5 = t1 * s1;
|
||||
var m6 = t2 * s2;
|
||||
var m7 = t3 * s3;
|
||||
|
||||
var u1 = m1 + m2;
|
||||
var u2 = m1 + m6;
|
||||
var u3 = u2 + m7;
|
||||
var u4 = u2 + m5;
|
||||
var u5 = u4 + m3;
|
||||
var u6 = u3 - m4;
|
||||
var u7 = u3 + m5;
|
||||
|
||||
return new Matrix2x4(
|
||||
u1.X, u1.Y, u5.X, u5.Y,
|
||||
u6.X, u6.Y, u7.X, u7.Y
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="matrix"></param>
|
||||
/// <param name="vec"></param>
|
||||
/// <returns></returns>
|
||||
public static unsafe Colum2D multiple(this Matrix2x3 matrix, Colum3D vec)
|
||||
{
|
||||
return new Colum2D(
|
||||
matrix.row1.X * vec.X + matrix.row1.Y * vec.Y + matrix.row1.Z * vec.Z,
|
||||
matrix.row2.X * vec.X + matrix.row2.Y * vec.Y + matrix.row2.Z * vec.Z
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="matrix1"></param>
|
||||
/// <param name="matrix2"></param>
|
||||
/// <returns></returns>
|
||||
public static unsafe Matrix2x2 multiple(this Matrix2x3 matrix1, Matrix3x2 matrix2)
|
||||
{
|
||||
float* p1 = &matrix1.row1.X;
|
||||
float* p2 = &matrix2.row1.X;
|
||||
|
||||
Matrix2x2 a1 = new Matrix2x2(*(p1 + 0), *(p1 + 1), *(p1 + 3), *(p1 + 4));
|
||||
Colum2D a2 = new Colum2D(*(p1 + 2), *(p1 + 5));
|
||||
|
||||
Matrix2x2 b1 = new Matrix2x2(*(p2 + 0), *(p2 + 1), *(p2 + 2), *(p2 + 3));
|
||||
Vec2D b2 = new Vec2D(*(p2 + 4), *(p2 + 5));
|
||||
|
||||
return a1.multiple(b1) + a2.multiple(b2);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="matrix1"></param>
|
||||
/// <param name="matrix2"></param>
|
||||
/// <returns></returns>
|
||||
public static unsafe Matrix2x2 multiple(this Matrix2x4 matrix1, Matrix4x2 matrix2)
|
||||
{
|
||||
float* p1 = &matrix1.row1.X;
|
||||
float* p2 = &matrix2.row1.X;
|
||||
|
||||
Vec2D a11 = new Vec2D(*(p1 + 0), *(p1 + 1));
|
||||
Vec2D a12 = new Vec2D(*(p1 + 2), *(p1 + 3));
|
||||
Vec2D a21 = new Vec2D(*(p1 + 4), *(p1 + 5));
|
||||
Vec2D a22 = new Vec2D(*(p1 + 6), *(p1 + 7));
|
||||
|
||||
Colum2D b11 = new Colum2D(*(p2 + 0), *(p2 + 2));
|
||||
Colum2D b12 = new Colum2D(*(p2 + 1), *(p2 + 3));
|
||||
Colum2D b21 = new Colum2D(*(p2 + 4), *(p2 + 6));
|
||||
Colum2D b22 = new Colum2D(*(p2 + 5), *(p2 + 7));
|
||||
|
||||
var s1 = a21 + a22;
|
||||
var s2 = s1 - a11;
|
||||
var s3 = a11 - a21;
|
||||
var s4 = a12 - s2;
|
||||
|
||||
var t1 = b12 - b11;
|
||||
var t2 = b22 - t1;
|
||||
var t3 = b22 - b12;
|
||||
var t4 = t2 - b21;
|
||||
|
||||
var m1 = a11.multiple(b11);
|
||||
var m2 = a12.multiple(b21);
|
||||
var m3 = s4.multiple(b22);
|
||||
var m4 = a22.multiple(t4);
|
||||
var m5 = s1.multiple(t1);
|
||||
var m6 = s2.multiple(t2);
|
||||
var m7 = s3.multiple(t3);
|
||||
|
||||
var u1 = m1 + m2;
|
||||
var u2 = m1 + m6;
|
||||
var u3 = u2 + m7;
|
||||
var u4 = u2 + m5;
|
||||
var u5 = u4 + m3;
|
||||
var u6 = u3 - m4;
|
||||
var u7 = u3 + m5;
|
||||
|
||||
return new Matrix2x2(u1, u5, u6, u7);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="matrix1"></param>
|
||||
/// <param name="matrix2"></param>
|
||||
/// <returns></returns>
|
||||
public static unsafe Matrix2x3 multiple(this Matrix2x4 matrix1, Matrix4x3 matrix2)
|
||||
{
|
||||
float* p1 = &matrix1.row1.X;
|
||||
float* p2 = &matrix2.row1.X;
|
||||
|
||||
return new Matrix2x3(
|
||||
getMatrixMember(p1,p2), getMatrixMember(p1, p2+1), getMatrixMember(p1, p2+2),
|
||||
getMatrixMember(p1+4, p2), getMatrixMember(p1+4, p2+1), getMatrixMember(p1+4, p2 + 2)
|
||||
);
|
||||
|
||||
static float getMatrixMember(float* p1, float* p2)
|
||||
{
|
||||
float result = 0;
|
||||
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
result += *p1 * *p2;
|
||||
p1 += 1;
|
||||
p2 += 3;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public static unsafe Matrix3x3 multiple(this Matrix3x2 matrix1, Matrix2x3 matrix2)
|
||||
{
|
||||
float* p1 = &matrix1.row1.X;
|
||||
float* p2 = &matrix2.row2.X;
|
||||
|
||||
Matrix2x2 a1 = new Matrix2x2(*(p1 + 0), *(p1 + 1), *(p1 + 2), *(p1 + 3));
|
||||
Vec2D a2 = new Vec2D(*(p1 + 4), *(p1 + 5));
|
||||
|
||||
Matrix2x2 b1 = new Matrix2x2(*(p2 + 0), *(p2 + 1), *(p2 + 3), *(p2 + 4));
|
||||
Colum2D b2 = new Colum2D(*(p2 + 2), *(p2 + 5));
|
||||
|
||||
var m11 = a1.multiple(b1);
|
||||
var m12 = a1.multiple(b2);
|
||||
var m21 = a2.multiple(b1);
|
||||
var m22 = a2.multiple(b2);
|
||||
|
||||
float* p3 = &m11.row1.X;
|
||||
|
||||
return new Matrix3x3(
|
||||
*(p1 + 0), *(p1 + 1),m12.X,
|
||||
*(p1 + 2), *(p1 + 3),m12.Y,
|
||||
m21.X, m21.X, m22
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="matrix1"></param>
|
||||
/// <param name="matrix2"></param>
|
||||
/// <returns></returns>
|
||||
public static unsafe Matrix3x3 multiple(this Matrix3x3 matrix1, Matrix3x3 matrix2)
|
||||
{
|
||||
float* p1 = &matrix1.row1.X;
|
||||
float* p2 = &matrix2.row1.X;
|
||||
|
||||
Matrix2x2 a11 = new Matrix2x2(*(p1 + 0), *(p1 + 1), *(p1 + 3), *(p1 + 4));
|
||||
Colum2D a12 = new Colum2D(*(p1 + 2), *(p1 + 5));
|
||||
Vec2D a21 = new Vec2D(*(p1 + 6), *(p1 + 7));
|
||||
float a22 = *(p1 + 8);
|
||||
|
||||
Matrix2x2 b11 = new Matrix2x2(*(p2 + 0), *(p2 + 1), *(p2 + 3), *(p2 + 4));
|
||||
Colum2D b12 = new Colum2D(*(p2 + 2), *(p2 + 5));
|
||||
Vec2D b21 = new Vec2D(*(p2 + 6), *(p2 + 7));
|
||||
float b22 = *(p2 + 8);
|
||||
|
||||
var c11 = a11.multiple(b11) + a12.multiple(b21);
|
||||
var c12 = a11.multiple(b12) + a12 * b22;
|
||||
var c21 = a21.multiple(b11) + b21 * a22;
|
||||
var c22 = a21.multiple(b12) + a22 * b22;
|
||||
|
||||
float* p = &(c11.row1.X);
|
||||
|
||||
return new Matrix3x3(
|
||||
*(p + 0), *(p + 1), c12.X,
|
||||
*(p + 2), *(p + 3), c12.Y,
|
||||
c21.X, c21.Y, c22
|
||||
);
|
||||
}
|
||||
|
||||
public static unsafe Matrix3x2 multiple(this Matrix3x4 matrix1, Matrix4x2 matrix2)
|
||||
{
|
||||
float* p1 = &matrix1.row1.X;
|
||||
float* p2 = &matrix2.row1.X;
|
||||
|
||||
Matrix2x2 a11 = new Matrix2x2(*(p1 + 0), *(p1 + 1), *(p1 + 4), *(p1 + 5));
|
||||
Matrix2x2 a12 = new Matrix2x2(*(p1 + 2), *(p1 + 3), *(p1 + 6), *(p1 + 7));
|
||||
Vec2D a21 = new Vec2D(*(p1 + 8), *(p1 + 9));
|
||||
Vec2D a22 = new Vec2D(*(p1 + 10), *(p1 + 11));
|
||||
|
||||
Matrix2x2 b1 = new Matrix2x2(*(p2 + 0), *(p2 + 1), *(p2 + 2), *(p2 + 3));
|
||||
Matrix2x2 b2 = new Matrix2x2(*(p2 + 4), *(p2 + 5), *(p2 + 6), *(p2 + 7));
|
||||
|
||||
var c1 = a11.multiple(b1) + a12.multiple(b1);
|
||||
var c2 = a21.multiple(b2) + a22.multiple(b2);
|
||||
|
||||
return new Matrix3x2(c1.row1,c1.row2,c2);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="matrix1"></param>
|
||||
/// <param name="matrix2"></param>
|
||||
/// <returns></returns>
|
||||
public static unsafe Matrix3x3 multiple(this Matrix3x4 matrix1, Matrix4x3 matrix2)
|
||||
{
|
||||
float* p1 = &matrix1.row1.X;
|
||||
float* p2 = &matrix2.row1.X;
|
||||
|
||||
Matrix2x2 a11 = new Matrix2x2(
|
||||
*(p1 + 0), *(p1 + 1), *(p1 + 4), *(p1 + 5)
|
||||
);
|
||||
Matrix2x2 a12 = new Matrix2x2(
|
||||
*(p1 + 2), *(p1 + 3), *(p1 + 6), *(p1 + 7)
|
||||
);
|
||||
Vec2D a21 = new Vec2D(*(p1 + 8), *(p1 + 9));
|
||||
Vec2D a22 = new Vec2D(*(p1 + 10), *(p1 + 11));
|
||||
|
||||
Matrix2x2 b11 = new Matrix2x2(
|
||||
*(p2 + 0), *(p2 + 1), *(p2 + 3), *(p2 + 4)
|
||||
);
|
||||
Colum2D b12 = new Colum2D(*(p2 + 2), *(p2 + 5));
|
||||
Matrix2x2 b21 = new Matrix2x2(
|
||||
*(p2 + 6), *(p2 + 7), *(p2 + 9), *(p2 + 10)
|
||||
);
|
||||
Colum2D b22 = new Colum2D(*(p2 + 8), *(p2 + 11));
|
||||
|
||||
var c11 = a11.multiple(b11) + a12.multiple(b21);
|
||||
var c12 = a11.multiple(b12) + a12.multiple(b22);
|
||||
var c21 = a21.multiple(b11) + a22.multiple(b21);
|
||||
var c22 = a21.multiple(b12) + a22.multiple(b22);
|
||||
|
||||
float* pc1 = &c11.row1.X;
|
||||
|
||||
return new Matrix3x3(
|
||||
*(pc1 + 0), *(pc1 + 1), c12.X,
|
||||
*(pc1 + 2), *(pc1 + 3), c12.Y,
|
||||
c21.X, c21.Y, c22
|
||||
);
|
||||
}
|
||||
|
||||
public static unsafe Matrix3x4 multiple(this Matrix3x4 matrix1, Matrix4x4 matrix2)
|
||||
{
|
||||
float* p1 = &matrix1.row1.X;
|
||||
float* p2 = &matrix2.row1.X;
|
||||
|
||||
var a11 = new Matrix2x2(matrix1.row1.Point1,matrix1.row2.Point1);
|
||||
var a12 = new Matrix2x2(matrix1.row1.Point2,matrix1.row2.Point2);
|
||||
var a21 = matrix1.row3.Point1;
|
||||
var a22 = matrix1.row3.Point2;
|
||||
|
||||
var b11 = new Matrix2x2(matrix2.row1.Point1, matrix2.row2.Point1);
|
||||
var b12 = new Matrix2x2(matrix2.row1.Point2, matrix2.row2.Point2);
|
||||
var b21 = new Matrix2x2(matrix2.row3.Point1, matrix2.row4.Point1);
|
||||
var b22 = new Matrix2x2(matrix2.row3.Point2, matrix2.row4.Point2);
|
||||
|
||||
var c1 = a11.multiple(b11) + a12.multiple(b21);
|
||||
var c2 = a11.multiple(b12) + a12.multiple(b22);
|
||||
var c3 = a21.multiple(b11) + a22.multiple(b21);
|
||||
var c4 = a21.multiple(b12) + a22.multiple(b22);
|
||||
|
||||
return new Matrix3x4(
|
||||
new Vec4D(c1.row1,c2.row1),
|
||||
new Vec4D(c1.row2,c2.row2),
|
||||
new Vec4D(c3,c4)
|
||||
) ;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="matrix1"></param>
|
||||
/// <param name="matrix2"></param>
|
||||
/// <returns></returns>
|
||||
public static unsafe Matrix4x4 multiple(this Matrix4x2 matrix1, Matrix2x4 matrix2)
|
||||
{
|
||||
float* p1 = &matrix1.row1.X;
|
||||
float* p2 = &matrix2.row1.X;
|
||||
|
||||
Colum2D a11 = new Colum2D(*(p1 + 0), *(p1 + 2));
|
||||
Colum2D a12 = new Colum2D(*(p1 + 1), *(p1 + 3));
|
||||
Colum2D a21 = new Colum2D(*(p1 + 4), *(p1 + 6));
|
||||
Colum2D a22 = new Colum2D(*(p1 + 5), *(p1 + 7));
|
||||
|
||||
Vec2D b11 = new Vec2D(*(p2 + 0), *(p2 + 1));
|
||||
Vec2D b12 = new Vec2D(*(p2 + 2), *(p2 + 3));
|
||||
Vec2D b21 = new Vec2D(*(p2 + 4), *(p2 + 5));
|
||||
Vec2D b22 = new Vec2D(*(p2 + 6), *(p2 + 7));
|
||||
|
||||
var s1 = a21 + a22;
|
||||
var s2 = s1 - a11;
|
||||
var s3 = a11 - a21;
|
||||
var s4 = a12 - s2;
|
||||
|
||||
var t1 = b12 - b11;
|
||||
var t2 = b22 - t1;
|
||||
var t3 = b22 - b12;
|
||||
var t4 = t2 - b21;
|
||||
|
||||
var m1 = a11.multiple(b11);
|
||||
var m2 = a12.multiple(b21);
|
||||
var m3 = s4.multiple(b22);
|
||||
var m4 = a22.multiple(t4);
|
||||
var m5 = s1.multiple(t1);
|
||||
var m6 = s2.multiple(t2);
|
||||
var m7 = s3.multiple(t3);
|
||||
|
||||
var u1 = m1 + m2;
|
||||
var u2 = m1 + m6;
|
||||
var u3 = u2 + m7;
|
||||
var u4 = u2 + m5;
|
||||
var u5 = u4 + m3;
|
||||
var u6 = u3 - m4;
|
||||
var u7 = u3 + m5;
|
||||
|
||||
return new Matrix4x4(
|
||||
new Vec4D(u1.row1.X, u1.row1.Y, u5.row1.X, u5.row1.Y),
|
||||
new Vec4D(u1.row2.X, u1.row2.Y, u5.row2.X, u5.row2.Y),
|
||||
new Vec4D(u6.row1.X, u6.row1.Y, u7.row1.X, u7.row1.Y),
|
||||
new Vec4D(u6.row2.X, u6.row2.Y, u7.row2.X, u7.row2.Y)
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="matrix1"></param>
|
||||
/// <param name="matrix2"></param>
|
||||
/// <returns></returns>
|
||||
public static unsafe Matrix4x4 multiple(this Matrix4x4 matrix1, Matrix4x4 matrix2)
|
||||
{
|
||||
float* p1 = &matrix1.row1.X;
|
||||
float* p2 = &matrix2.row1.X;
|
||||
|
||||
Matrix2x2 a11 = new Matrix2x2(*p1, *(p1 + 1), *(p1 + 4), *(p1 + 5));
|
||||
Matrix2x2 a12 = new Matrix2x2(*(p1 + 2), *(p1 + 3), *(p1 + 6), *(p1 + 7));
|
||||
Matrix2x2 a21 = new Matrix2x2(*(p1 + 8), *(p1 + 9), *(p1 + 12), *(p1 + 13));
|
||||
Matrix2x2 a22 = new Matrix2x2(*(p1 + 10), *(p1 + 11), *(p1 + 14), *(p1 + 15));
|
||||
|
||||
Matrix2x2 b11 = new Matrix2x2(*p1, *(p1 + 1), *(p1 + 4), *(p1 + 5));
|
||||
Matrix2x2 b12 = new Matrix2x2(*(p1 + 2), *(p1 + 3), *(p1 + 6), *(p1 + 7));
|
||||
Matrix2x2 b21 = new Matrix2x2(*(p1 + 8), *(p1 + 9), *(p1 + 12), *(p1 + 13));
|
||||
Matrix2x2 b22 = new Matrix2x2(*(p1 + 10), *(p1 + 11), *(p1 + 14), *(p1 + 15));
|
||||
|
||||
var s1 = a21 + a22;
|
||||
var s2 = s1 - a11;
|
||||
var s3 = a11 - a21;
|
||||
var s4 = a12 - s2;
|
||||
|
||||
var t1 = b12 - b11;
|
||||
var t2 = b22 - t1;
|
||||
var t3 = b22 - b12;
|
||||
var t4 = t2 - b21;
|
||||
|
||||
var m1 = a11.multiple(b11);
|
||||
var m2 = a12.multiple(b21);
|
||||
var m3 = s4.multiple(b22);
|
||||
var m4 = a22.multiple(t4);
|
||||
var m5 = s1.multiple(t1);
|
||||
var m6 = s2.multiple(t2);
|
||||
var m7 = s3.multiple(t3);
|
||||
|
||||
var u1 = m1 + m2;
|
||||
var u2 = m1 + m6;
|
||||
var u3 = u2 + m7;
|
||||
var u4 = u2 + m5;
|
||||
var u5 = u4 + m3;
|
||||
var u6 = u3 - m4;
|
||||
var u7 = u3 + m5;
|
||||
|
||||
return new Matrix4x4(
|
||||
new Vec4D(u1.row1.X, u1.row1.Y, u5.row1.X, u5.row1.Y),
|
||||
new Vec4D(u1.row2.X, u1.row2.Y, u5.row2.X, u5.row2.Y),
|
||||
new Vec4D(u6.row1.X, u6.row1.Y, u7.row1.X, u7.row1.Y),
|
||||
new Vec4D(u6.row2.X, u6.row2.Y, u7.row2.X, u7.row2.Y)
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user