diff options
| -rw-r--r-- | src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs | 8 |
1 files changed, 4 insertions, 4 deletions
diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index a169687..5782d86 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -113,18 +113,18 @@ impl FixedVelocityConstraint { let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); if velocity_based_erp_inv_dt != 0.0 { - let error = anchor2 * anchor1.inverse(); - let lin_err = error.translation.vector; + let lin_err = anchor2.translation.vector - anchor1.translation.vector; + let ang_err = anchor2.rotation * anchor1.rotation.inverse(); #[cfg(feature = "dim2")] { - let ang_err = error.rotation.angle(); + let ang_err = ang_err.angle(); rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt; } #[cfg(feature = "dim3")] { - let ang_err = error.rotation.scaled_axis(); + let ang_err = ang_err.scaled_axis(); rhs += Vector6::new( lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, ) * velocity_based_erp_inv_dt; |