From 59796e47670797de29db93e9800f039fe4951654 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Fri, 19 Feb 2021 14:41:11 +0100 Subject: fix --- src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src') 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; -- cgit