From dda5f26d8993da9bbc47204537d383238afaed46 Mon Sep 17 00:00:00 2001
From: Wojtek Figat <wojtek@figat.pl>
Date: Tue, 26 Jan 2021 12:11:41 +0100
Subject: [PATCH] Add `Transform.LocalToWorldVector` and
 `Transform.WorldToLocalVector`

---
 Source/Engine/Core/Math/Transform.cpp | 25 +++++++++++++++++++
 Source/Engine/Core/Math/Transform.cs  | 35 +++++++++++++++++++++++++++
 Source/Engine/Core/Math/Transform.h   | 14 +++++++++++
 3 files changed, 74 insertions(+)

diff --git a/Source/Engine/Core/Math/Transform.cpp b/Source/Engine/Core/Math/Transform.cpp
index 5ac71779b..728fc6a90 100644
--- a/Source/Engine/Core/Math/Transform.cpp
+++ b/Source/Engine/Core/Math/Transform.cpp
@@ -94,6 +94,13 @@ Vector3 Transform::LocalToWorld(const Vector3& point) const
     return result + Translation;
 }
 
+Vector3 Transform::LocalToWorldVector(const Vector3& vector) const
+{
+    Vector3 result = vector * Scale;
+    Vector3::Transform(result, Orientation, result);
+    return result;
+}
+
 void Transform::LocalToWorld(const Vector3& point, Vector3& result) const
 {
     Vector3 tmp = point * Scale;
@@ -171,6 +178,24 @@ Vector3 Transform::WorldToLocal(const Vector3& point) const
     return result * invScale;
 }
 
+Vector3 Transform::WorldToLocalVector(const Vector3& vector) const
+{
+    Vector3 invScale = Scale;
+    if (invScale.X != 0.0f)
+        invScale.X = 1.0f / invScale.X;
+    if (invScale.Y != 0.0f)
+        invScale.Y = 1.0f / invScale.Y;
+    if (invScale.Z != 0.0f)
+        invScale.Z = 1.0f / invScale.Z;
+
+    const Quaternion invRotation = Orientation.Conjugated();
+
+    Vector3 result;
+    Vector3::Transform(vector, invRotation, result);
+
+    return result * invScale;
+}
+
 void Transform::WorldToLocal(const Vector3* points, int32 pointsCount, Vector3* result) const
 {
     Vector3 invScale = Scale;
diff --git a/Source/Engine/Core/Math/Transform.cs b/Source/Engine/Core/Math/Transform.cs
index 21ed705ff..7065309e3 100644
--- a/Source/Engine/Core/Math/Transform.cs
+++ b/Source/Engine/Core/Math/Transform.cs
@@ -197,6 +197,18 @@ namespace FlaxEngine
             return point + Translation;
         }
 
+        /// <summary>
+        /// Performs transformation of the given vector in local space to the world space of this transform.
+        /// </summary>
+        /// <param name="vector">The local space vector.</param>
+        /// <returns>The world space vector.</returns>
+        public Vector3 LocalToWorldVector(Vector3 vector)
+        {
+            vector *= Scale;
+            Vector3.Transform(ref vector, ref Orientation, out vector);
+            return vector;
+        }
+
         /// <summary>
         /// Perform transformation of the given points in local space
         /// </summary>
@@ -259,6 +271,29 @@ namespace FlaxEngine
             return result * invScale;
         }
 
+        /// <summary>
+        /// Perform transformation of the given vector in world space
+        /// </summary>
+        /// <param name="vector">World space vector</param>
+        /// <returns>Local space vector</returns>
+        public Vector3 WorldToLocalVector(Vector3 vector)
+        {
+            Vector3 invScale = Scale;
+            if (invScale.X != 0.0f)
+                invScale.X = 1.0f / invScale.X;
+            if (invScale.Y != 0.0f)
+                invScale.Y = 1.0f / invScale.Y;
+            if (invScale.Z != 0.0f)
+                invScale.Z = 1.0f / invScale.Z;
+
+            Quaternion invRotation = Orientation;
+            invRotation.Invert();
+
+            Vector3.Transform(ref vector, ref invRotation, out var result);
+
+            return result * invScale;
+        }
+
         /// <summary>
         /// Perform transformation of the given points in world space
         /// </summary>
diff --git a/Source/Engine/Core/Math/Transform.h b/Source/Engine/Core/Math/Transform.h
index c5e67c986..5a39c2d86 100644
--- a/Source/Engine/Core/Math/Transform.h
+++ b/Source/Engine/Core/Math/Transform.h
@@ -184,6 +184,13 @@ public:
     /// <returns>The world space point.</returns>
     Vector3 LocalToWorld(const Vector3& point) const;
 
+    /// <summary>
+    /// Performs transformation of the given vector in local space to the world space of this transform.
+    /// </summary>
+    /// <param name="vector">The local space vector.</param>
+    /// <returns>The world space vector.</returns>
+    Vector3 LocalToWorldVector(const Vector3& vector) const;
+
     /// <summary>
     /// Performs transformation of the given point in local space to the world space of this transform.
     /// </summary>
@@ -220,6 +227,13 @@ public:
     /// <returns>The local space point.</returns>
     Vector3 WorldToLocal(const Vector3& point) const;
 
+    /// <summary>
+    /// Performs transformation of the given vector in world space to the local space of this transform.
+    /// </summary>
+    /// <param name="vector">The world space vector.</param>
+    /// <returns>The local space vector.</returns>
+    Vector3 WorldToLocalVector(const Vector3& vector) const;
+
     /// <summary>
     /// Performs transformation of the given points in world space to the local space of this transform.
     /// </summary>