Improving transforms API

Fixed issue when inverting a matrix which results in M44 not being exactly 1 due to precission loss.

examples/SharpGLTF.Plotly/PlotlyToolkit.cs

@@ -90,8 +90,10 @@ namespace SharpGLTF
             var baseColor = material.GetChannel(KnownChannel.BaseColor);
             if (baseColor != null) color = (Vector4)baseColor.Parameters[KnownProperty.RGBA];
 
+            #pragma warning disable CS0618 // Type or member is obsolete
             var diffuseColor = material.GetChannel(KnownChannel.Diffuse);
             if (diffuseColor != null) color = (Vector4)diffuseColor.Parameters[KnownProperty.RGBA];
+            #pragma warning restore CS0618 // Type or member is obsolete
 
             color *= 0.8f;
             color *= 255;

src/Shared/_Extensions.cs

@@ -179,6 +179,8 @@ namespace SharpGLTF
         {
             if (!Matrix4x4.Invert(src, out Matrix4x4 dst)) Guard.IsTrue(false, nameof(src), "Matrix cannot be inverted.");
 
+            if (src.M44 == 1) dst.M44 = 1; // fix precission loss;
+
             return dst;
         }        
 

src/SharpGLTF.Core/Transforms/AffineTransform.cs

@@ -92,7 +92,7 @@ namespace SharpGLTF.Transforms
         {
             return new AffineTransform(matrix);
         }
-
+        
         public static AffineTransform CreateDecomposed(Matrix4x4 matrix)
         {
             if (!Matrix4x4.Decompose(matrix, out var s, out var r, out var t)) throw new ArgumentException("Can't decompose", nameof(matrix));
@@ -299,29 +299,54 @@ namespace SharpGLTF.Transforms
         /// <inheritdoc/>
         public bool Equals(AffineTransform other)
         {
-            if (this.IsSRT && other.IsSRT)
+            if (this._Representation != other._Representation) return false;
+
+            if (this._Translation != other._Translation) return false;
+            if (this._M11 != other._M11) return false;
+            if (this._M12 != other._M12) return false;
+            if (this._M13 != other._M13) return false;
+
+            if (this._M21 != other._M21) return false;
+            if (this._M22 != other._M22) return false;
+            if (this._M23 != other._M23) return false;
+            if (this._M31 != other._M31) return false;
+
+            if (this.IsMatrix && other.IsMatrix)
             {
-                if (this._Translation != other._Translation) return false;
-                if (this._M11 != other._M11) return false;
-                if (this._M12 != other._M12) return false;
-                if (this._M13 != other._M13) return false;
-
-                if (this._M21 != other._M21) return false;
-                if (this._M22 != other._M22) return false;
-                if (this._M23 != other._M23) return false;
-                if (this._M31 != other._M31) return false;
-
-                System.Diagnostics.Debug.Assert(this._M32 == other._M32);
-                System.Diagnostics.Debug.Assert(this._M33 == other._M33);
+                if (this._M32 != other._M32) return false;
+                if (this._M33 != other._M33) return false;
             }
 
-            return this.Matrix.Equals(other.Matrix);
+            return true;
         }
 
         public static bool operator ==(in AffineTransform a, in AffineTransform b) { return a.Equals(b); }
 
         public static bool operator !=(in AffineTransform a, in AffineTransform b) { return !a.Equals(b); }
 
+        /// <summary>
+        /// Checks whether two transform represent the same geometric spatial transformation.
+        /// </summary>
+        /// <param name="a">the first transform to check.</param>
+        /// <param name="b">the second transform to check.</param>
+        /// <param name="tolerance">the tolerance to handle floating point error.</param>
+        /// <returns>true if both transforms can be considered geometryically equivalent.</returns>
+        public static bool AreGeometricallyEquivalent(in AffineTransform a, in AffineTransform b, float tolerance = 0.00001f)
+        {
+            var ax = Transform(Vector3.UnitX, a);
+            var ay = Transform(Vector3.UnitY, a);
+            var az = Transform(Vector3.UnitZ, a);
+
+            var bx = Transform(Vector3.UnitX, b);
+            var by = Transform(Vector3.UnitY, b);
+            var bz = Transform(Vector3.UnitZ, b);
+
+            if (Vector3.Distance(ax, bx) > tolerance) return false;
+            if (Vector3.Distance(ay, by) > tolerance) return false;
+            if (Vector3.Distance(az, bz) > tolerance) return false;
+            return true;
+        }
+
         #endregion
 
         #region properties
@@ -609,7 +634,110 @@ namespace SharpGLTF.Transforms
 
             return new AffineTransform(s, r, t);
         }
-        
+
+        /// <summary>
+        /// Inverts the specified transform. The return value indicates whether the operation succeeded.
+        /// </summary>
+        /// <remarks>
+        /// SRT format with uneven scale can produce results that differ from a matrix
+        /// </remarks>
+        /// <param name="xform">The transform to invert.</param>
+        /// <param name="inverse">The inverted result.</param>
+        /// <returns>True if the operation succeeds.</returns>
+        public static bool TryInvert(in AffineTransform xform, out AffineTransform inverse)
+        {
+            if (xform.IsMatrix)
+            {
+                if (Matrix4x4.Invert(xform.Matrix, out var result))
+                {
+                    result.M44 = 1; // fix Matrix4x4.Invert precission loss.
+                    inverse = result;
+                    return true;
+                }
+                else
+                {
+                    inverse = default;
+                    return false;
+                }
+            }
+
+            if (xform.IsSRT)
+            {
+                if (xform.Rotation.IsIdentity) // SRTs with no rotation can be safely inverted
+                {
+                    var si = Vector3.One / xform.Scale;
+                    var ti = -si * xform.Translation;
+                    inverse = new AffineTransform(si, Quaternion.Identity, ti);
+                }
+                else
+                {
+                    // SRTs with uneven scaling cannot be inverted, so we need to handle them as matrices
+                    if (xform.Scale.X != xform.Scale.Y || xform.Scale.Y != xform.Scale.Z)
+                    {
+                        return TryInvert(xform.Matrix, out inverse);
+                    }
+
+                    var si = 1f / xform.Scale.X;
+                    var ri = Quaternion.Normalize(Quaternion.Conjugate(xform.Rotation));
+                    var ti = -si * _Vector3Transform(xform.Translation, ri);
+
+                    inverse = new AffineTransform(new Vector3(si), ri, ti);
+                }
+
+                return true;
+            }
+
+            inverse = default;
+            return false;
+        }
+
+        /// <summary>
+        /// Transforms a vector by a specified transform.
+        /// </summary>
+        /// <param name="vector">The vector to transform.</param>
+        /// <param name="xform">The transform to apply.</param>
+        /// <returns>The transformed vector.</returns>        
+        private static Vector3 Transform(Vector3 vector, in AffineTransform xform)
+        {
+            if (xform.IsMatrix)
+            {
+                return Vector3.Transform(vector, xform.Matrix);
+            }
+
+            if (xform.IsSRT)
+            {
+                vector *= xform.Scale;
+                vector = _Vector3Transform(vector, xform.Rotation);
+                vector += xform.Translation;
+                return vector;
+            }
+
+            throw new ArgumentException("Undefined transform", nameof(xform));
+        }
+
+        /// <summary>
+        /// Transforms a vector normal by a specified transform.
+        /// </summary>
+        /// <param name="vector">The vector to transform.</param>
+        /// <param name="xform">The transform to apply.</param>
+        /// <returns>The transformed vector.</returns>        
+        public static Vector3 TransformNormal(Vector3 vector, in AffineTransform xform)
+        {
+            if (xform.IsMatrix)
+            {
+                return Vector3.TransformNormal(vector, xform.Matrix);
+            }
+
+            if (xform.IsSRT)
+            {
+                vector *= xform.Scale;
+                vector = _Vector3Transform(vector, xform.Rotation);
+                return vector;
+            }
+
+            throw new ArgumentException("Undefined transform", nameof(xform));
+        }
+
         #endregion
 
         #region internals
@@ -660,12 +788,15 @@ namespace SharpGLTF.Transforms
 
         /// <summary>
         /// This method is equivalent to System.Numerics.Vector3.Transform(Vector3 v, Quaternion q)
-        /// </summary>
+        /// </summary>        
         /// <param name="v">The vector to transform</param>
         /// <param name="q">The transform rotation</param>
-        /// <returns>The rotated vector</returns>
+        /// <returns>The transformed vector</returns>
+        [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
         private static Vector3 _Vector3Transform(Vector3 v, Quaternion q)
         {
+            // https://github.com/dotnet/runtime/blob/985eedd68df0b4fb3f541fe266c95fa0a1bc4a0a/src/libraries/System.Private.CoreLib/src/System/Numerics/Vector3.cs#L543
+
             // Extract the vector part of the quaternion
             var u = new Vector3(q.X, q.Y, q.Z);
 

src/SharpGLTF.Core/Transforms/Matrix4x4Factory.cs

@@ -119,6 +119,28 @@ namespace SharpGLTF.Transforms
 
         #region API
 
+        public static Matrix4x4 CreateFromRows(Vector3 rowX, Vector3 rowY, Vector3 rowZ)
+        {
+            return new Matrix4x4
+                (
+                rowX.X, rowX.Y, rowX.Z, 0,
+                rowY.X, rowY.Y, rowY.Z, 0,
+                rowZ.X, rowZ.Y, rowZ.Z, 0,
+                0, 0, 0, 1
+                );
+        }
+
+        public static Matrix4x4 CreateFromRows(Vector3 rowX, Vector3 rowY, Vector3 rowZ, Vector3 translation)
+        {
+            return new Matrix4x4
+                (
+                rowX.X, rowX.Y, rowX.Z, 0,
+                rowY.X, rowY.Y, rowY.Z, 0,
+                rowZ.X, rowZ.Y, rowZ.Z, 0,
+                translation.X, translation.Y, translation.Z, 1
+                );
+        }
+
         /// <summary>
         /// Evaluates a <see cref="Matrix4x4"/> transform based on the available parameters.
         /// </summary>
@@ -149,12 +171,9 @@ namespace SharpGLTF.Transforms
         public static Matrix4x4 WorldToLocal(in Matrix4x4 parentWorld, in Matrix4x4 childWorld)
         {
             GuardMatrix(nameof(parentWorld), parentWorld, MatrixCheck.WorldTransform);
-            GuardMatrix(nameof(childWorld), childWorld, MatrixCheck.WorldTransform);
-
-            var parentInverse = parentWorld.Inverse();
-            parentInverse.M44 = 1f;
+            GuardMatrix(nameof(childWorld), childWorld, MatrixCheck.WorldTransform);            
 
-            return childWorld * parentInverse;
+            return childWorld * parentWorld.Inverse();
         }
 
         /// <summary>

src/SharpGLTF.Core/Transforms/MeshTransforms.cs

@@ -471,6 +471,8 @@ namespace SharpGLTF.Transforms
         {
             if (!Matrix4x4Double.Invert(jointWorldTransform, out Matrix4x4Double invJoint)) Guard.IsTrue(false, nameof(jointWorldTransform), "Matrix cannot be inverted.");
 
+            if (jointWorldTransform.M44 == 1) invJoint.M44 = 1; // fix precission loss;
+
             if (meshWorldTransform == Matrix4x4Double.Identity) return invJoint;
 
             return meshWorldTransform * invJoint;

src/SharpGLTF.Toolkit/Animations/CurveBuilder.cs

@@ -369,6 +369,8 @@ namespace SharpGLTF.Animations
 
         #region IConvertibleCurve API
 
+        #pragma warning disable CA1033 // Interface methods should be callable by child types
+
         IReadOnlyDictionary<float, T> IConvertibleCurve<T>.ToStepCurve()
         {
             if (MaxDegree != 0) throw new NotSupportedException();
@@ -497,6 +499,8 @@ namespace SharpGLTF.Animations
             return d;
         }
 
+        #pragma warning restore CA1033 // Interface methods should be callable by child types
+
         #endregion
     }
 

src/SharpGLTF.Toolkit/Materials/MaterialEnums.cs

@@ -86,11 +86,16 @@ namespace SharpGLTF.Materials
     {
         private IReadOnlyList<KnownChannel> _GetValidChannels()
         {
+
             switch (ShaderStyle)
             {
                 case SHADERUNLIT: return _UnlitChannels;
                 case SHADERPBRMETALLICROUGHNESS: return _MetRouChannels;
+
+                #pragma warning disable CS0618 // Type or member is obsolete
                 case SHADERPBRSPECULARGLOSSINESS: return _SpeGloChannels;
+                #pragma warning restore CS0618 // Type or member is obsolete
+
                 default: throw new NotImplementedException();
             }
         }
@@ -121,12 +126,13 @@ namespace SharpGLTF.Materials
             KnownChannel.VolumeAttenuation
         };
 
+        [Obsolete("Deprecated by Khronos")]
         private static readonly KnownChannel[] _SpeGloChannels = new[]
         {
             KnownChannel.Normal,
             KnownChannel.Occlusion,
             KnownChannel.Emissive,
-
+            
             KnownChannel.Diffuse,
             KnownChannel.SpecularGlossiness,
         };

src/SharpGLTF.Toolkit/Schema2/SceneExtensions.cs

@@ -83,10 +83,10 @@ namespace SharpGLTF.Schema2
             Guard.NotNull(node, nameof(node));
             Guard.NotNull(joints, nameof(joints));
 
-            foreach (var j in joints)
+            foreach (var (j, ibm) in joints)
             {
-                Guard.MustShareLogicalParent(node, j.Joint, nameof(joints));
-                Guard.IsTrue(Matrix4x4.Invert(j.InverseBindMatrix, out Matrix4x4 r), nameof(joints), "Invalid Matrix");
+                Guard.MustShareLogicalParent(node, j, nameof(joints));
+                Guard.IsTrue(Matrix4x4.Invert(ibm, out _), nameof(joints), "Invalid Matrix");
             }
 
             var skin = node.LogicalParent.CreateSkin();
@@ -119,10 +119,10 @@ namespace SharpGLTF.Schema2
             Guard.NotNull(joints, nameof(joints));
             Guard.MustShareLogicalParent(node, mesh, nameof(mesh));
 
-            foreach (var j in joints)
+            foreach (var (j, ibm) in joints)
             {
-                Guard.MustShareLogicalParent(node, j.Joint, nameof(joints));
-                Guard.IsTrue(Matrix4x4.Invert(j.InverseBindMatrix, out Matrix4x4 r), nameof(joints), "Invalid Matrix");
+                Guard.MustShareLogicalParent(node, j, nameof(joints));
+                Guard.IsTrue(Matrix4x4.Invert(ibm, out _), nameof(joints), "Invalid Matrix");
             }
 
             // TODO: the joints must be visible in the visual tree that contains node.

tests/SharpGLTF.Core.Tests/Transforms/AffineTransformMatrixTests.cs

@@ -1,129 +1,156 @@
-using System;
-using System.Collections.Generic;
-using System.Numerics;
-using System.Text;
-
-using NUnit.Framework;
-
-namespace SharpGLTF.Transforms
-{
-    [Category("Core.Transforms")]
-    public class AffineTransformMatrixTests
-    {
-        [TestCase(0, 0, 0, 0, 0, 0)]
-        [TestCase(1, 2, 4, 3, 2, 1)]
-        [TestCase(-1, 1, 3, 2, 0, 1)]
-        [TestCase(0, 0, 1, 0, 1, 0)]
-        [TestCase(0, -1, 1, -2, 1, 0)]
-        public void CalculateInverseBindMatrix(float mx, float my, float mz, float jx, float jy, float jz)
-        {
-            var model = Matrix4x4.CreateFromYawPitchRoll(mx, my, mz);            
-            var joint = Matrix4x4.CreateFromYawPitchRoll(jx, jy, jz);
-            joint.Translation = new Vector3(jx, jy, jz);
-
-            var invBindMatrix = SkinnedTransform.CalculateInverseBinding(model, joint);
-
-            Matrix4x4.Invert(model, out Matrix4x4 xform);            
-            Matrix4x4.Invert(joint * xform, out Matrix4x4 result);
-            NumericsAssert.AreEqual(result, invBindMatrix, 0.000001f);
-
-            Matrix4x4.Invert(joint, out Matrix4x4 invJoint);
-            result = model * invJoint;
-            NumericsAssert.AreEqual(result, invBindMatrix, 0.000001f);
-        }
-
-
-        [Test]
-        public void TestMatrixNormalization()
-        {
-            void testMatrix(Matrix4x4 m, float tolerance = 0)
-            {
-                var o = m;
-
-                Matrix4x4Factory.NormalizeMatrix(ref m);
-
-                NumericsAssert.AreEqual(o, m, tolerance);
-
-                Assert.IsTrue(Matrix4x4.Decompose(m, out _, out _, out _));
-                Assert.IsTrue(Matrix4x4.Invert(m, out _));
-            }
-
-            void testSkewed(Func<Matrix4x4, Matrix4x4> mf, float tolerance = 0)
-            {
-                var m = Matrix4x4.Identity;
-
-                var o = m = mf(m);
-
-                Assert.IsFalse(Matrix4x4.Decompose(m, out _, out _, out _));
-
-                Matrix4x4Factory.NormalizeMatrix(ref m);
-
-                NumericsAssert.AreEqual(o, m, tolerance);                
-
-                Assert.IsTrue(Matrix4x4.Decompose(m, out _, out _, out _));
-                Assert.IsTrue(Matrix4x4.Invert(m, out _));               
-            }
-            
-            testSkewed(m => { m.M12 += 0.34f; return m; }, 0.34f);
-            testSkewed(m => { m.M13 += 0.34f; return m; }, 0.34f);
-            testSkewed(m => { m.M21 += 0.34f; return m; }, 0.34f);
-            testSkewed(m => { m.M23 += 0.34f; return m; }, 0.34f);
-            testSkewed(m => { m.M31 += 0.34f; return m; }, 0.34f);
-            testSkewed(m => { m.M32 += 0.34f; return m; }, 0.34f);
-
-            testSkewed(m => { m.M12 += 0.1f; m.M23 -= 0.1f; m.M31 += 0.05f; return m; }, 0.20f);
-
-            // test normalization with uneven scaling
-
-            testMatrix(Matrix4x4.CreateScale(0.0001f) * Matrix4x4.CreateFromYawPitchRoll(1, 2, 3), 0.0001f);
-            testMatrix(Matrix4x4.CreateScale(1000) * Matrix4x4.CreateFromYawPitchRoll(1, 2, 3), 0.0002f);
-
-            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 _));
-            testMatrix(SxR, 0.0001f);
-
-            Assert.IsFalse(Matrix4x4.Decompose(RxS, out _, out _, out _));
-            testMatrix(RxS, 100);           
-        }
-
-        [Test]
-        public void TestAffineTransformIdentity()
-        {
-            var asMatrix = new AffineTransform(Matrix4x4.Identity);
-            var asDecomposed = new AffineTransform(null, null, null);
-
-            NumericsAssert.AreEqual(Matrix4x4.Identity, asMatrix.Matrix);
-            NumericsAssert.AreEqual(Matrix4x4.Identity, asDecomposed.Matrix);
-
-            Assert.IsTrue(asMatrix.IsIdentity);
-            Assert.IsTrue(asDecomposed.IsIdentity);
-        }
-
-        [Test]
-        public void TestAffineTransformMult()
-        {
-            var s_a = new Vector3(1, 2, 4);
-            var r_a = Quaternion.CreateFromYawPitchRoll(1, 2, 3);
-            var t_a = Vector3.Zero;
-
-            var s_b = new Vector3(1, 1, 1);
-            var r_b = Quaternion.CreateFromYawPitchRoll(1, 0, 2);
-            var t_b = new Vector3(3, -4, 2);
-
-            var mat_a = Matrix4x4.CreateScale(s_a) * Matrix4x4.CreateFromQuaternion(r_a) * Matrix4x4.CreateTranslation(t_a);
-            var mat_b = Matrix4x4.CreateScale(s_b) * Matrix4x4.CreateFromQuaternion(r_b) * Matrix4x4.CreateTranslation(t_b);
-            var mat_ab = Matrix4x4.Multiply(mat_a, mat_b);
-            var mat_ba = Matrix4x4.Multiply(mat_b, mat_a);
-
-            var srt_a = new AffineTransform(s_a, r_a, t_a);
-            var srt_b = new AffineTransform(s_b, r_b, t_b);
-            var srt_ab = AffineTransform.Multiply(srt_a, srt_b);
-            var srt_ba = AffineTransform.Multiply(srt_b, srt_a);
-
-            NumericsAssert.AreEqual(mat_ab, srt_ab.Matrix, 0.0001f);
-            NumericsAssert.AreEqual(mat_ba, srt_ba.Matrix, 0.0001f);
-        }
-    }    
-}
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using System.Text;
+using System.Xml;
+
+using NUnit.Framework;
+
+namespace SharpGLTF.Transforms
+{
+    [Category("Core.Transforms")]
+    public class AffineTransformMatrixTests
+    {
+        [TestCase(0, 0, 0, 0, 0, 0)]
+        [TestCase(1, 2, 4, 3, 2, 1)]
+        [TestCase(-1, 1, 3, 2, 0, 1)]
+        [TestCase(0, 0, 1, 0, 1, 0)]
+        [TestCase(0, -1, 1, -2, 1, 0)]
+        public void CalculateInverseBindMatrix(float mx, float my, float mz, float jx, float jy, float jz)
+        {
+            var model = Matrix4x4.CreateFromYawPitchRoll(mx, my, mz);            
+            var joint = Matrix4x4.CreateFromYawPitchRoll(jx, jy, jz);
+            joint.Translation = new Vector3(jx, jy, jz);
+
+            var invBindMatrix = SkinnedTransform.CalculateInverseBinding(model, joint);
+
+            Matrix4x4.Invert(model, out Matrix4x4 xform);            
+            Matrix4x4.Invert(joint * xform, out Matrix4x4 result);
+            NumericsAssert.AreEqual(result, invBindMatrix, 0.000001f);
+
+            Matrix4x4.Invert(joint, out Matrix4x4 invJoint);
+            result = model * invJoint;
+            NumericsAssert.AreEqual(result, invBindMatrix, 0.000001f);
+        }
+
+
+        [Test]
+        public void TestMatrixNormalization()
+        {
+            void testMatrix(Matrix4x4 m, float tolerance = 0)
+            {
+                var o = m;
+
+                Matrix4x4Factory.NormalizeMatrix(ref m);
+
+                NumericsAssert.AreEqual(o, m, tolerance);
+
+                Assert.IsTrue(Matrix4x4.Decompose(m, out _, out _, out _));
+                Assert.IsTrue(Matrix4x4.Invert(m, out _));
+            }
+
+            void testSkewed(Func<Matrix4x4, Matrix4x4> mf, float tolerance = 0)
+            {
+                var m = Matrix4x4.Identity;
+
+                var o = m = mf(m);
+
+                Assert.IsFalse(Matrix4x4.Decompose(m, out _, out _, out _));
+
+                Matrix4x4Factory.NormalizeMatrix(ref m);
+
+                NumericsAssert.AreEqual(o, m, tolerance);                
+
+                Assert.IsTrue(Matrix4x4.Decompose(m, out _, out _, out _));
+                Assert.IsTrue(Matrix4x4.Invert(m, out _));               
+            }
+            
+            testSkewed(m => { m.M12 += 0.34f; return m; }, 0.34f);
+            testSkewed(m => { m.M13 += 0.34f; return m; }, 0.34f);
+            testSkewed(m => { m.M21 += 0.34f; return m; }, 0.34f);
+            testSkewed(m => { m.M23 += 0.34f; return m; }, 0.34f);
+            testSkewed(m => { m.M31 += 0.34f; return m; }, 0.34f);
+            testSkewed(m => { m.M32 += 0.34f; return m; }, 0.34f);
+
+            testSkewed(m => { m.M12 += 0.1f; m.M23 -= 0.1f; m.M31 += 0.05f; return m; }, 0.20f);
+
+            // test normalization with uneven scaling
+
+            testMatrix(Matrix4x4.CreateScale(0.0001f) * Matrix4x4.CreateFromYawPitchRoll(1, 2, 3), 0.0001f);
+            testMatrix(Matrix4x4.CreateScale(1000) * Matrix4x4.CreateFromYawPitchRoll(1, 2, 3), 0.0002f);
+
+            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 _));
+            testMatrix(SxR, 0.0001f);
+
+            Assert.IsFalse(Matrix4x4.Decompose(RxS, out _, out _, out _));
+            testMatrix(RxS, 100);           
+        }
+
+        [Test]
+        public void TestAffineTransformIdentity()
+        {
+            var asMatrix = new AffineTransform(Matrix4x4.Identity);
+            var asDecomposed = new AffineTransform(null, null, null);
+
+            NumericsAssert.AreEqual(Matrix4x4.Identity, asMatrix.Matrix);
+            NumericsAssert.AreEqual(Matrix4x4.Identity, asDecomposed.Matrix);
+
+            Assert.IsTrue(asMatrix.IsIdentity);
+            Assert.IsTrue(asDecomposed.IsIdentity);
+        }
+
+        [Test]
+        public void TestAffineTransformMult()
+        {
+            var s_a = new Vector3(1, 2, 4);
+            var r_a = Quaternion.CreateFromYawPitchRoll(1, 2, 3);
+            var t_a = Vector3.Zero;
+
+            var s_b = new Vector3(1, 1, 1);
+            var r_b = Quaternion.CreateFromYawPitchRoll(1, 0, 2);
+            var t_b = new Vector3(3, -4, 2);
+
+            var mat_a = Matrix4x4.CreateScale(s_a) * Matrix4x4.CreateFromQuaternion(r_a) * Matrix4x4.CreateTranslation(t_a);
+            var mat_b = Matrix4x4.CreateScale(s_b) * Matrix4x4.CreateFromQuaternion(r_b) * Matrix4x4.CreateTranslation(t_b);
+            var mat_ab = Matrix4x4.Multiply(mat_a, mat_b);
+            var mat_ba = Matrix4x4.Multiply(mat_b, mat_a);
+
+            var srt_a = new AffineTransform(s_a, r_a, t_a);
+            var srt_b = new AffineTransform(s_b, r_b, t_b);
+            var srt_ab = AffineTransform.Multiply(srt_a, srt_b);
+            var srt_ba = AffineTransform.Multiply(srt_b, srt_a);
+
+            NumericsAssert.AreEqual(mat_ab, srt_ab.Matrix, 0.0001f);
+            NumericsAssert.AreEqual(mat_ba, srt_ba.Matrix, 0.0001f);
+        }
+
+
+        [TestCase(true, 1, 10, 100, 0, 0, 0, 5,5,5)]
+        [TestCase(true, 1, 1, 1, 0, 0, 0, 0, 0, 0)]
+        [TestCase(true, 1, 1, 1, 2, 0, -1, 100, 50, 0)]
+        [TestCase(true, 5, 5, 5, 1, 2, 3, 100, 50, 0)]
+        [TestCase(false, 1, 2, 3, 1, 2, 3, 1, 2, 3)]
+        [TestCase(false, 1, 2, 30, 1, 2, 3, 1, 2, 3)]
+        [TestCase(false, -1, -2, 3, 0, 1, 0, 1, 0, 0)]
+        public void TestAffineTransformInverse(bool isInvertibleToSRT, float sx, float sy, float sz, float y, float p, float r, float tx, float ty, float tz)
+        {
+            var xf = new AffineTransform(new Vector3(sx, sy, sz), Quaternion.CreateFromYawPitchRoll(y, p, r), new Vector3(tx, ty, tz));
+
+            Assert.IsTrue(AffineTransform.TryInvert(xf, out var xi));            
+            Assert.IsTrue(Matrix4x4.Invert(xf.Matrix, out var mi));
+            mi.M44 = 1f;
+
+            if (isInvertibleToSRT) Assert.IsTrue(xi.IsSRT);
+
+            var xmi = xi.Matrix;
+
+            var tolerance = NumericsAssert.AreGeometryicallyEquivalent(mi, xmi, 0.00001f);
+            TestContext.WriteLine(tolerance);
+
+            Assert.IsTrue(AffineTransform.AreGeometricallyEquivalent(mi, xi, 0.00001f));
+        }
+    }    
+}

tests/SharpGLTF.NUnit/NumericsAssert.cs

@@ -7,7 +7,7 @@ using NUnit.Framework;
 
 namespace SharpGLTF
 {
-    [System.Diagnostics.DebuggerStepThrough]
+    // [System.Diagnostics.DebuggerStepThrough]
     public static class NumericsAssert
     {
         public static double UnitError(this Vector3 v) { return v.LengthError(1); }
@@ -114,11 +114,17 @@ namespace SharpGLTF
             Assert.AreEqual(expected.Y, actual.Y, tolerance, "Y");
         }
 
-        public static void AreEqual(Vector3 expected, Vector3 actual, double tolerance = 0)
+        public static float AreEqual(Vector3 expected, Vector3 actual, double tolerance = 0)
         {
             Assert.AreEqual(expected.X, actual.X, tolerance, "X");
             Assert.AreEqual(expected.Y, actual.Y, tolerance, "Y");
             Assert.AreEqual(expected.Z, actual.Z, tolerance, "Z");
+
+            // get tolerance
+            var tx = Math.Abs(expected.X - actual.X);
+            var ty = Math.Abs(expected.Y - actual.Y);
+            var tz = Math.Abs(expected.Z - actual.Z);
+            return Math.Max(tx, Math.Max(ty, tz));
         }
 
         public static void AreEqual(Vector4 expected, Vector4 actual, double tolerance = 0)
@@ -160,6 +166,22 @@ namespace SharpGLTF
             Assert.AreEqual(expected.M44, actual.M44, tolerance, "M44");
         }
 
+        public static float AreGeometryicallyEquivalent(Matrix4x4 expected, Matrix4x4 actual, double tolerance = 0)
+        {
+            var expectedX = Vector3.Transform(Vector3.UnitX, expected);
+            var expectedY = Vector3.Transform(Vector3.UnitY, expected);
+            var expectedZ = Vector3.Transform(Vector3.UnitZ, expected);
+
+            var actualX = Vector3.Transform(Vector3.UnitX, actual);
+            var actualY = Vector3.Transform(Vector3.UnitY, actual);
+            var actualZ = Vector3.Transform(Vector3.UnitZ, actual);
+
+            var tx = AreEqual(expectedX, actualX, tolerance);
+            var ty = AreEqual(expectedY, actualY, tolerance);
+            var tz = AreEqual(expectedZ, actualZ, tolerance);
+            return Math.Max(tx, Math.Max(ty, tz));
+        }
+
         public static void IsInvertible(Matrix3x2 matrix)
         {
             IsFinite(matrix);