Add Add/Subtract methods to Transform

Source/Engine/Core/Math/Quaternion.cs

@@ -324,6 +324,14 @@ namespace FlaxEngine
             Z = -Z;
         }
 
+        /// <summary>
+        /// Gets the conjugated quaternion.
+        /// </summary>
+        public Quaternion Conjugated()
+        {
+            return new Quaternion(-X, -Y, -Z, W);
+        }
+
         /// <summary>
         /// Conjugates and renormalizes the quaternion.
         /// </summary>

Source/Engine/Core/Math/Transform.cpp

@@ -43,37 +43,42 @@ void Transform::GetWorld(Matrix& result) const
 Transform Transform::Add(const Vector3& translation) const
 {
     Transform result;
-
     result.Orientation = Orientation;
     result.Scale = Scale;
     Vector3::Add(Translation, translation, result.Translation);
-
     return result;
 }
 
 Transform Transform::Add(const Transform& other) const
 {
     Transform result;
-
     Quaternion::Multiply(Orientation, other.Orientation, result.Orientation);
     result.Orientation.Normalize();
     Vector3::Multiply(Scale, other.Scale, result.Scale);
     Vector3::Add(Translation, other.Translation, result.Translation);
+    return result;
+}
 
+Transform Transform::Subtract(const Transform& other) const
+{
+    Transform result;
+    Vector3::Subtract(Translation, other.Translation, result.Translation);
+    const Quaternion invRotation = other.Orientation.Conjugated();
+    Quaternion::Multiply(Orientation, invRotation, result.Orientation);
+    result.Orientation.Normalize();
+    Vector3::Divide(Scale, other.Scale, result.Scale);
     return result;
 }
 
 Transform Transform::LocalToWorld(const Transform& other) const
 {
     Transform result;
-
     Quaternion::Multiply(Orientation, other.Orientation, result.Orientation);
     result.Orientation.Normalize();
     Vector3::Multiply(Scale, other.Scale, result.Scale);
     Vector3 tmp = other.Translation * Scale;
     Vector3::Transform(tmp, Orientation, tmp);
     Vector3::Add(tmp, Translation, result.Translation);
-
     return result;
 }
 

Source/Engine/Core/Math/Transform.cs

@@ -169,6 +169,37 @@ namespace FlaxEngine
             Matrix.Transformation(ref Scale, ref Orientation, ref Translation, out result);
         }
 
+        /// <summary>
+        /// Adds two transforms.
+        /// </summary>
+        /// <param name="left">The first transform to add.</param>
+        /// <param name="right">The second transform to add.</param>
+        /// <returns>The sum of the two transforms.</returns>
+        public static Transform Add(Transform left, Transform right)
+        {
+            Transform result;
+            Quaternion.Multiply(ref left.Orientation, ref right.Orientation, out result.Orientation);
+            Vector3.Multiply(ref left.Scale, ref right.Scale, out result.Scale);
+            Vector3.Add(ref left.Translation, ref right.Translation, out result.Translation);
+            return result;
+        }
+
+        /// <summary>
+        /// Subtracts two transforms.
+        /// </summary>
+        /// <param name="left">The first transform to subtract from.</param>
+        /// <param name="right">The second transform to subtract.</param>
+        /// <returns>The difference of the two transforms.</returns>
+        public static Transform Subtract(Transform left, Transform right)
+        {
+            Transform result;
+            Vector3.Subtract(ref left.Translation, ref right.Translation, out result.Translation);
+            Quaternion invRotation = right.Orientation.Conjugated();
+            Quaternion.Multiply(ref left.Orientation, ref invRotation, out result.Orientation);
+            Vector3.Divide(ref left.Scale, ref right.Scale, out result.Scale);
+            return result;
+        }
+
         /// <summary>
         /// Perform transformation of the given transform in local space
         /// </summary>
@@ -176,12 +207,10 @@ namespace FlaxEngine
         /// <returns>World space transform</returns>
         public Transform LocalToWorld(Transform other)
         {
-            Transform result = new Transform(Vector3.Zero);
-
+            Transform result;
             Quaternion.Multiply(ref Orientation, ref other.Orientation, out result.Orientation);
             Vector3.Multiply(ref Scale, ref other.Scale, out result.Scale);
             result.Translation = LocalToWorld(other.Translation);
-
             return result;
         }
 
@@ -229,7 +258,6 @@ namespace FlaxEngine
         /// <returns>Local space transform</returns>
         public Transform WorldToLocal(Transform other)
         {
-            Transform result = new Transform(Vector3.Zero);
             Vector3 invScale = Scale;
             if (invScale.X != 0.0f)
                 invScale.X = 1.0f / invScale.X;
@@ -238,6 +266,7 @@ namespace FlaxEngine
             if (invScale.Z != 0.0f)
                 invScale.Z = 1.0f / invScale.Z;
 
+            Transform result;
             result.Orientation = Orientation;
             result.Orientation.Invert();
             Quaternion.Multiply(ref result.Orientation, ref other.Orientation, out result.Orientation);
@@ -404,6 +433,28 @@ namespace FlaxEngine
             return !left.Equals(ref right);
         }
 
+        /// <summary>
+        /// Adds two transformations.
+        /// </summary>
+        /// <param name="left">The first transform to add.</param>
+        /// <param name="right">The second transform to add.</param>
+        /// <returns>The sum of the two transformations.</returns>
+        public static Transform operator +(Transform left, Transform right)
+        {
+            return Add(left, right);
+        }
+
+        /// <summary>
+        /// Subtracts two transformations.
+        /// </summary>
+        /// <param name="left">The first transform to subtract from.</param>
+        /// <param name="right">The second transform to subtract.</param>
+        /// <returns>The difference of the two transformations.</returns>
+        public static Transform operator -(Transform left, Transform right)
+        {
+            return Subtract(left, right);
+        }
+
         /// <summary>
         /// Returns a <see cref="System.String" /> that represents this instance.
         /// </summary>

Source/Engine/Core/Math/Transform.h

@@ -163,6 +163,13 @@ public:
     /// <returns>The sum of two transformations.</returns>
     Transform Add(const Transform& other) const;
 
+    /// <summary>
+    /// Subtracts transformation from this transform.
+    /// </summary>
+    /// <param name="other">The other transformation.</param>
+    /// <returns>The different of two transformations.</returns>
+    Transform Subtract(const Transform& other) const;
+
     /// <summary>
     /// Performs transformation of the given transform in local space to the world space of this transform.
     /// </summary>
@@ -254,6 +261,11 @@ public:
         return Add(other);
     }
 
+    FORCE_INLINE Transform operator-(const Transform& other) const
+    {
+        return Subtract(other);
+    }
+
     FORCE_INLINE Transform operator+(const Vector3& other) const
     {
         return Add(other);

Source/Tools/FlaxEngine.Tests/TestTransform.cs

@@ -143,7 +143,29 @@ namespace FlaxEngine.Tests
                 Transform ab = a.LocalToWorld(b);
                 Transform ba = a.WorldToLocal(ab);
 
-                Assert.IsTrue(Transform.NearEqual(ref b, ref ba, 0.00001f));
+                Assert.IsTrue(Transform.NearEqual(ref b, ref ba, 0.00001f), $"Got: {b} but expected {ba}");
+            }
+        }
+
+        /// <summary>
+        /// Test conversions between transform local/world space
+        /// </summary>
+        [Test]
+        public void TestAddSubtract()
+        {
+            var rand = new Random(10);
+            for (int i = 0; i < 10; i++)
+            {
+                Transform a = new Transform(rand.NextVector3(), Quaternion.Euler(i * 10, 0, i), rand.NextVector3() * 10.0f);
+                Transform b = new Transform(rand.NextVector3(), Quaternion.Euler(i, 1, 22), rand.NextVector3() * 0.3f);
+
+                Transform ab = a + b;
+                Transform newA = ab - b;
+                Assert.IsTrue(Transform.NearEqual(ref a, ref newA, 0.00001f), $"Got: {newA} but expected {a}");
+
+                Transform ba = b + a;
+                Transform newB = ba - a;
+                Assert.IsTrue(Transform.NearEqual(ref b, ref newB, 0.00001f), $"Got: {newB} but expected {b}");
             }
         }
     }