From 5a402df3ff72c91654f62c324aa045e39fb64666 Mon Sep 17 00:00:00 2001 From: Wojtek Figat Date: Wed, 27 Jan 2021 15:07:02 +0100 Subject: [PATCH] Add Add/Subtract methods to Transform --- Source/Engine/Core/Math/Quaternion.cs | 8 +++ Source/Engine/Core/Math/Transform.cpp | 15 +++-- Source/Engine/Core/Math/Transform.cs | 59 +++++++++++++++++-- Source/Engine/Core/Math/Transform.h | 12 ++++ .../Tools/FlaxEngine.Tests/TestTransform.cs | 24 +++++++- 5 files changed, 108 insertions(+), 10 deletions(-) diff --git a/Source/Engine/Core/Math/Quaternion.cs b/Source/Engine/Core/Math/Quaternion.cs index bc86fa2c9..0e809f271 100644 --- a/Source/Engine/Core/Math/Quaternion.cs +++ b/Source/Engine/Core/Math/Quaternion.cs @@ -324,6 +324,14 @@ namespace FlaxEngine Z = -Z; } + /// + /// Gets the conjugated quaternion. + /// + public Quaternion Conjugated() + { + return new Quaternion(-X, -Y, -Z, W); + } + /// /// Conjugates and renormalizes the quaternion. /// diff --git a/Source/Engine/Core/Math/Transform.cpp b/Source/Engine/Core/Math/Transform.cpp index 728fc6a90..35f616236 100644 --- a/Source/Engine/Core/Math/Transform.cpp +++ b/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; } diff --git a/Source/Engine/Core/Math/Transform.cs b/Source/Engine/Core/Math/Transform.cs index 7065309e3..129ddf92a 100644 --- a/Source/Engine/Core/Math/Transform.cs +++ b/Source/Engine/Core/Math/Transform.cs @@ -169,6 +169,37 @@ namespace FlaxEngine Matrix.Transformation(ref Scale, ref Orientation, ref Translation, out result); } + /// + /// Adds two transforms. + /// + /// The first transform to add. + /// The second transform to add. + /// The sum of the two transforms. + 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; + } + + /// + /// Subtracts two transforms. + /// + /// The first transform to subtract from. + /// The second transform to subtract. + /// The difference of the two transforms. + 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; + } + /// /// Perform transformation of the given transform in local space /// @@ -176,12 +207,10 @@ namespace FlaxEngine /// World space transform 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 /// Local space transform 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); } + /// + /// Adds two transformations. + /// + /// The first transform to add. + /// The second transform to add. + /// The sum of the two transformations. + public static Transform operator +(Transform left, Transform right) + { + return Add(left, right); + } + + /// + /// Subtracts two transformations. + /// + /// The first transform to subtract from. + /// The second transform to subtract. + /// The difference of the two transformations. + public static Transform operator -(Transform left, Transform right) + { + return Subtract(left, right); + } + /// /// Returns a that represents this instance. /// diff --git a/Source/Engine/Core/Math/Transform.h b/Source/Engine/Core/Math/Transform.h index 5a39c2d86..069a49992 100644 --- a/Source/Engine/Core/Math/Transform.h +++ b/Source/Engine/Core/Math/Transform.h @@ -163,6 +163,13 @@ public: /// The sum of two transformations. Transform Add(const Transform& other) const; + /// + /// Subtracts transformation from this transform. + /// + /// The other transformation. + /// The different of two transformations. + Transform Subtract(const Transform& other) const; + /// /// Performs transformation of the given transform in local space to the world space of this transform. /// @@ -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); diff --git a/Source/Tools/FlaxEngine.Tests/TestTransform.cs b/Source/Tools/FlaxEngine.Tests/TestTransform.cs index d947c295e..38f71e217 100644 --- a/Source/Tools/FlaxEngine.Tests/TestTransform.cs +++ b/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}"); + } + } + + /// + /// Test conversions between transform local/world space + /// + [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}"); } } }