Motion Matching: Performance improvement for calculating joint velocities (#8134)

* Profiling the latest version showed that calculating the joint velocities took a significant amount of time. After diving it turned out that we're sampling the animation too often, so consolidating that and sampling it only once decreased the time needed to calculate the joint velocities for the current pose significantly.
* Removed an unneeded/unused variable from the motion matching instance.
* Replaced a condition in the base feature with a clamp to simplify the code.

Signed-off-by: Benjamin Jillich <[email protected]>

Gems/MotionMatching/Code/Source/Feature.cpp

@@ -106,29 +106,22 @@ namespace EMotionFX::MotionMatching
         const float halfTimeRange = timeRange * 0.5f;
         const float startTime = originalTime - halfTimeRange;
         const float frameDelta = timeRange / numSamples;
+        const float motionDuration = motionInstance->GetMotion()->GetDuration();
 
         AZ::Vector3 accumulatedVelocity = AZ::Vector3::CreateZero();
 
         for (size_t sampleIndex = 0; sampleIndex < numSamples + 1; ++sampleIndex)
         {
             float sampleTime = startTime + sampleIndex * frameDelta;
-            if (sampleTime < 0.0f)
-            {
-                sampleTime = 0.0f;
-            }
-            if (sampleTime >= motionInstance->GetMotion()->GetDuration())
-            {
-                sampleTime = motionInstance->GetMotion()->GetDuration();
-            }
+            sampleTime = AZ::GetClamp(sampleTime, 0.0f, motionDuration);
+            motionInstance->SetCurrentTime(sampleTime);
 
             if (sampleIndex == 0)
             {
-                motionInstance->SetCurrentTime(sampleTime);
                 motionInstance->GetMotion()->Update(bindPose, &prevPose->GetPose(), motionInstance);
                 continue;
             }
 
-            motionInstance->SetCurrentTime(sampleTime);
             motionInstance->GetMotion()->Update(bindPose, &currentPose->GetPose(), motionInstance);
 
             const Transform inverseJointWorldTransform = currentPose->GetPose().GetWorldSpaceTransform(relativeToJointIndex).Inversed();

Gems/MotionMatching/Code/Source/MotionMatchingInstance.h

@@ -12,8 +12,6 @@
 #include <AzCore/Memory/Memory.h>
 #include <AzCore/RTTI/RTTI.h>
 
-#include <AzFramework/Entity/EntityDebugDisplayBus.h>
-
 #include <EMotionFX/Source/EMotionFXConfig.h>
 #include <Feature.h>
 #include <TrajectoryHistory.h>
@@ -115,7 +113,5 @@ namespace EMotionFX::MotionMatching
         /// Buffers used for FindLowestCostFrameIndex().
         AZStd::vector<float> m_tempCosts;
         AZStd::vector<float> m_minCosts;
-
-        AZStd::vector<AzFramework::DebugDisplayRequests*> m_debugDisplays;
     };
 } // namespace EMotionFX::MotionMatching

Gems/MotionMatching/Code/Source/PoseDataJointVelocities.cpp

@@ -7,7 +7,9 @@
  */
 
 #include <EMotionFX/Source/ActorInstance.h>
+#include <EMotionFX/Source/AnimGraphPose.h>
 #include <EMotionFX/Source/MotionInstance.h>
+#include <EMotionFX/Source/TransformData.h>
 #include <EMotionFX/Source/Velocity.h>
 #include <Allocators.h>
 #include <Feature.h>
@@ -138,15 +140,69 @@ namespace EMotionFX::MotionMatching
 
     void PoseDataJointVelocities::CalculateVelocity(MotionInstance* motionInstance, size_t relativeToJointIndex)
     {
+        const size_t numJoints = m_velocities.size();
         SetRelativeToJointIndex(relativeToJointIndex);
         ActorInstance* actorInstance = motionInstance->GetActorInstance();
-        m_velocities.resize(actorInstance->GetNumNodes());
-        m_angularVelocities.resize(actorInstance->GetNumNodes());
-        for (size_t i = 0; i < m_velocities.size(); ++i)
+        m_velocities.resize(numJoints);
+        m_angularVelocities.resize(numJoints);
+
+        const float originalTime = motionInstance->GetCurrentTime();
+
+        // Prepare for sampling.
+        AnimGraphPosePool& posePool = GetEMotionFX().GetThreadData(actorInstance->GetThreadIndex())->GetPosePool();
+        AnimGraphPose* prevPose = posePool.RequestPose(actorInstance);
+        AnimGraphPose* currentPose = posePool.RequestPose(actorInstance);
+        Pose* bindPose = actorInstance->GetTransformData()->GetBindPose();
+
+        const size_t numSamples = 3;
+        const float timeRange = 0.05f; // secs
+        const float halfTimeRange = timeRange * 0.5f;
+        const float startTime = originalTime - halfTimeRange;
+        const float frameDelta = timeRange / numSamples;
+        const float motionDuration = motionInstance->GetMotion()->GetDuration();
+
+        // Zero all linear and angular velocities.
+        Reset();
+
+        for (size_t sampleIndex = 0; sampleIndex < numSamples + 1; ++sampleIndex)
+        {
+            float sampleTime = startTime + sampleIndex * frameDelta;
+            sampleTime = AZ::GetClamp(sampleTime, 0.0f, motionDuration);
+            motionInstance->SetCurrentTime(sampleTime);
+
+            if (sampleIndex == 0)
+            {
+                motionInstance->GetMotion()->Update(bindPose, &prevPose->GetPose(), motionInstance);
+                continue;
+            }
+
+            motionInstance->GetMotion()->Update(bindPose, &currentPose->GetPose(), motionInstance);
+
+            const Transform inverseJointWorldTransform = currentPose->GetPose().GetWorldSpaceTransform(relativeToJointIndex).Inversed();
+
+            for (size_t jointIndex = 0; jointIndex < numJoints; ++jointIndex)
+            {
+                // Calculate the linear velocity.
+                const AZ::Vector3 prevPosition = prevPose->GetPose().GetWorldSpaceTransform(jointIndex).m_position;
+                const AZ::Vector3 currentPosition = currentPose->GetPose().GetWorldSpaceTransform(jointIndex).m_position;
+                const AZ::Vector3 velocity = CalculateLinearVelocity(prevPosition, currentPosition, frameDelta);
+                m_velocities[jointIndex] += inverseJointWorldTransform.TransformVector(velocity);
+            }
+
+            *prevPose = *currentPose;
+        }
+
+        const float numSamplesFloat = aznumeric_cast<float>(numSamples);
+        for (size_t i = 0; i < numJoints; ++i)
         {
-            Feature::CalculateVelocity(i, m_relativeToJointIndex, motionInstance, m_velocities[i]);
-            // TODO: Angular velocity not used yet.
+            m_velocities[i] /= numSamplesFloat;
+            m_angularVelocities[i] /= numSamplesFloat;
         }
+
+        motionInstance->SetCurrentTime(originalTime); // Set the current time back to what it was.
+
+        posePool.FreePose(prevPose);
+        posePool.FreePose(currentPose);
     }
 
     void PoseDataJointVelocities::Reflect(AZ::ReflectContext* context)

Gems/MotionMatching/Code/Source/PoseDataJointVelocities.h

@@ -15,9 +15,7 @@
 
 namespace EMotionFX::MotionMatching
 {
-    /**
-     * Extends a given pose with joint-relative linear and angular velocities.
-     **/
+    //! Extends a given pose with joint-relative linear and angular velocities.
     class EMFX_API PoseDataJointVelocities
         : public PoseData
     {
@@ -32,6 +30,8 @@ namespace EMotionFX::MotionMatching
 
         void LinkToActorInstance(const ActorInstance* actorInstance) override;
         void LinkToActor(const Actor* actor) override;
+
+        //! Zero all linear and angular velocities.
         void Reset() override;
 
         void CopyFrom(const PoseData* from) override;