Fix dimforge#416 - update ball joint motor pos

2 files changed, 5 insertions, 2 deletions

diff --git a/examples3d/spherical_joint_testing.rs b/examples3d/spherical_joint_testing.rs
index e5f6ace..7a601f9 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 62fc434..11ea890 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!(),
         }