From 48afbac6cefbcf1ad6ab663ce15c503bb549cb69 Mon Sep 17 00:00:00 2001 From: Emil Ernerfeldt Date: Thu, 18 Feb 2021 18:18:47 +0100 Subject: Implement revolute wide --- src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs | 1 + 1 file changed, 1 insertion(+) (limited to 'src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs') diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 46f375b..15869e2 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -107,6 +107,7 @@ impl RevoluteVelocityConstraint { let axis1 = rb1.position * joint.local_axis1; let axis2 = rb2.position * joint.local_axis2; + let ang_err = Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); let ang_err = ang_err.scaled_axis(); -- cgit