Fix #416 - update ball joint motor pos

dimforge · Dec 27, 2023 · 26459f3 · 26459f3
1 parent e962839
commit 26459f3
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 2 deletions.
diff --git 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
@@ -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!(),
         }