From 97157c9423f3360c5e941b4065377689221014ae Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Fri, 26 Mar 2021 18:16:27 +0100 Subject: First working version of non-linear CCD based on single-substep motion-clamping. --- .../joint_constraint/revolute_position_constraint.rs | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) (limited to 'src/dynamics/solver/joint_constraint/revolute_position_constraint.rs') diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index 2da49e6..96391a2 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -145,23 +145,23 @@ impl RevolutePositionGroundConstraint { let local_basis2; if flipped { - anchor1 = rb1.predicted_position * cparams.local_anchor2; + anchor1 = rb1.next_position * cparams.local_anchor2; local_anchor2 = cparams.local_anchor1; - axis1 = rb1.predicted_position * cparams.local_axis2; + axis1 = rb1.next_position * cparams.local_axis2; local_axis2 = cparams.local_axis1; basis1 = [ - rb1.predicted_position * cparams.basis2[0], - rb1.predicted_position * cparams.basis2[1], + rb1.next_position * cparams.basis2[0], + rb1.next_position * cparams.basis2[1], ]; local_basis2 = cparams.basis1; } else { - anchor1 = rb1.predicted_position * cparams.local_anchor1; + anchor1 = rb1.next_position * cparams.local_anchor1; local_anchor2 = cparams.local_anchor2; - axis1 = rb1.predicted_position * cparams.local_axis1; + axis1 = rb1.next_position * cparams.local_axis1; local_axis2 = cparams.local_axis2; basis1 = [ - rb1.predicted_position * cparams.basis1[0], - rb1.predicted_position * cparams.basis1[1], + rb1.next_position * cparams.basis1[0], + rb1.next_position * cparams.basis1[1], ]; local_basis2 = cparams.basis2; }; -- cgit