From 2ed34bacf4c32cea5fa419a604b685822392c77a Mon Sep 17 00:00:00 2001 From: AnonymousAcid Date: Tue, 26 Dec 2023 22:47:33 -0800 Subject: Fix dimforge#416 - update ball joint motor pos --- src/dynamics/joint/multibody_joint/multibody_joint.rs | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src') 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!(), } -- cgit