From 95245e2424681d2b86c35cb1c7f9847540ff2451 Mon Sep 17 00:00:00 2001
From: Benjamin Jillich <43751992+amzn-jillich@users.noreply.github.com>
Date: Tue, 4 Jan 2022 09:13:14 +0100
Subject: [PATCH] EMotion FX: Linear and angular velocity calculation and debug
 visualization helpers (#6613)

* Added helper functions for calculating linear and angular velocities.
* Added debug visualization helper for linear and angular velocities.

Signed-off-by: Benjamin Jillich <jillich@amazon.com>
---
 .../Code/EMotionFX/Source/Velocity.cpp        | 83 +++++++++++++++++++
 .../Code/EMotionFX/Source/Velocity.h          | 23 +++++
 .../Code/EMotionFX/emotionfx_files.cmake      |  2 +
 3 files changed, 108 insertions(+)
 create mode 100644 Gems/EMotionFX/Code/EMotionFX/Source/Velocity.cpp
 create mode 100644 Gems/EMotionFX/Code/EMotionFX/Source/Velocity.h

diff --git a/Gems/EMotionFX/Code/EMotionFX/Source/Velocity.cpp b/Gems/EMotionFX/Code/EMotionFX/Source/Velocity.cpp
new file mode 100644
index 0000000000..e02262fa8e
--- /dev/null
+++ b/Gems/EMotionFX/Code/EMotionFX/Source/Velocity.cpp
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#include <EMotionFX/Source/Velocity.h>
+#include <AzCore/Math/MathUtils.h>
+
+namespace EMotionFX
+{
+    AZ::Vector3 CalculateLinearVelocity(const AZ::Vector3& lastPosition,
+        const AZ::Vector3& currentPosition,
+        float timeDelta)
+    {
+        if (timeDelta <= AZ::Constants::FloatEpsilon)
+        {
+            return AZ::Vector3::CreateZero();
+        }
+
+        const AZ::Vector3 deltaPosition = currentPosition - lastPosition;
+        const AZ::Vector3 velocity = deltaPosition / timeDelta;
+
+        if (velocity.GetLength() > AZ::Constants::FloatEpsilon)
+        {
+            return velocity;
+        }
+
+        return AZ::Vector3::CreateZero();
+    }
+
+    AZ::Vector3 CalculateAngularVelocity(const AZ::Quaternion& lastRotation,
+        const AZ::Quaternion& currentRotation,
+        float timeDelta)
+    {
+        if (timeDelta <= AZ::Constants::FloatEpsilon)
+        {
+            return AZ::Vector3::CreateZero();
+        }
+
+        const AZ::Quaternion deltaRotation = currentRotation * lastRotation.GetInverseFull();
+        const AZ::Quaternion shortestEquivalent = deltaRotation.GetShortestEquivalent().GetNormalized();
+        const AZ::Vector3 scaledAxisAngle = shortestEquivalent.ConvertToScaledAxisAngle();
+        const AZ::Vector3 angularVelocity = scaledAxisAngle / timeDelta;
+
+        if (angularVelocity.GetLength() > AZ::Constants::FloatEpsilon)
+        {
+            return angularVelocity;
+        }
+
+        return AZ::Vector3::CreateZero();
+    }
+
+    void DebugDrawVelocity(AzFramework::DebugDisplayRequests& debugDisplay,
+        const AZ::Vector3& position, const AZ::Vector3& velocity, const AZ::Color& color)
+    {
+        // Don't visualize joints that remain motionless (zero velocity).
+        if (velocity.GetLength() < AZ::Constants::FloatEpsilon)
+        {
+            return;
+        }
+
+        const float scale = 0.15f;
+        const AZ::Vector3 arrowPosition = position + velocity;
+
+        debugDisplay.DepthTestOff();
+        debugDisplay.SetColor(color);
+
+        debugDisplay.DrawSolidCylinder(/*center=*/(arrowPosition + position) * 0.5f,
+            /*direction=*/(arrowPosition - position).GetNormalizedSafe(),
+            /*radius=*/0.003f,
+            /*height=*/(arrowPosition - position).GetLength(),
+            /*drawShaded=*/false);
+
+        debugDisplay.DrawSolidCone(position + velocity,
+            velocity,
+            0.1f * scale,
+            scale * 0.5f,
+            /*drawShaded=*/false);
+    }
+} // namespace EMotionFX
diff --git a/Gems/EMotionFX/Code/EMotionFX/Source/Velocity.h b/Gems/EMotionFX/Code/EMotionFX/Source/Velocity.h
new file mode 100644
index 0000000000..a37a888024
--- /dev/null
+++ b/Gems/EMotionFX/Code/EMotionFX/Source/Velocity.h
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#pragma once
+
+#include <AzCore/Math/Vector3.h>
+#include <AzCore/Math/Quaternion.h>
+#include <AzFramework/Entity/EntityDebugDisplayBus.h>
+#include <EMotionFX/Source/EMotionFXConfig.h>
+
+namespace EMotionFX
+{
+    AZ::Vector3 EMFX_API CalculateLinearVelocity(const AZ::Vector3& lastPosition, const AZ::Vector3& currentPosition, float timeDelta);
+    AZ::Vector3 EMFX_API CalculateAngularVelocity(const AZ::Quaternion& lastRotation, const AZ::Quaternion& currentRotation, float timeDelta);
+
+    void EMFX_API DebugDrawVelocity(AzFramework::DebugDisplayRequests& debugDisplay,
+        const AZ::Vector3& position, const AZ::Vector3& velocity, const AZ::Color& color);
+} // namespace EMotionFX
diff --git a/Gems/EMotionFX/Code/EMotionFX/emotionfx_files.cmake b/Gems/EMotionFX/Code/EMotionFX/emotionfx_files.cmake
index 8135f14f12..52c9d6aa15 100644
--- a/Gems/EMotionFX/Code/EMotionFX/emotionfx_files.cmake
+++ b/Gems/EMotionFX/Code/EMotionFX/emotionfx_files.cmake
@@ -144,6 +144,8 @@ set(FILES
     Source/TransformData.h
     Source/TriggerActionSetup.cpp
     Source/TriggerActionSetup.h
+    Source/Velocity.cpp
+    Source/Velocity.h
     Source/VertexAttributeLayer.cpp
     Source/VertexAttributeLayer.h
     Source/VertexAttributeLayerAbstractData.cpp