134 lines
3.2 KiB
C#
134 lines
3.2 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.IO.Compression;
|
|
using System.Runtime.InteropServices;
|
|
using System.Text;
|
|
|
|
namespace StgSharp.Math
|
|
{
|
|
[StructLayout(LayoutKind.Explicit)]
|
|
public struct Matrix2x2:IEquatable<Matrix2x2>
|
|
{
|
|
[FieldOffset(0)]
|
|
public Vec2D row1;
|
|
[FieldOffset(8)]
|
|
public Vec2D row2;
|
|
|
|
public Matrix2x2(Vec2D c0, Vec2D c1)
|
|
{
|
|
row1 = c0;
|
|
row2 = c1;
|
|
}
|
|
|
|
public Matrix2x2(
|
|
float a11,
|
|
float a12,
|
|
float a21,
|
|
float a22
|
|
)
|
|
{
|
|
row1 = new Vec2D(a11, a12);
|
|
row2 = new Vec2D(a21, a22);
|
|
}
|
|
|
|
bool IEquatable<Matrix2x2>.Equals(Matrix2x2 other)
|
|
{
|
|
if (this.GetHashCode() != other.GetHashCode())
|
|
{
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
return
|
|
this.row1 == other.row1
|
|
&& this.row2 == other.row2;
|
|
}
|
|
}
|
|
|
|
public override bool Equals(object obj)
|
|
{
|
|
if (obj == null)
|
|
{
|
|
return false;
|
|
}
|
|
else if (obj.GetType() != typeof(Matrix2x2))
|
|
{
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
return this.Equals((Vec3D)obj);
|
|
}
|
|
}
|
|
|
|
public override int GetHashCode()
|
|
{
|
|
return HashCode.Combine(row1,row2);
|
|
}
|
|
|
|
public override string ToString()
|
|
{
|
|
return $"{row1}\n{row2}";
|
|
}
|
|
|
|
public static Matrix2x2 operator +(Matrix2x2 array1, Matrix2x2 array2)
|
|
{
|
|
return new Matrix2x2(
|
|
array1.row1 + array2.row1,
|
|
array1.row2 + array2.row2
|
|
);
|
|
}
|
|
|
|
public static Matrix2x2 operator -(Matrix2x2 array1, Matrix2x2 array2)
|
|
{
|
|
return new Matrix2x2(
|
|
array1.row1 - array2.row1,
|
|
array1.row2 - array2.row2
|
|
);
|
|
}
|
|
|
|
public static unsafe Matrix2x2 operator *(Matrix2x2 array1, Matrix2x2 array2)
|
|
{
|
|
float* a11 = &array1.row1.X;
|
|
float* a12 = a11++ ;
|
|
float* a21 = a12++ ;
|
|
float* a22 = a21++ ;
|
|
|
|
float* b11 = &array2.row1.X ;
|
|
float* b12 = b11++ ;
|
|
float* b21 = b12++ ;
|
|
float* b22 = b21++ ;
|
|
|
|
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 Matrix2x2(u1,u5,u6,u7);
|
|
|
|
}
|
|
|
|
}
|
|
}
|