Matrix4x4 normalization

MatrixNormalize.cs

public static void NormalizeMatrix(ref Matrix4x4 xform)
        {
            var vx = new Vector3(xform.M11, xform.M12, xform.M13);
            var vy = new Vector3(xform.M21, xform.M22, xform.M23);
            var vz = new Vector3(xform.M31, xform.M32, xform.M33);

            var lx = vx.Length();
            var ly = vy.Length();
            var lz = vz.Length();

            // normalize axis vectors
            vx /= lx;
            vy /= ly;
            vz /= lz;

            // determine the skew of each axis (the smaller, the more orthogonal the axis is)
            var kxy = Math.Abs(Vector3.Dot(vx, vy));
            var kxz = Math.Abs(Vector3.Dot(vx, vz));
            var kyz = Math.Abs(Vector3.Dot(vy, vz));

            var kx = kxy + kxz;
            var ky = kxy + kyz;
            var kz = kxz + kyz;

            // we will use the axis with less skew as our fixed pivot.

            // axis X as pivot
            if (kx < ky && kx < kz)
            {
                if (ky < kz)
                {
                    vz = Vector3.Cross(vx, vy);
                    vy = Vector3.Cross(vz, vx);
                }
                else
                {
                    vy = Vector3.Cross(vz, vx);
                    vz = Vector3.Cross(vx, vy);
                }
            }

            // axis Y as pivot
            else if (ky < kx && ky < kz)
            {
                if (kx < kz)
                {
                    vz = Vector3.Cross(vx, vy);
                    vx = Vector3.Cross(vy, vz);
                }
                else
                {
                    vx = Vector3.Cross(vy, vz);
                    vz = Vector3.Cross(vx, vy);
                }
            }

            // axis z as pivot
            else
            {
                if (kx < ky)
                {
                    vy = Vector3.Cross(vz, vx);
                    vx = Vector3.Cross(vy, vz);
                }
                else
                {
                    vx = Vector3.Cross(vy, vz);
                    vy = Vector3.Cross(vz, vx);
                }
            }

            // restore axes original lengths
            vx *= lx;
            vy *= ly;
            vz *= lz;

            xform.M11 = vx.X;
            xform.M12 = vx.Y;
            xform.M13 = vx.Z;

            xform.M21 = vy.X;
            xform.M22 = vy.Y;
            xform.M23 = vy.Z;

            xform.M31 = vz.X;
            xform.M32 = vz.Y;
            xform.M33 = vz.Z;
        }

        [Test]
        public void TestMatrixNormalization()
        {
            void testSkewed(Func<Matrix4x4,Matrix4x4> mf)
            {
                var m = Matrix4x4.Identity;

                var o = m = mf(m);
                
                AffineTransform.NormalizeMatrix(ref m);

                NumericsAssert.AreEqual(o, m, 0.34f);                

                Assert.IsTrue(Matrix4x4.Decompose(m, out _, out _, out _));
                Assert.IsTrue(Matrix4x4.Invert(m, out _));               
            }
            
            testSkewed(m => { m.M12 += 0.34f; return m; });
            testSkewed(m => { m.M13 += 0.34f; return m; });
            testSkewed(m => { m.M21 += 0.34f; return m; });
            testSkewed(m => { m.M23 += 0.34f; return m; });
            testSkewed(m => { m.M31 += 0.34f; return m; });
            testSkewed(m => { m.M32 += 0.34f; return m; });

            testSkewed(m => { m.M12 += 0.1f; m.M23 -= 0.1f; m.M31 += 0.05f; return m; });

            // test normalization with uneven scaling

            var SxR = Matrix4x4.CreateScale(5, 1, 1) * Matrix4x4.CreateFromYawPitchRoll(1, 2, 3);   // Decomposable
            var RxS = Matrix4x4.CreateFromYawPitchRoll(1, 2, 3) * Matrix4x4.CreateScale(5, 1, 1);   // Not Decomposable            

            Assert.IsTrue(Matrix4x4.Decompose(SxR, out _, out _, out _));
            Assert.IsFalse(Matrix4x4.Decompose(RxS, out _, out _, out _));
            
            testSkewed(m => SxR);
        }

Updated the normalization code to use Rows instead of Columns.

Adding a unit test example (for NUnit)