From 554125dde2cfe221596ce6a3fd1a9b71b80e0cdd Mon Sep 17 00:00:00 2001
From: Martin Felis <martin@fysx.org>
Date: Tue, 16 Nov 2021 22:57:58 +0100
Subject: [PATCH] Added LockTranslationNode.

---
 CMakeLists.txt                       |  1 +
 src/AnimNodes/LockTranslationNode.cc | 59 ++++++++++++++++++++++++++++
 src/AnimNodes/LockTranslationNode.h  | 52 ++++++++++++++++++++++++
 src/AnimationController.cc           |  9 ++++-
 4 files changed, 119 insertions(+), 2 deletions(-)
 create mode 100644 src/AnimNodes/LockTranslationNode.cc
 create mode 100644 src/AnimNodes/LockTranslationNode.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5ccb793..27d29a9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -48,6 +48,7 @@ add_library(AnimTestbedCode OBJECT
         src/AnimNodes/AnimSamplerNode.cc
         src/AnimNodes/SpeedScaleNode.cc
         src/AnimNodes/BlendNode.cc
+        src/AnimNodes/LockTranslationNode.cc
         src/AnimationController.cc
         3rdparty/imgui/imgui.cpp
         3rdparty/imgui/imgui_draw.cpp
diff --git a/src/AnimNodes/LockTranslationNode.cc b/src/AnimNodes/LockTranslationNode.cc
new file mode 100644
index 0000000..8b4f049
--- /dev/null
+++ b/src/AnimNodes/LockTranslationNode.cc
@@ -0,0 +1,59 @@
+//
+// Created by martin on 16.11.21.
+//
+
+#include "LockTranslationNode.h"
+
+#include <imgui.h>
+#include "ozz/base/maths/soa_transform.h"
+
+void LockTranslationNode::Evaluate(
+    ozz::vector<ozz::math::SoaTransform>* local_matrices) {
+  m_input->Evaluate(local_matrices);
+  ozz::math::SoaFloat3 translation = (*local_matrices)[m_locked_bone_index].translation;
+  float x[4];
+  float y[4];
+  float z[4];
+  _mm_store_ps(x, translation.x);
+  _mm_store_ps(y, translation.y);
+  _mm_store_ps(z, translation.z);
+
+  if (m_lock_x) {
+    x[0] = 0.f;
+  }
+
+  if (m_lock_y) {
+    y[0] = 0.f;
+  }
+
+  if (m_lock_z) {
+    z[0] = 0.f;
+  }
+
+  translation.x = _mm_load_ps(x);
+  translation.y = _mm_load_ps(y);
+  translation.z = _mm_load_ps(z);
+
+  //translation = ozz::math::SoaFloat3::zero();
+//  ozz::math::SetX(translation, 0.f);
+//  ozz::math::SetZ(translation, 0.f);
+   (*local_matrices)[m_locked_bone_index].translation = translation;
+}
+
+void LockTranslationNode::DrawDebugUi() {
+  const ozz::animation::Skeleton& skeleton = m_animation_controller->m_skinned_mesh->m_skeleton;
+  ozz::span<const char* const> joint_names = skeleton.joint_names();
+
+  const char* items[255] = {0};
+  int item_current = 0;
+  for (int i = 0; i < joint_names.size(); i++) {
+    items[i] = joint_names[i];
+  }
+
+  ImGui::Combo("Bone", &m_locked_bone_index, items, joint_names.size());
+  ImGui::Checkbox("Lock X", &m_lock_x);
+  ImGui::Checkbox("Lock Y", &m_lock_y);
+  ImGui::Checkbox("Lock Z", &m_lock_z);
+
+
+}
\ No newline at end of file
diff --git a/src/AnimNodes/LockTranslationNode.h b/src/AnimNodes/LockTranslationNode.h
new file mode 100644
index 0000000..0d328e9
--- /dev/null
+++ b/src/AnimNodes/LockTranslationNode.h
@@ -0,0 +1,52 @@
+//
+// Created by martin on 16.11.21.
+//
+
+#ifndef ANIMTESTBED_LOCKBONES_H
+#define ANIMTESTBED_LOCKBONES_H
+
+#include "../AnimNode.h"
+
+struct LockTranslationNode : public AnimNode {
+  LockTranslationNode(AnimationController* animation_controller)
+      : AnimNode(animation_controller),
+        m_input(nullptr),
+        m_locked_bone_index(0),
+        m_lock_x(false),
+        m_lock_y(false),
+        m_lock_z(false) {}
+
+  virtual ~LockTranslationNode() {}
+
+  AnimNode* m_input;
+  int m_locked_bone_index;
+  bool m_lock_x;
+  bool m_lock_y;
+  bool m_lock_z;
+
+  virtual void Reset() { m_current_time = 0.f; }
+
+  virtual void UpdateIsSynced(bool is_synced) override {
+    m_is_time_synced = is_synced;
+
+    m_input->UpdateIsSynced(m_is_time_synced);
+  }
+
+  virtual void EvalAnimDuration() override {
+    m_anim_duration = m_input->m_anim_duration;
+  }
+
+  virtual void UpdateTime(float dt) { m_input->UpdateTime(dt); }
+
+  virtual void Evaluate(
+      ozz::vector<ozz::math::SoaTransform>* local_matrices) override;
+
+  virtual void GetInputNodes(
+      std::vector<AnimNode*>& input_nodes) const override {
+    input_nodes.push_back(m_input);
+  };
+
+  virtual void DrawDebugUi() override;
+};
+
+#endif  //ANIMTESTBED_LOCKBONES_H
diff --git a/src/AnimationController.cc b/src/AnimationController.cc
index 34117bb..3a843cb 100644
--- a/src/AnimationController.cc
+++ b/src/AnimationController.cc
@@ -5,7 +5,6 @@
 #include "AnimationController.h"
 
 #include <imgui.h>
-#include <ozz/animation/runtime/blending_job.h>
 #include <ozz/animation/runtime/local_to_model_job.h>
 #include <ozz/animation/runtime/sampling_job.h>
 
@@ -13,6 +12,7 @@
 
 #include "AnimNodes/AnimSamplerNode.h"
 #include "AnimNodes/BlendNode.h"
+#include "AnimNodes/LockTranslationNode.h"
 #include "AnimNodes/SpeedScaleNode.h"
 #include "SkinnedMesh.h"
 
@@ -54,7 +54,12 @@ AnimationController::AnimationController(SkinnedMesh* skinned_mesh)
   speed_node1->m_input_node = blend_node;
   m_anim_nodes.push_back(speed_node1);
 
-  m_output_node = speed_node1;
+  LockTranslationNode* lock_node = new LockTranslationNode(this);
+  lock_node->m_name = "LockNode0";
+  lock_node->m_locked_bone_index = 0;
+  lock_node->m_input = speed_node1;
+
+  m_output_node = lock_node;
 
   UpdateOrderedNodes();