From b015fba2c8a481efe65fe29da10a1be5022d9cd6 Mon Sep 17 00:00:00 2001 From: Aaron Franke Date: Fri, 6 Oct 2023 15:39:46 -0500 Subject: [PATCH] Orthonormalize PhysicalBone3D transforms --- scene/3d/physics_body_3d.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/scene/3d/physics_body_3d.cpp b/scene/3d/physics_body_3d.cpp index ed64c1656400..38c1f232ad88 100644 --- a/scene/3d/physics_body_3d.cpp +++ b/scene/3d/physics_body_3d.cpp @@ -2239,11 +2239,14 @@ void PhysicalBone3D::reset_physics_simulation_state() { void PhysicalBone3D::reset_to_rest_position() { if (parent_skeleton) { - if (-1 == bone_id) { - set_global_transform(parent_skeleton->get_global_transform() * body_offset); + Transform3D new_transform = parent_skeleton->get_global_transform(); + if (bone_id == -1) { + new_transform *= body_offset; } else { - set_global_transform(parent_skeleton->get_global_transform() * parent_skeleton->get_bone_global_pose(bone_id) * body_offset); + new_transform *= parent_skeleton->get_bone_global_pose(bone_id) * body_offset; } + new_transform.orthonormalize(); + set_global_transform(new_transform); } }