Add damped skeleton modifier

This could be used as a placeholder for blending

Author: GuilhermeGSousa

SConstruct

@@ -28,6 +28,7 @@ sources = Glob("src/*.cpp")
 sources += Glob("src/editor/*.cpp")
 sources += Glob("src/features/*.cpp")
 sources += Glob("src/math/*.cpp")
+sources += Glob("src/modifiers/*.cpp")
 sources += Glob("src/synchronizers/*.cpp")
 
 if env["platform"] == "macos":

demo/scenes/rin.tscn

@@ -539,6 +539,8 @@ bones/294/scale = Vector3(1, 1, 1)
 bones/296/position = Vector3(0.0011635, 0.0935117, 5.08575e-11)
 bones/296/rotation = Quaternion(0, 1, 6.23271e-17, 7.54979e-08)
 
+[node name="DampedSkeletonModifier" type="DampedSkeletonModifier" parent="rin/Armature/Skeleton3D" index="25"]
+
 [node name="AnimationPlayer" type="AnimationPlayer" parent="."]
 root_node = NodePath("../rin")
 root_motion_track = NodePath("Armature/Skeleton3D:root")

src/modifiers/damped_skeleton_modifier.cpp

@@ -0,0 +1,61 @@
+#include "modifiers/damped_skeleton_modifier.h"
+
+#include "damped_skeleton_modifier.h"
+#include "math/spring.hpp"
+
+void DampedSkeletonModifier::_ready() {
+
+    Skeleton3D* skeleton = get_skeleton();
+
+    if (skeleton) {
+        _skeleton_state = SkeletonState(skeleton);
+        for (int b = 0; b < skeleton->get_bone_count(); ++b) {
+            BoneState& bone = _skeleton_state[b];
+            bone.pos = skeleton->get_bone_pose_position(b);
+            bone.rot = skeleton->get_bone_pose_rotation(b);
+            bone.scl = skeleton->get_bone_pose_scale(b);
+        }
+    }
+}
+
+void DampedSkeletonModifier::_process_modification() {
+    if (!is_active()) {
+        return;
+    }
+
+    Skeleton3D* skeleton = get_skeleton();
+    const float delta =
+        skeleton->get_modifier_callback_mode_process() == Skeleton3D::ModifierCallbackModeProcess::MODIFIER_CALLBACK_MODE_PROCESS_IDLE ? get_process_delta_time() : get_physics_process_delta_time();
+
+    for (auto bone_id = 0; bone_id < skeleton->get_bone_count(); ++bone_id) {
+
+        if (skeleton->get_bone_parent(bone_id) == -1) {
+            // Skip root bone
+            continue;
+        }
+
+        const Vector3 desired_pos = skeleton->get_bone_pose_position(bone_id);
+        const Quaternion desired_rot = skeleton->get_bone_pose_rotation(bone_id);
+
+        Spring::_simple_spring_damper_exact(
+            _skeleton_state[bone_id].pos,
+            _skeleton_state[bone_id].vel,
+            desired_pos,
+            halflife,
+            delta);
+
+        Spring::_simple_spring_damper_exact(
+            _skeleton_state[bone_id].rot,
+            _skeleton_state[bone_id].ang_vel,
+            desired_rot,
+            halflife,
+            delta);
+
+        skeleton->set_bone_pose_position(bone_id, _skeleton_state[bone_id].pos);
+        skeleton->set_bone_pose_rotation(bone_id, _skeleton_state[bone_id].rot);
+    }
+}
+
+void DampedSkeletonModifier::_bind_methods() {
+    BINDER_PROPERTY_PARAMS(DampedSkeletonModifier, Variant::FLOAT, halflife);
+}

src/modifiers/damped_skeleton_modifier.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include "common.h"
+#include "mm_bone_state.h"
+
+#include <godot_cpp/classes/skeleton_modifier3d.hpp>
+
+using namespace godot;
+
+class DampedSkeletonModifier : public SkeletonModifier3D {
+    GDCLASS(DampedSkeletonModifier, SkeletonModifier3D)
+
+public:
+    virtual void _ready() override;
+    virtual void _process_modification() override;
+
+    GETSET(float, halflife, 0.1);
+
+protected:
+    static void _bind_methods();
+
+private:
+    SkeletonState _skeleton_state;
+};

src/register_types.cpp

@@ -17,6 +17,8 @@
 #include "features/mm_feature.h"
 #include "features/mm_trajectory_feature.h"
 
+#include "modifiers/damped_skeleton_modifier.h"
+
 #include "synchronizers/mm_clamp_synchronizer.h"
 #include "synchronizers/mm_rootmotion_synchronizer.h"
 #include "synchronizers/mm_synchronizer.h"
@@ -44,6 +46,8 @@ void initialize_example_module(ModuleInitializationLevel p_level) {
 
         ClassDB::register_class<MMCharacter>();
 
+        ClassDB::register_class<DampedSkeletonModifier>();
+
         ClassDB::register_abstract_class<MMSynchronizer>();
         ClassDB::register_class<MMClampSynchronizer>();
         ClassDB::register_class<MMRootMotionSynchronizer>();