From 26459f323146805af5954ef64c3af3e7554ee333 Mon Sep 17 00:00:00 2001 From: AnonymousAcid Date: Tue, 26 Dec 2023 22:47:33 -0800 Subject: [PATCH] Fix dimforge#416 - update ball joint motor pos --- examples3d/spherical_joint_testing.rs | 4 ++-- src/dynamics/joint/multibody_joint/multibody_joint.rs | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/examples3d/spherical_joint_testing.rs b/examples3d/spherical_joint_testing.rs index e5f6acefb..7a601f94d 100644 --- a/examples3d/spherical_joint_testing.rs +++ b/examples3d/spherical_joint_testing.rs @@ -1,11 +1,11 @@ use rapier3d::prelude::*; -use rapier_testbed3d::{Testbed, TestbedApp}; +use rapier_testbed3d::Testbed; pub fn init_world(testbed: &mut Testbed) { let mut bodies = RigidBodySet::new(); let mut colliders = ColliderSet::new(); - let impulse_joints = ImpulseJointSet::new(); + let mut impulse_joints = ImpulseJointSet::new(); let mut multibody_joints = MultibodyJointSet::new(); diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index da650e609..7df2b0a16 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -115,6 +115,9 @@ impl MultibodyJoint { let angvel = Vector3::from_row_slice(&vels[curr_free_dof..curr_free_dof + 3]); let disp = UnitQuaternion::new_eps(angvel * dt, 0.0); self.joint_rot = disp * self.joint_rot; + self.coords[3] += angvel[0] * dt; + self.coords[4] += angvel[1] * dt; + self.coords[5] += angvel[2] * dt; } _ => unreachable!(), }