API Improvements to AffineTransform.cs

Added AffineTransform multiply based on code from: https://gist.github.com/ptasev/bce22ca61d9da83a702c2011229b76ab

src/Shared/_Extensions.cs

@@ -170,7 +170,7 @@ namespace SharpGLTF
             return new Vector4(n, s > 0 ? 1 : -1);
         }
 
-        internal static Matrix4x4 Inverse(this Matrix4x4 src)
+        internal static Matrix4x4 Inverse(this in Matrix4x4 src)
         {
             if (!Matrix4x4.Invert(src, out Matrix4x4 dst)) Guard.IsTrue(false, nameof(src), "Matrix cannot be inverted.");
 

src/SharpGLTF.Core/Runtime/NodeTemplate.cs

@@ -134,7 +134,7 @@ namespace SharpGLTF.Runtime
             var r = _Rotation?.GetValueAt(trackLogicalIndex, time);
             var t = _Translation?.GetValueAt(trackLogicalIndex, time);
 
-            return Transforms.AffineTransform.Create(s, r, t);
+            return new Transforms.AffineTransform(s, r, t);
         }
 
         public Transforms.AffineTransform GetLocalTransform(ReadOnlySpan<int> track, ReadOnlySpan<float> time, ReadOnlySpan<float> weight)

src/SharpGLTF.Core/Schema2/gltf.Node.cs

@@ -192,7 +192,7 @@ namespace SharpGLTF.Schema2
         /// </summary>
         public Transforms.AffineTransform LocalTransform
         {
-            get => new Transforms.AffineTransform(_matrix, _scale, _rotation, _translation);
+            get => Transforms.AffineTransform.CreateFromAny(_matrix, _scale, _rotation, _translation);
             set
             {
                 Guard.IsFalse(this._skin.HasValue, _NOTRANSFORMMESSAGE);

src/SharpGLTF.Core/Transforms/AffineTransform.cs

@@ -6,45 +6,51 @@ using System.Text;
 namespace SharpGLTF.Transforms
 {
     /// <summary>
-    /// Represents an affine transform in 3D space,
-    /// defined by a <see cref="Quaternion"/> rotation,
-    /// a <see cref="Vector3"/> scale
-    /// and a <see cref="Vector3"/> translation.
+    /// Represents an affine transform in 3D space, defined by:
+    /// - A <see cref="Vector3"/> scale.
+    /// - A <see cref="Quaternion"/> rotation.
+    /// - A <see cref="Vector3"/> translation.
     /// </summary>
-    /// <see href="https://github.com/KhronosGroup/glTF-Validator/issues/33"/>
+    /// <remarks>
+    /// <see cref="AffineTransform"/> cannot represent skewed matrices. This means
+    /// that it can be used to represent <see cref="Schema2.Node"/> local transforms,
+    /// but since chained transforms can become skewed, a world transform cannot be
+    /// represented by a <see cref="AffineTransform"/>.
+    /// </remarks>
+    /// <see href="https://github.com/vpenades/SharpGLTF/issues/41"/>
     public struct AffineTransform
     {
         #region lifecycle
 
-        public static AffineTransform Create(Matrix4x4 matrix)
+        public static implicit operator AffineTransform(Matrix4x4 matrix) { return new AffineTransform(matrix); }
+
+        public AffineTransform(Matrix4x4 matrix)
         {
-            return new AffineTransform(matrix, null, null, null);
+            if (!Matrix4x4.Decompose(matrix, out this.Scale, out this.Rotation, out this.Translation))
+            {
+                throw new ArgumentException("matrix is invalid or skewed.", nameof(matrix));
+            }
         }
 
-        public static AffineTransform Create(Vector3? scale, Quaternion? rotation, Vector3? translation)
+        public AffineTransform(Vector3? scale, Quaternion? rotation, Vector3? translation)
         {
-            return new AffineTransform(null, scale, rotation, translation);
+            this.Scale = scale ?? Vector3.One;
+            this.Rotation = rotation ?? Quaternion.Identity;
+            this.Translation = translation ?? Vector3.Zero;
         }
 
-        internal AffineTransform(Matrix4x4? matrix, Vector3? scale, Quaternion? rotation, Vector3? translation)
+        public static AffineTransform CreateFromAny(Matrix4x4? matrix, Vector3? scale, Quaternion? rotation, Vector3? translation)
         {
             if (matrix.HasValue)
             {
-                Matrix4x4.Decompose(matrix.Value, out this.Scale, out this.Rotation, out this.Translation);
+                return new AffineTransform(matrix.Value);
             }
             else
             {
-                this.Scale = scale ?? Vector3.One;
-                this.Rotation = rotation ?? Quaternion.Identity;
-                this.Translation = translation ?? Vector3.Zero;
+                return new AffineTransform(scale, rotation, translation);
             }
         }
 
-        public static implicit operator AffineTransform(Matrix4x4 matrix)
-        {
-            return new AffineTransform(matrix, null, null, null);
-        }
-
         #endregion
 
         #region data
@@ -62,7 +68,7 @@ namespace SharpGLTF.Transforms
         /// <summary>
         /// Translation
         /// </summary>
-        public Vector3 Translation;
+        public  Vector3 Translation;
 
         #endregion
 
@@ -77,12 +83,9 @@ namespace SharpGLTF.Transforms
         {
             get
             {
-                return
-                    Matrix4x4.CreateScale(Scale)
-                    *
-                    Matrix4x4.CreateFromQuaternion(Rotation.Sanitized())
-                    *
-                    Matrix4x4.CreateTranslation(Translation);
+                var m = Matrix4x4.CreateScale(this.Scale) * Matrix4x4.CreateFromQuaternion(this.Rotation.Sanitized());
+                m.Translation = this.Translation;
+                return m;
             }
         }
 
@@ -130,7 +133,47 @@ namespace SharpGLTF.Transforms
 
             r = Quaternion.Normalize(r);
 
-            return AffineTransform.Create(s, r, t);
+            return new  AffineTransform(s, r, t);
+        }
+
+        public static AffineTransform operator *(in AffineTransform a, in AffineTransform b)
+        {
+            return Multiply(a, b);
+        }
+
+        public static AffineTransform Multiply(in AffineTransform a, in AffineTransform b)
+        {
+            AffineTransform r;
+
+            r.Scale = Vector3Transform(b.Scale * Vector3Transform(a.Scale, a.Rotation), Quaternion.Inverse(a.Rotation));
+
+            r.Rotation = Quaternion.Multiply(b.Rotation, a.Rotation);
+
+            r.Translation
+                = b.Translation
+                + Vector3Transform(a.Translation * b.Scale, b.Rotation);
+
+            return r;
+        }
+
+        /// <summary>
+        /// This method is equivalent to System.Numerics.Vector3.Transform(Vector3 v, Quaternion q)
+        /// </summary>
+        /// <param name="v">The vector to transform</param>
+        /// <param name="q">The transform rotation</param>
+        /// <returns>The rotated vector</returns>
+        private static Vector3 Vector3Transform(Vector3 v, Quaternion q)
+        {
+            // Extract the vector part of the quaternion
+            var u = new Vector3(q.X, q.Y, q.Z);
+
+            // Extract the scalar part of the quaternion
+            var s = q.W;
+
+            // Do the math
+            return (2.0f * Vector3.Dot(u, v) * u)
+                + (((s * s) - Vector3.Dot(u, u)) * v)
+                + (2.0f * s * Vector3.Cross(u, v));
         }
 
         #endregion

src/SharpGLTF.Core/Transforms/Matrix4x4Factory.cs

@@ -23,15 +23,15 @@ namespace SharpGLTF.Transforms
                 return transform.Value;
             }
 
-            return new AffineTransform(null, scale, rotation, translation).Matrix;
+            return new AffineTransform(scale, rotation, translation).Matrix;
         }
 
-        public static Matrix4x4 LocalToWorld(Matrix4x4 parentWorld, Matrix4x4 childLocal)
+        public static Matrix4x4 LocalToWorld(in Matrix4x4 parentWorld, in Matrix4x4 childLocal)
         {
             return childLocal * parentWorld;
         }
 
-        public static Matrix4x4 WorldToLocal(Matrix4x4 parentWorld, Matrix4x4 childWorld)
+        public static Matrix4x4 WorldToLocal(in Matrix4x4 parentWorld, in Matrix4x4 childWorld)
         {
             return childWorld * parentWorld.Inverse();
         }

src/SharpGLTF.Toolkit/Scenes/NodeBuilder.cs

@@ -141,11 +141,7 @@ namespace SharpGLTF.Scenes
         /// </summary>
         public Transforms.AffineTransform LocalTransform
         {
-            get => _Matrix.HasValue
-                ?
-                Transforms.AffineTransform.Create(_Matrix.Value)
-                :
-                Transforms.AffineTransform.Create(Scale?.Value, Rotation?.Value, Translation?.Value);
+            get => Transforms.AffineTransform.CreateFromAny(_Matrix, _Scale.Value, _Rotation.Value, Translation.Value);
             set
             {
                 Guard.IsTrue(value.IsValid, nameof(value));
@@ -299,7 +295,7 @@ namespace SharpGLTF.Scenes
 
         private void _DecomposeMatrix(Matrix4x4 matrix)
         {
-            var affine = Transforms.AffineTransform.Create(matrix);
+            var affine = new Transforms.AffineTransform(matrix);
 
             UseScale().Value = affine.Scale;
             UseRotation().Value = affine.Rotation;
@@ -374,7 +370,7 @@ namespace SharpGLTF.Scenes
             var rotation = Rotation?.GetValueAt(animationTrack, time);
             var translation = Translation?.GetValueAt(animationTrack, time);
 
-            return Transforms.AffineTransform.Create(scale, rotation, translation);
+            return new Transforms.AffineTransform(scale, rotation, translation);
         }
 
         public Matrix4x4 GetWorldMatrix(string animationTrack, float time)

tests/SharpGLTF.Tests/Transforms/InverseBindMatrixTest.cs

@@ -87,5 +87,19 @@ namespace SharpGLTF.Transforms
             Assert.IsFalse(Matrix4x4.Decompose(RxS, out _, out _, out _));
             testMatrix(RxS, 100);           
         }
+
+        [Test]
+        public void TestAffineTransformMult()
+        {
+            var a = Matrix4x4.CreateScale(1,2,4) * Matrix4x4.CreateFromYawPitchRoll(1, 2, 3);
+            var b = Matrix4x4.CreateFromYawPitchRoll(1, 0, 2) * Matrix4x4.CreateTranslation(3, -4, 2);
+            var r = Matrix4x4.Multiply(a, b);
+
+            var aa = new AffineTransform(a);
+            var bb = new AffineTransform(b);
+            var rr = AffineTransform.Multiply(aa, bb);
+
+            NumericsAssert.AreEqual(r, rr.Matrix, 0.0001f);
+        }
     }    
 }