WIP root bone transform calc

src/AnimNodes/AnimSamplerNode.cc

@@ -44,6 +44,14 @@ void AnimSamplerNode::Evaluate(
     ozz::log::Err() << "Error sampling animation." << std::endl;
   }
 
+  ozz::vector<ozz::math::SoaTransform> root_bone_cur;
+  root_bone_prev.push_back(ozz::math::SoaTransform());
+  sampling_job.ratio = m_anim_ratio;
+  sampling_job.output = make_span (root_bone_cur);
+  if (!sampling_job.Run()) {
+    ozz::log::Err() << "Error sampling animation." << std::endl;
+  }
+     
   ozz::math::SoaFloat3 translation =
       root_bone_prev[m_root_bone_index].translation;
   float root_prev_trans_x[4];
@@ -61,6 +69,10 @@ void AnimSamplerNode::Evaluate(
   _mm_store_ps(root_current_trans_x, translation.x);
   _mm_store_ps(root_current_trans_y, translation.y);
   _mm_store_ps(root_current_trans_z, translation.z);
+
+  root_transform->translation.x = root_current_trans_x[0];
+  root_transform->translation.y = root_current_trans_y[0];
+  root_transform->translation.z = root_current_trans_z[0];
 }
 
 void AnimSamplerNode::DrawDebugUi() {

tests/AnimSampleNodeTests.cc

@@ -3,16 +3,169 @@
 //
 
 #include "AnimNodes/AnimSamplerNode.h"
+#define OZZ_INCLUDE_PRIVATE_HEADER
+#include "../src/animation/runtime/animation_keyframe.h"
 #include "catch.hpp"
 
-TEST_CASE("AnimSamplerNode", "[AnimSamplerNode]") {
+void get_bone_transform(
+    int i_bone_idx,
+    const ozz::vector<ozz::math::SoaTransform>& i_local_matrices,
+    ozz::math::Transform& o_transform) {
+  int matrix_index = i_bone_idx / 4;
+  short simd_component = i_bone_idx % 4;
+  o_transform.translation.x =
+      i_local_matrices[matrix_index].translation.x[simd_component];
+  o_transform.translation.y =
+      i_local_matrices[matrix_index].translation.y[simd_component];
+  o_transform.translation.z =
+      i_local_matrices[matrix_index].translation.z[simd_component];
+}
+
+void sample_bone_transform(
+    float ratio,
+    int i_bone_idx,
+    const ozz::animation::Animation* i_animation,
+    ozz::animation::SamplingCache& io_cache,
+    ozz::vector<ozz::math::SoaTransform>& io_local_matrices,
+    ozz::math::Transform& o_transform) {
+  ozz::animation::SamplingJob sampling_job;
+  sampling_job.animation = i_animation;
+  sampling_job.cache = &io_cache;
+  sampling_job.ratio = ratio;
+  sampling_job.output = make_span(io_local_matrices);
+
+  if (!sampling_job.Run()) {
+    ozz::log::Err() << "Error sampling animation." << std::endl;
+  }
+
+  get_bone_transform(i_bone_idx, io_local_matrices, o_transform);
+}
+
+void calc_bone_translation(
+    float ratio,
+    int i_bone_idx,
+    const ozz::animation::Animation* i_animation,
+    ozz::math::Transform& o_transform) {
+  int key_idx = 0;
+  const ozz::span<const ozz::animation::Float3Key>& translations =
+      i_animation->translations();
+
+  int key_idx_prev = 0;
+  int key_idx_next = 0;
+  while (key_idx < translations.size()) {
+    key_idx_next = key_idx;
+    if (translations[key_idx].track == i_bone_idx) {
+      if (translations[key_idx].ratio < ratio) {
+        key_idx_prev = key_idx;
+      } else {
+        break;
+      }
+    }
+    key_idx++;
+  }
+
+  const ozz::animation::Float3Key& key_prev = translations[key_idx_prev];
+  const ozz::animation::Float3Key& key_next = translations[key_idx_next];
+
+  float r = 0.f;
+  if (key_prev.ratio != key_next.ratio) {
+    r = (ratio - key_prev.ratio) / (key_next.ratio - key_prev.ratio);
+  }
+  o_transform.translation.x =
+      ozz::math::HalfToFloat(key_prev.value[0])
+      + r
+            * (ozz::math::HalfToFloat(key_next.value[0])
+               - ozz::math::HalfToFloat(key_prev.value[0]));
+  o_transform.translation.y =
+      ozz::math::HalfToFloat(key_prev.value[1])
+      + r
+            * (ozz::math::HalfToFloat(key_next.value[1])
+               - ozz::math::HalfToFloat(key_prev.value[1]));
+  o_transform.translation.z =
+      ozz::math::HalfToFloat(key_prev.value[2])
+      + r
+            * (ozz::math::HalfToFloat(key_next.value[2])
+               - ozz::math::HalfToFloat(key_prev.value[2]));
+}
+
+TEST_CASE("Sample single bone channel", "[AnimSamplerNode]") {
+  SkinnedMesh skinned_mesh;
+  skinned_mesh.LoadSkeleton("../media/MixamoYBot-skeleton.ozz");
+  skinned_mesh.LoadAnimation("../media/Walking-loop.ozz");
+
+  ozz::animation::Animation* animation = skinned_mesh.m_animations[0];
+  const int num_soa_joints = skinned_mesh.m_skeleton.num_soa_joints();
+  const int num_joints = skinned_mesh.m_skeleton.num_joints();
+
+  ozz::vector<ozz::math::SoaTransform> local_matrices;
+  ozz::animation::SamplingCache sampling_cache;
+
+  local_matrices.resize(num_soa_joints);
+  sampling_cache.Resize(num_joints);
+
+  // sample at ratio 0.
+  float ratio = 0.f;
+
+  ozz::animation::SamplingJob sampling_job;
+  sampling_job.animation = animation;
+  sampling_job.cache = &sampling_cache;
+  sampling_job.ratio = ratio;
+  sampling_job.output = make_span(local_matrices);
+
+  if (!sampling_job.Run()) {
+    ozz::log::Err() << "Error sampling animation." << std::endl;
+  }
+
+  ozz::math::Transform root_transform_0;
+  sample_bone_transform(
+      0.f,
+      0,
+      animation,
+      sampling_cache,
+      local_matrices,
+      root_transform_0);
+
+  int n_samples = 20;
+  for (int i = 0; i <= n_samples; i++) {
+    float ratio = i * 1.f / n_samples;
+    ozz::math::Transform sampled_root_transform;
+    sample_bone_transform(
+        i * 1.f / n_samples,
+        0,
+        animation,
+        sampling_cache,
+        local_matrices,
+        sampled_root_transform);
+
+    ozz::math::Transform calculated_root_transform;
+    calc_bone_translation(ratio, 0, animation, calculated_root_transform);
+
+    std::cout << "ratio: " << ratio << "\t" << "err: " << ozz::math::Length(sampled_root_transform.translation - calculated_root_transform.translation) << std::endl;
+  }
+
+  ozz::math::Transform root_transform_1;
+  calc_bone_translation(0.f, 0, animation, root_transform_0);
+  calc_bone_translation(1.f, 0, animation, root_transform_1);
+}
+
+TEST_CASE("Root Bone Sampling", "[AnimSamplerNode]") {
   SkinnedMesh skinned_mesh;
   skinned_mesh.LoadSkeleton("../media/MixamoYBot-skeleton.ozz");
   int anim_idx = 0;
   skinned_mesh.LoadAnimation("../media/Walking-loop.ozz");
   float walking_markers[] = {0.293, 0.762};
-  skinned_mesh.m_animation_sync_track[anim_idx] = SyncTrack::CreateFromMarkers(skinned_mesh.m_animations[anim_idx]->duration(), 2, walking_markers);
+  skinned_mesh.m_animation_sync_track[anim_idx] = SyncTrack::CreateFromMarkers(
+      skinned_mesh.m_animations[anim_idx]->duration(),
+      2,
+      walking_markers);
 
   AnimationController anim_controller(&skinned_mesh);
   AnimSamplerNode test_node(&anim_controller);
+  test_node.SetAnimation(skinned_mesh.m_animations[0], SyncTrack());
+
+  test_node.Reset();
+  test_node.UpdateTime(0.2f);
+
+  ozz::math::Transform root_transform;
+  //  test_node.Evaluate(&skinned_mesh.m_local_matrices, &root_transform);
 }