diff options
Diffstat (limited to 'src/dynamics/solver/velocity_constraint_element.rs')
| -rw-r--r-- | src/dynamics/solver/velocity_constraint_element.rs | 28 |
1 files changed, 14 insertions, 14 deletions
diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs index 406f68e..e153626 100644 --- a/src/dynamics/solver/velocity_constraint_element.rs +++ b/src/dynamics/solver/velocity_constraint_element.rs @@ -30,8 +30,8 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> { pub fn solve( &mut self, tangents1: [&Vector<N>; DIM - 1], - im1: N, - im2: N, + im1: &Vector<N>, + im2: &Vector<N>, limit: N, mj_lambda1: &mut DeltaVel<N>, mj_lambda2: &mut DeltaVel<N>, @@ -50,10 +50,10 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> { let dlambda = new_impulse - self.impulse[0]; self.impulse[0] = new_impulse; - mj_lambda1.linear += tangents1[0] * (im1 * dlambda); + mj_lambda1.linear += tangents1[0].component_mul(im1) * dlambda; mj_lambda1.angular += self.gcross1[0] * dlambda; - mj_lambda2.linear += tangents1[0] * (-im2 * dlambda); + mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda; mj_lambda2.angular += self.gcross2[0] * dlambda; } @@ -84,12 +84,12 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> { let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; - mj_lambda1.linear += - tangents1[0] * (im1 * dlambda[0]) + tangents1[1] * (im1 * dlambda[1]); + mj_lambda1.linear += tangents1[0].component_mul(im1) * dlambda[0] + + tangents1[1].component_mul(im1) * dlambda[1]; mj_lambda1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1]; - mj_lambda2.linear += - tangents1[0] * (-im2 * dlambda[0]) + tangents1[1] * (-im2 * dlambda[1]); + mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0] + + tangents1[1].component_mul(im2) * -dlambda[1]; mj_lambda2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1]; } } @@ -121,8 +121,8 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> { pub fn solve( &mut self, dir1: &Vector<N>, - im1: N, - im2: N, + im1: &Vector<N>, + im2: &Vector<N>, mj_lambda1: &mut DeltaVel<N>, mj_lambda2: &mut DeltaVel<N>, ) where @@ -136,10 +136,10 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> { let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; - mj_lambda1.linear += dir1 * (im1 * dlambda); + mj_lambda1.linear += dir1.component_mul(im1) * dlambda; mj_lambda1.angular += self.gcross1 * dlambda; - mj_lambda2.linear += dir1 * (-im2 * dlambda); + mj_lambda2.linear += dir1.component_mul(im2) * -dlambda; mj_lambda2.angular += self.gcross2 * dlambda; } } @@ -163,8 +163,8 @@ impl<N: SimdRealField + Copy> VelocityConstraintElement<N> { elements: &mut [Self], dir1: &Vector<N>, #[cfg(feature = "dim3")] tangent1: &Vector<N>, - im1: N, - im2: N, + im1: &Vector<N>, + im2: &Vector<N>, limit: N, mj_lambda1: &mut DeltaVel<N>, mj_lambda2: &mut DeltaVel<N>, |