Motion Matching: Added query pose and query joint velocity debug visualization and new CVARs for debug rendering control (#8372)

* Added new helper to render the query pose and joint velocities (input pose for the motion matching search)
* Added CVAR to enable/disable usage of the Kd-tree and switch between brute-force search and using the acceleration structure.
* Added CVAR to enable/disable all debug visualizations.
* Added new helper function to render joint velocities based on a given pose, rather than always using the current character pose.

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

Gems/MotionMatching/Code/Source/FeatureVelocity.cpp

@@ -29,7 +29,7 @@ namespace EMotionFX::MotionMatching
 
     void FeatureVelocity::FillQueryFeatureValues(size_t startIndex, AZStd::vector<float>& queryFeatureValues, const FrameCostContext& context)
     {
-        PoseDataJointVelocities* velocityPoseData = static_cast<PoseDataJointVelocities*>(context.m_currentPose.GetPoseDataByType(azrtti_typeid<PoseDataJointVelocities>()));
+        PoseDataJointVelocities* velocityPoseData = context.m_currentPose.GetPoseData<PoseDataJointVelocities>();
         AZ_Assert(velocityPoseData, "Cannot calculate velocity feature cost without joint velocity pose data.");
         const AZ::Vector3 currentVelocity = velocityPoseData->GetVelocity(m_jointIndex);
 
@@ -60,16 +60,14 @@ namespace EMotionFX::MotionMatching
     }
 
     void FeatureVelocity::DebugDraw(AzFramework::DebugDisplayRequests& debugDisplay,
-        MotionMatchingInstance* instance,
+        const Pose& pose,
         const AZ::Vector3& velocity,
         size_t jointIndex,
         size_t relativeToJointIndex,
         const AZ::Color& color)
     {
-        const ActorInstance* actorInstance = instance->GetActorInstance();
-        const Pose* pose = actorInstance->GetTransformData()->GetCurrentPose();
-        const Transform jointModelTM = pose->GetModelSpaceTransform(jointIndex);
-        const Transform relativeToWorldTM = pose->GetWorldSpaceTransform(relativeToJointIndex);
+        const Transform jointModelTM = pose.GetModelSpaceTransform(jointIndex);
+        const Transform relativeToWorldTM = pose.GetWorldSpaceTransform(relativeToJointIndex);
 
         const AZ::Vector3 jointPosition = relativeToWorldTM.TransformPoint(jointModelTM.m_position);
         const AZ::Vector3 velocityWorldSpace = relativeToWorldTM.TransformVector(velocity);
@@ -77,6 +75,18 @@ namespace EMotionFX::MotionMatching
         DebugDrawVelocity(debugDisplay, jointPosition, velocityWorldSpace * mm_debugDrawVelocityScale, color);
     }
 
+    void FeatureVelocity::DebugDraw(AzFramework::DebugDisplayRequests& debugDisplay,
+        MotionMatchingInstance* instance,
+        const AZ::Vector3& velocity,
+        size_t jointIndex,
+        size_t relativeToJointIndex,
+        const AZ::Color& color)
+    {
+        const ActorInstance* actorInstance = instance->GetActorInstance();
+        const Pose* pose = actorInstance->GetTransformData()->GetCurrentPose();
+        DebugDraw(debugDisplay, *pose, velocity, jointIndex, relativeToJointIndex, color);
+    }
+
     void FeatureVelocity::DebugDraw(AzFramework::DebugDisplayRequests& debugDisplay,
         MotionMatchingInstance* instance,
         size_t frameIndex)
@@ -93,7 +103,7 @@ namespace EMotionFX::MotionMatching
 
     float FeatureVelocity::CalculateFrameCost(size_t frameIndex, const FrameCostContext& context) const
     {
-        PoseDataJointVelocities* velocityPoseData = static_cast<PoseDataJointVelocities*>(context.m_currentPose.GetPoseDataByType(azrtti_typeid<PoseDataJointVelocities>()));
+        PoseDataJointVelocities* velocityPoseData = context.m_currentPose.GetPoseData<PoseDataJointVelocities>();
         AZ_Assert(velocityPoseData, "Cannot calculate velocity feature cost without joint velocity pose data.");
 
         const AZ::Vector3 currentVelocity = velocityPoseData->GetVelocity(m_jointIndex);

Gems/MotionMatching/Code/Source/FeatureVelocity.h

@@ -39,9 +39,16 @@ namespace EMotionFX::MotionMatching
 
         void ExtractFeatureValues(const ExtractFeatureContext& context) override;
 
+        static void DebugDraw(AzFramework::DebugDisplayRequests& debugDisplay,
+            const Pose& pose,
+            const AZ::Vector3& velocity, // in relative-to-joint space
+            size_t jointIndex,
+            size_t relativeToJointIndex,
+            const AZ::Color& color);
+
         static void DebugDraw(AzFramework::DebugDisplayRequests& debugDisplay,
             MotionMatchingInstance* instance,
-            const AZ::Vector3& velocity, // in world space
+            const AZ::Vector3& velocity, // in relative-to-joint space
             size_t jointIndex,
             size_t relativeToJointIndex,
             const AZ::Color& color);

Gems/MotionMatching/Code/Source/MotionMatchingInstance.cpp

@@ -6,30 +6,40 @@
  *
  */
 
+#include <AzCore/Console/IConsole.h>
 #include <AzCore/Debug/Timer.h>
 #include <AzCore/Component/ComponentApplicationBus.h>
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/SerializeContext.h>
 
 #include <EMotionFX/Source/ActorInstance.h>
-#include <Allocators.h>
 #include <EMotionFX/Source/EMotionFXManager.h>
 #include <EMotionFX/Source/Motion.h>
 #include <EMotionFX/Source/MotionInstance.h>
 #include <EMotionFX/Source/MotionInstancePool.h>
-#include <MotionMatchingData.h>
-#include <MotionMatchingInstance.h>
+#include <EMotionFX/Source/Pose.h>
+#include <EMotionFX/Source/TransformData.h>
+
+#include <Allocators.h>
 #include <Feature.h>
 #include <FeatureSchema.h>
 #include <FeatureTrajectory.h>
-#include <KdTree.h>
+#include <FeatureVelocity.h>
 #include <ImGuiMonitorBus.h>
-#include <EMotionFX/Source/Pose.h>
-#include <EMotionFX/Source/TransformData.h>
+#include <KdTree.h>
+#include <MotionMatchingData.h>
+#include <MotionMatchingInstance.h>
 #include <PoseDataJointVelocities.h>
 
+
 namespace EMotionFX::MotionMatching
 {
+    AZ_CVAR_EXTERNED(bool, mm_debugDraw);
+    AZ_CVAR_EXTERNED(float, mm_debugDrawVelocityScale);
+    AZ_CVAR_EXTERNED(bool, mm_debugDrawQueryPose);
+    AZ_CVAR_EXTERNED(bool, mm_debugDrawQueryVelocities);
+    AZ_CVAR_EXTERNED(bool, mm_useKdTree);
+
     AZ_CLASS_ALLOCATOR_IMPL(MotionMatchingInstance, MotionMatchAllocator, 0)
 
     MotionMatchingInstance::~MotionMatchingInstance()
@@ -118,6 +128,11 @@ namespace EMotionFX::MotionMatching
 
     void MotionMatchingInstance::DebugDraw(AzFramework::DebugDisplayRequests& debugDisplay)
     {
+        if (!mm_debugDraw)
+        {
+            return;
+        }
+
         AZ_PROFILE_SCOPE(Animation, "MotionMatchingInstance::DebugDraw");
 
         // Get the lowest cost frame index from the last search. As we're searching the feature database with a much lower
@@ -153,6 +168,42 @@ namespace EMotionFX::MotionMatching
 
         // Draw the trajectory history starting after the sampled version of the past trajectory.
         m_trajectoryHistory.DebugDraw(debugDisplay, trajectoryQueryColor, m_cachedTrajectoryFeature->GetPastTimeRange());
+
+        // Draw the input for the motion matching search.
+        DebugDrawQueryPose(debugDisplay, mm_debugDrawQueryPose, mm_debugDrawQueryVelocities);
+    }
+
+    void MotionMatchingInstance::DebugDrawQueryPose(AzFramework::DebugDisplayRequests& debugDisplay, bool drawPose, bool drawVelocities) const
+    {
+        const AZ::Color color = AZ::Color::CreateOne();
+
+        if (drawPose)
+        {
+            m_queryPose.DebugDraw(debugDisplay, color);
+        }
+
+        if (drawVelocities)
+        {
+            PoseDataJointVelocities* velocityPoseData = m_queryPose.GetPoseData<PoseDataJointVelocities>();
+            if (velocityPoseData)
+            {
+                const Skeleton* skeleton = m_actorInstance->GetActor()->GetSkeleton();
+                for (const Feature* feature : m_data->GetFeatureSchema().GetFeatures())
+                {
+                    if (const FeatureVelocity* velocityFeature = azdynamic_cast<const FeatureVelocity*>(feature))
+                    {
+                        Node* joint = skeleton->FindNodeByName(velocityFeature->GetJointName());
+                        if (joint)
+                        {
+                            const size_t jointIndex = joint->GetNodeIndex();
+                            const size_t relativeToJointIndex = feature->GetRelativeToNodeIndex();
+                            const AZ::Vector3& velocity = velocityPoseData->GetVelocities()[jointIndex];
+                            velocityFeature->DebugDraw(debugDisplay, m_queryPose, velocity, jointIndex, relativeToJointIndex, color);
+                        }
+                    }
+                }
+            }
+        }
     }
 
     void MotionMatchingInstance::SamplePose(MotionInstance* motionInstance, Pose& outputPose)
@@ -432,6 +483,7 @@ namespace EMotionFX::MotionMatching
         const FeatureTrajectory* trajectoryFeature = m_cachedTrajectoryFeature;
 
         // 1. Broad-phase search using KD-tree
+        if (mm_useKdTree)
         {
             // Build the input query features that will be compared to every entry in the feature database in the motion matching search.
             size_t startOffset = 0;
@@ -455,12 +507,10 @@ namespace EMotionFX::MotionMatching
         float minTrajectoryFutureCost = 0.0f;
 
         // Iterate through the frames filtered by the broad-phase search.
-#ifdef SEARCH_THROUGH_WHOLE_MOTIONDATABASE
-        for (size_t frameIndex = 0; frameIndex < frameDatabase.GetNumFrames(); ++frameIndex)
-#else
-        for (const size_t frameIndex : m_nearestFrames)
-#endif
+        const size_t numFrames = mm_useKdTree ? m_nearestFrames.size() : frameDatabase.GetNumFrames();
+        for (size_t i = 0; i < numFrames; ++i)
         {
+            const size_t frameIndex = mm_useKdTree ? m_nearestFrames[i] : i;
             const Frame& frame = frameDatabase.GetFrame(frameIndex);
 
             // TODO: This shouldn't be there, we should be discarding the frames when extracting the features and not at runtime when checking the cost.

Gems/MotionMatching/Code/Source/MotionMatchingInstance.h

@@ -85,6 +85,7 @@ namespace EMotionFX::MotionMatching
 
     private:
         MotionInstance* CreateMotionInstance() const;
+        void DebugDrawQueryPose(AzFramework::DebugDisplayRequests& debugDisplay, bool drawPose, bool drawVelocities) const;
         void SamplePose(MotionInstance* motionInstance, Pose& outputPose);
         void SamplePose(Motion* motion, Pose& outputPose, float sampleTime) const;
 

Gems/MotionMatching/Code/Source/MotionMatchingSystemComponent.cpp

@@ -30,9 +30,22 @@
 
 namespace EMotionFX::MotionMatching
 {
+    AZ_CVAR(bool, mm_debugDraw, true, nullptr, AZ::ConsoleFunctorFlags::Null,
+        "Global flag for motion matching debug drawing. Feature-wise debug drawing can be enabled or disabled in the anim graph itself.");
+
     AZ_CVAR(float, mm_debugDrawVelocityScale, 0.1f, nullptr, AZ::ConsoleFunctorFlags::Null,
         "Scaling value used for velocity debug rendering.");
 
+    AZ_CVAR(bool, mm_debugDrawQueryPose, false, nullptr, AZ::ConsoleFunctorFlags::Null,
+        "Draw the query skeletal pose used as input pose for the motion matching search.");
+
+    AZ_CVAR(bool, mm_debugDrawQueryVelocities, false, nullptr, AZ::ConsoleFunctorFlags::Null,
+        "Draw the query joint velocities used as input for the motion matching search.");
+
+    AZ_CVAR(bool, mm_useKdTree, true, nullptr, AZ::ConsoleFunctorFlags::Null,
+        "Use Kd-Tree to accelerate the motion matching search for the best next matching frame. "
+        "Disabling it will heavily slow down performance and should only be done for debugging purposes");
+
     void MotionMatchingSystemComponent::Reflect(AZ::ReflectContext* context)
     {
         if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))