stgsharp/源代码/StgSharp/Vector/Matrix2x4.cs

using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.Text;

namespace StgSharp.Math
{

    [StructLayout(LayoutKind.Explicit)]
    public struct Matrix2x4 : IEquatable<Matrix2x4>
    {
        [FieldOffset(0)]
        public Vec4D row1;
        [FieldOffset(16)]
        public Vec4D row2;

        public Matrix2x4(Vec4D r1,Vec4D r2)
        {
            row1 = r1;
            row2 = r2;
        }

        

        public bool Equals(Matrix2x4 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(Matrix2x4))
            {
                return false;
            }
            else
            {
                return this.Equals((Matrix2x4)obj);
            }
        }

        public override int GetHashCode()
        {
            return HashCode.Combine(this.row1, this.row2);
        }

        public override string ToString()
        {
            return $"{this.row1}\n{this.row2}";
        }

        public static Matrix2x4 operator +(Matrix2x4 array1,Matrix2x4 array2)
        {
            return new Matrix2x4(
                array1.row1 + array2.row1,
                array1.row2 + array2.row2
                ) ;
        }

        public static Matrix2x4 operator -(Matrix2x4 array1, Matrix2x4 array2)
        {
            return new Matrix2x4(
                array1.row1 - array2.row1,
                array1.row2 - array2.row2
                );
        }

        public static Matrix2x4 operator *(Matrix2x4 array, float x)
        {
            return new Matrix2x4(
                array.row1 * x,
                array.row2 * x
                );
        }

        public static Matrix2x4 operator /(Matrix2x4 array, float x)
        {
            return new Matrix2x4(
                array.row1 / x,
                array.row2 / x
                );
        }

        public static unsafe Matrix2x2 operator *(Matrix2x4 array1,Matrix4x2 array2)
        {
            float* p1 = &array1.row1.X;
            float* p2 = &array2.row2.X;

            Vec2D a11 = new Vec2D(*(p1 + 0), *(p1 + 2));
            Vec2D a12 = new Vec2D(*(p1 + 1), *(p1 + 3));
            Vec2D a21 = new Vec2D(*(p1 + 4), *(p1 + 6));
            Vec2D a22 = new Vec2D(*(p1 + 5), *(p1 + 7));

            Colum2D b11 = new Colum2D(*(p2 + 0), *(p2 + 1));
            Colum2D b12 = new Colum2D(*(p2 + 2), *(p2 + 3));
            Colum2D b21 = new Colum2D(*(p2 + 4), *(p2 + 5));
            Colum2D b22 = new Colum2D(*(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 * 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);
        }
    }
}