korenkonder_PD_Tool/KKdBaseLib/Mat3.cs

using System.Runtime.InteropServices;

namespace KKdBaseLib
{
    [StructLayout(LayoutKind.Explicit)]
    public struct Mat3
    {
        [FieldOffset(0x00)] public Vec3 Row0;
        [FieldOffset(0x0C)] public Vec3 Row1;
        [FieldOffset(0x18)] public Vec3 Row2;

        public Vec3 Column0 { get => new Vec3(Row0.X, Row1.X, Row2.X);
            set { Row0.X = value.X; Row1.X = value.Y; Row2.X = value.Z; } }
        public Vec3 Column1 { get => new Vec3(Row0.Y, Row1.Y, Row2.Y);
            set { Row0.Y = value.X; Row1.Y = value.Y; Row2.Y = value.Z; } }
        public Vec3 Column2 { get => new Vec3(Row0.Z, Row1.Z, Row2.Z);
            set { Row0.Z = value.X; Row1.Z = value.Y; Row2.Z = value.Z;} }

        public static Mat3 Identity => new Mat3(new Vec3(1.0f, 0.0f, 0.0f),
                                                new Vec3(0.0f, 1.0f, 0.0f),
                                                new Vec3(0.0f, 0.0f, 1.0f));

        public Mat3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22)
        { Row0 = new Vec3(m00, m01, m02); Row1 = new Vec3(m10, m11, m12); Row2 = new Vec3(m20, m21, m22);}

        public Mat3(Vec3 row0, Vec3 row1, Vec3 row2)
        { Row0 = row0; Row1 = row1; Row2 = row2; }

        public Mat3(Mat4 mat)
        { Row0 = new Vec3(mat.Row0); Row1 = new Vec3(mat.Row1); Row2 = new Vec3(mat.Row2); }

        public Mat3(Quat q)
        {
            float qx = q.X;
            float qy = q.Y;
            float qz = q.Z;
            float qw = q.W;
            float g = qx + qx;
            float h = qy + qy;
            float k = qz + qz;
            float a = qx * g;
            float l = qx * h;
            float m = qy * h;
            qx *= k;
            qy *= k;
            qz *= k;

            Row0 = new Vec3(1.0f - qz - m,    l - qw * k,   qx + qw * h);
            Row1 = new Vec3(   l + qw * k, 1.0f - qz - a,   qy - qw * g);
            Row2 = new Vec3(  qx - qw * h,   qy + qw * g, 1.0f -  m - a);
        }

        public Quat ToQuat()
        {
            Vec3 row0 = Row0;
            Vec3 row1 = Row1;
            Vec3 row2 = Row2;

            Quat q = default;
            var trace = 0.25 * (row0.X + row1.Y + row2.Z+ 1.0);

            if (trace > 0)
            {
                double sq = System.Math.Sqrt(trace);

                q.W = (float)sq;
                sq = 1.0 / (4.0 * sq);
                q.X = (float)((row1.Z - row2.Y) * sq);
                q.Y = (float)((row2.X - row0.Z) * sq);
                q.Z = (float)((row0.Y - row1.X) * sq);
            }
            else if (row0.X > row1.Y && row0.X > row2.Z)
            {
                double sq = 2.0 * System.Math.Sqrt(1.0 + row0.X - row1.Y - row2.Z);

                q.X = (float)(0.25 * sq);
                sq = 1.0 / sq;
                q.W = (float)((row2.Y - row1.Z) * sq);
                q.Y = (float)((row1.X + row0.Y) * sq);
                q.Z = (float)((row2.X + row0.Z) * sq);
            }
            else if (row1.Y > row2.Z)
            {
                double sq = 2.0 * System.Math.Sqrt(1.0 + row1.Y - row0.X - row2.Z);

                q.Y = (float)(0.25 * sq);
                sq = 1.0 / sq;
                q.W = (float)((row2.X - row0.Z) * sq);
                q.X = (float)((row1.X + row0.Y) * sq);
                q.Z = (float)((row2.Y + row1.Z) * sq);
            }
            else
            {
                double sq = 2.0 * System.Math.Sqrt(1.0 + row2.Z - row0.X - row1.Y);

                q.Z = (float)(0.25 * sq);
                sq = 1.0 / sq;
                q.W = (float)((row1.X - row0.Y) * sq);
                q.X = (float)((row2.X + row0.Z) * sq);
                q.Y = (float)((row2.Y + row1.Z) * sq);
            }

            return q.Normalized;
        }

        public Mat3 Invert() => -this;

        public static Mat3 operator +(Mat3 left, Mat3 right)
        { left.Row0 += right.Row0; left.Row1 += right.Row1; left.Row2 += right.Row2; return left; }

        public static Mat3 operator -(Mat3 left, Mat3 right)
        { left.Row0 -= right.Row0; left.Row1 -= right.Row1; left.Row2 -= right.Row2; return left; }

        public static Mat3 operator -(Mat3 mat)
        {
            int[]   colIdx = {  0,  0,  0 };
            int[]   rowIdx = {  0,  0,  0 };
            int[] pivotIdx = { -1, -1, -1 };

            Mat3 result = mat;
            float[,] inverse =
            {
                { mat.Row0.X, mat.Row0.Y, mat.Row0.Z },
                { mat.Row1.X, mat.Row1.Y, mat.Row1.Z },
                { mat.Row2.X, mat.Row2.Y, mat.Row2.Z }
            };
            int icol = 0;
            int irow = 0;
            for (int i = 0; i < 3; i++)
            {
                float maxPivot = 0.0f;
                for (int j = 0; j < 3; j++)
                {
                    if (pivotIdx[j] == 0) continue;

                    for (int k = 0; k < 3; k++)
                    {
                        if (pivotIdx[k] == -1)
                        {
                            float absVal = System.Math.Abs(inverse[j, k]);
                            if (absVal > maxPivot)
                            {
                                maxPivot = absVal;
                                irow = j;
                                icol = k;
                            }
                        }
                        else if (pivotIdx[k] > 0)
                            return result;
                    }
                }

                pivotIdx[icol]++;

                if (irow != icol)
                    for (int k = 0; k < 3; k++)
                    { float t = inverse[irow, k]; inverse[irow, k] = inverse[icol, k]; inverse[icol, k] = t; }

                rowIdx[i] = irow;
                colIdx[i] = icol;

                float pivot = inverse[icol, icol];

                float oneOverPivot = 1.0f / pivot;
                inverse[icol, icol] = 1.0f;
                for (int k = 0; k < 3; k++)
                    inverse[icol, k] *= oneOverPivot;

                for (int j = 0; j < 3; j++)
                {
                    if (icol == j) continue;

                    float f = inverse[j, icol];
                    inverse[j, icol] = 0.0f;
                    for (int k = 0; k < 3; k++)
                        inverse[j, k] -= inverse[icol, k] * f;
                }
            }

            for (int j = 2; j >= 0; j--)
            {
                int ir = rowIdx[j];
                int ic = colIdx[j];
                for (int k = 0; k < 3; k++)
                { float t = inverse[k, ic]; inverse[k, ic] = inverse[k, ir]; inverse[k, ir] = t; }
            }

            result.Row0.X = inverse[0, 0]; result.Row0.Y = inverse[0, 1]; result.Row0.Z = inverse[0, 2];
            result.Row1.X = inverse[1, 0]; result.Row1.Y = inverse[1, 1]; result.Row1.Z = inverse[1, 2];
            result.Row2.X = inverse[2, 0]; result.Row2.Y = inverse[2, 1]; result.Row2.Z = inverse[2, 2];
            return result;
        }

        public static Mat3 operator *(Mat3 left, Mat3 right)
        {
            Mat3 result = default;
            result.Row0 = left.Row0.X * right.Row0 + left.Row0.Y * right.Row1 + left.Row0.Z * right.Row2;
            result.Row1 = left.Row1.X * right.Row0 + left.Row1.Y * right.Row1 + left.Row1.Z * right.Row2;
            result.Row2 = left.Row2.X * right.Row0 + left.Row2.Y * right.Row1 + left.Row2.Z * right.Row2;
            return result;
        }

        public static Mat3 operator *(Mat3 mat, float scale) =>
            new Mat3(mat.Row0 * scale, mat.Row1 * scale, mat.Row2 * scale);

        public static bool operator ==(Mat3 A, Mat3 B) =>  A.Equals(B);
        public static bool operator !=(Mat3 A, Mat3 B) => !A.Equals(B);

        public bool Equals(Mat3 other) =>
            Row0 == other.Row0 && Row1 == other.Row1 && Row2 == other.Row2;

        public override bool Equals(object obj) => base.Equals(obj);
        public override int GetHashCode() => base.GetHashCode();
        public override string ToString() => $"({Row0}; {Row1}; {Row2})";
        public string ToString(int d) => $"({Row0.ToString(d)}; {Row1.ToString(d)}; {Row2.ToString(d)})";
    }
}