155 lines
4.5 KiB
C#
155 lines
4.5 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Runtime.InteropServices;
|
|
using System.Text;
|
|
|
|
namespace StgSharp.Math
|
|
{
|
|
[StructLayout(LayoutKind.Explicit)]
|
|
public struct Matrix4x4:IEquatable<Matrix4x4>
|
|
{
|
|
[FieldOffset(0)]
|
|
public Vec4D row1;
|
|
[FieldOffset(16)]
|
|
public Vec4D row2;
|
|
[FieldOffset(32)]
|
|
public Vec4D row3;
|
|
[FieldOffset(48)]
|
|
public Vec4D row4;
|
|
|
|
public Matrix4x4(Vec4D r1, Vec4D r2, Vec4D r3, Vec4D r4)
|
|
{
|
|
row1 = r1;
|
|
row2 = r2;
|
|
row3 = r3;
|
|
row4 = r4;
|
|
}
|
|
|
|
public Matrix4x4(
|
|
float a11, float a12, float a13, float a14,
|
|
float a21, float a22, float a23, float a24,
|
|
float a31, float a32, float a33, float a34,
|
|
float a41, float a42, float a43, float a44
|
|
)
|
|
{
|
|
row1 = new Vec4D(a11, a12, a13, a14);
|
|
row2 = new Vec4D(a11, a12, a13, a14);
|
|
row3 = new Vec4D(a11, a12, a13, a14);
|
|
row4 = new Vec4D(a11, a12, a13, a14);
|
|
}
|
|
|
|
public override bool Equals(object obj)
|
|
{
|
|
return base.Equals(obj);
|
|
}
|
|
|
|
bool IEquatable<Matrix4x4>.Equals(Matrix4x4 other)
|
|
{
|
|
if (this.GetHashCode() != other.GetHashCode())
|
|
{
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
return this.row1 == other.row1
|
|
&& this.row2 == other.row2
|
|
&& this.row3 == other.row3
|
|
&& this.row4 == other.row4;
|
|
}
|
|
}
|
|
|
|
public override int GetHashCode()
|
|
{
|
|
return HashCode.Combine(row1, row2, row3, row4);
|
|
}
|
|
|
|
public override string ToString()
|
|
{
|
|
return $"{this.row1}\n{this.row2}\n{this.row3}\n{this.row4}";
|
|
}
|
|
|
|
public static Matrix4x4 operator +(Matrix4x4 array1, Matrix4x4 array2)
|
|
{
|
|
return new Matrix4x4(
|
|
array1.row1 + array2.row1,
|
|
array1.row2 + array2.row2,
|
|
array1.row3 + array2.row3,
|
|
array1.row4 + array2.row4
|
|
);
|
|
}
|
|
|
|
public static Matrix4x4 operator -(Matrix4x4 array1, Matrix4x4 array2)
|
|
{
|
|
return new Matrix4x4(
|
|
array1.row1 - array2.row1,
|
|
array1.row2 - array2.row2,
|
|
array1.row3 - array2.row3,
|
|
array1.row4 - array2.row4
|
|
);
|
|
}
|
|
|
|
public static Matrix4x4 operator *(Matrix4x4 array, float x)
|
|
{
|
|
return new Matrix4x4(
|
|
array.row1 * x,
|
|
array.row2 * x,
|
|
array.row3 * x,
|
|
array.row4 * x
|
|
);
|
|
}
|
|
|
|
public static unsafe Matrix4x4 operator *(Matrix4x4 array1, Matrix4x4 array2)
|
|
{
|
|
float* p1 = &array1.row1.X;
|
|
float* p2 = &array2.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 * b11;
|
|
var m2 = a12 * b11;
|
|
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 Matrix4x4(
|
|
new Vec4D(u1.row1,u5.row1),
|
|
new Vec4D(u1.row2,u5.row2),
|
|
new Vec4D(u6.row1,u7.row1),
|
|
new Vec4D(u6.row2,u7.row2)
|
|
);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|