stgsharp/源代码/StgSharp/Math/Linear/matrix4x4.cs

using System;
using System.Drawing;
using System.Numerics;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using System.Threading.Tasks;



namespace StgSharp.Math
{
    

    [StructLayout(LayoutKind.Explicit, Size = 2*16*sizeof(float)+sizeof(bool), Pack = 16)]
    public struct Matrix4x4 : IEquatable<Matrix4x4>
    {
        [FieldOffset(0)] internal Mat4 mat;
        [FieldOffset(64*sizeof(float))] internal Mat4 transpose;
        [FieldOffset(2*64*sizeof(float))]internal bool isTransposed;
        

        public unsafe Matrix4x4(
            float a00, float a01, float a02, float a03,
            float a10, float a11, float a12, float a13,
            float a20, float a21, float a22, float a23,
            float a30, float a31, float a32, float a33
            )
        {
            M128* c0ref = internalIO.loadptr_internal(a00, a10, a20, a30);
            M128* c1ref = internalIO.loadptr_internal(a01, a11, a21, a31);
            M128* c2ref = internalIO.loadptr_internal(a02, a12, a22, a32);
            M128* c3ref = internalIO.loadptr_internal(a03, a13, a23, a33);

            mat.colum0 = *c0ref;
            mat.colum1 = *c1ref;
            mat.colum2 = *c2ref;
            mat.colum3 = *c3ref;

            /**/
            internalIO.deinitM128_internal(c0ref);
            internalIO.deinitM128_internal(c1ref);
            internalIO.deinitM128_internal(c2ref);
            internalIO.deinitM128_internal(c3ref);
            /**/

        }

        public unsafe float this[int rowNum, int columNum]
        {
            get
            {
                if (rowNum>3||rowNum<0)
                {
                    throw new ArgumentOutOfRangeException(nameof(columNum));
                }
                if (columNum>3||columNum<0)
                {
                    throw new ArgumentOutOfRangeException(nameof(columNum));
                }
                InternalTranspose();
                fixed (float* p = &this.transpose.a00 )
                {
                    ulong pbit = (ulong)p 
                        + (ulong)sizeof(M128) * (ulong)rowNum 
                        + (ulong)sizeof(float) * (ulong)columNum;
                    return *(float*)pbit;
                }

            }
        }

        internal Matrix4x4(
            ref M128 vec0,
            ref M128 vec1,
            ref M128 vec2,
            ref M128 vec3
            )
        {
            mat.colum0 = vec0;
            mat.colum1 = vec1;
            mat.colum2 = vec2;
            mat.colum3 = vec3;
        }

        internal Matrix4x4(Mat4 mat)
        {
            this.mat = mat;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static unsafe Matrix4x4 operator +(Matrix4x4 mat1, Matrix4x4 Mat2)
        {
            Mat4* refret = internalIO.add_mat4(ref mat1.mat, ref Mat2.mat);
            Matrix4x4 ret = new Matrix4x4(*refret);
            internalIO.deinit_mat4(refret);
            return ret;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static unsafe Matrix4x4 operator -(Matrix4x4 mat1, Matrix4x4 Mat2)
        {
            Mat4* refret = internalIO.sub_mat4(ref mat1.mat, ref Mat2.mat);
            Matrix4x4 ret = new Matrix4x4(*refret);
            internalIO.deinit_mat4(refret);
            return ret;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static unsafe Matrix4x4 operator *(Matrix4x4 mat, float value)
        {
            Mat4* refret = internalIO.mul_const(ref mat.mat, value);
            Matrix4x4 ret = new Matrix4x4(*refret);
            internalIO.deinit_mat4(refret);
            return ret;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static unsafe Matrix4x2 operator *(Matrix4x4 left, Matrix4x2 right)
        {
            left.InternalTranspose();
            Mat4 tr = left.transpose;
            return new Matrix4x2(
                internalIO.mulps_sum_internal(&tr.colum0, &right.mat.colum0),
                internalIO.mulps_sum_internal(&tr.colum0, &right.mat.colum1),
                internalIO.mulps_sum_internal(&tr.colum1, &right.mat.colum0),
                internalIO.mulps_sum_internal(&tr.colum1, &right.mat.colum1),
                internalIO.mulps_sum_internal(&tr.colum2, &right.mat.colum0),
                internalIO.mulps_sum_internal(&tr.colum2, &right.mat.colum1),
                internalIO.mulps_sum_internal(&tr.colum3, &right.mat.colum0),
                internalIO.mulps_sum_internal(&tr.colum3, &right.mat.colum1)
                );

        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static unsafe Matrix4x4 operator *(Matrix4x4 left, Matrix4x4 right)
        {
            left.InternalTranspose();
            Mat4* p = internalIO.mul_4_4(&left.transpose, &right.mat);
            Matrix4x4 ret = new Matrix4x4(*p);
            internalIO.deinit_mat4(p);
            return ret;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public unsafe float Det()
        {
            InternalTranspose();
            fixed (Matrix4x4* _this = &this)
            {
                return internalIO.det_mat4(_this);
            }
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        internal unsafe void InternalTranspose()
        {
            if (!isTransposed)
            {
                fixed (Mat4* source = &this.mat, target = &this.transpose)
                {
                    internalIO.Transpose4to4_internal(source, target);
                }

                isTransposed = true;
            }
        }

        public override string ToString()
        {
            InternalTranspose();
            return
                $"{transpose.colum0}\n" +
                $"{transpose.colum1}\n" +
                $"{transpose.colum2}\n" +
                $"{transpose.colum3}"; 

        }

        public bool Equals(Matrix4x4 other)
        {
            throw new NotImplementedException();
        }
    }



}