diff options
Diffstat (limited to 'src/dynamics/solver')
3 files changed, 35 insertions, 35 deletions
diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index 239138f..f8945c3 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -8,8 +8,8 @@ use crate::utils::WAngularInertia; pub(crate) struct FixedPositionConstraint { position1: usize, position2: usize, - local_anchor1: Isometry<Real>, - local_anchor2: Isometry<Real>, + local_frame1: Isometry<Real>, + local_frame2: Isometry<Real>, local_com1: Point<Real>, local_com2: Point<Real>, im1: Real, @@ -38,8 +38,8 @@ impl FixedPositionConstraint { let ang_inv_lhs = (ii1 + ii2).inverse(); Self { - local_anchor1: cparams.local_anchor1, - local_anchor2: cparams.local_anchor2, + local_frame1: cparams.local_frame1, + local_frame2: cparams.local_frame2, position1: ids1.active_set_offset, position2: ids2.active_set_offset, im1, @@ -58,8 +58,8 @@ impl FixedPositionConstraint { let mut position2 = positions[self.position2 as usize]; // Angular correction. - let anchor1 = position1 * self.local_anchor1; - let anchor2 = position2 * self.local_anchor2; + let anchor1 = position1 * self.local_frame1; + let anchor2 = position2 * self.local_frame2; let ang_err = anchor2.rotation * anchor1.rotation.inverse(); #[cfg(feature = "dim3")] let ang_impulse = self @@ -75,8 +75,8 @@ impl FixedPositionConstraint { Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation; // Linear correction. - let anchor1 = position1 * Point::from(self.local_anchor1.translation.vector); - let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector); + let anchor1 = position1 * Point::from(self.local_frame1.translation.vector); + let anchor2 = position2 * Point::from(self.local_frame2.translation.vector); let err = anchor2 - anchor1; let impulse = err * (self.lin_inv_lhs * params.joint_erp); position1.translation.vector += self.im1 * impulse; @@ -91,7 +91,7 @@ impl FixedPositionConstraint { pub(crate) struct FixedPositionGroundConstraint { position2: usize, anchor1: Isometry<Real>, - local_anchor2: Isometry<Real>, + local_frame2: Isometry<Real>, local_com2: Point<Real>, im2: Real, ii2: AngularInertia<Real>, @@ -109,19 +109,19 @@ impl FixedPositionGroundConstraint { let (mprops2, ids2) = rb2; let anchor1; - let local_anchor2; + let local_frame2; if flipped { - anchor1 = poss1.next_position * cparams.local_anchor2; - local_anchor2 = cparams.local_anchor1; + anchor1 = poss1.next_position * cparams.local_frame2; + local_frame2 = cparams.local_frame1; } else { - anchor1 = poss1.next_position * cparams.local_anchor1; - local_anchor2 = cparams.local_anchor2; + anchor1 = poss1.next_position * cparams.local_frame1; + local_frame2 = cparams.local_frame2; }; Self { anchor1, - local_anchor2, + local_frame2, position2: ids2.active_set_offset, im2: mprops2.effective_inv_mass, ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), @@ -134,13 +134,13 @@ impl FixedPositionGroundConstraint { let mut position2 = positions[self.position2 as usize]; // Angular correction. - let anchor2 = position2 * self.local_anchor2; + let anchor2 = position2 * self.local_frame2; let ang_err = anchor2.rotation * self.anchor1.rotation.inverse(); position2.rotation = ang_err.powf(-params.joint_erp) * position2.rotation; // Linear correction. let anchor1 = Point::from(self.anchor1.translation.vector); - let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector); + let anchor2 = position2 * Point::from(self.local_frame2.translation.vector); let err = anchor2 - anchor1; // NOTE: no need to divide by im2 just to multiply right after. let impulse = err * params.joint_erp; diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index a0c0739..8bfc1a6 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -63,8 +63,8 @@ impl FixedVelocityConstraint { let (poss1, vels1, mprops1, ids1) = rb1; let (poss2, vels2, mprops2, ids2) = rb2; - let anchor1 = poss1.position * cparams.local_anchor1; - let anchor2 = poss2.position * cparams.local_anchor2; + let anchor1 = poss1.position * cparams.local_frame1; + let anchor2 = poss2.position * cparams.local_frame2; let im1 = mprops1.effective_inv_mass; let im2 = mprops2.effective_inv_mass; let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared(); @@ -280,13 +280,13 @@ impl FixedVelocityGroundConstraint { let (anchor1, anchor2) = if flipped { ( - poss1.position * cparams.local_anchor2, - poss2.position * cparams.local_anchor1, + poss1.position * cparams.local_frame2, + poss2.position * cparams.local_frame1, ) } else { ( - poss1.position * cparams.local_anchor1, - poss2.position * cparams.local_anchor2, + poss1.position * cparams.local_frame1, + poss2.position * cparams.local_frame2, ) }; diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs index 84e1fca..0421d49 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -91,12 +91,12 @@ impl WFixedVelocityConstraint { ]); let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; - let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]); - let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]); + let local_frame1 = Isometry::from(gather![|ii| cparams[ii].local_frame1]); + let local_frame2 = Isometry::from(gather![|ii| cparams[ii].local_frame2]); let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]); - let anchor1 = position1 * local_anchor1; - let anchor2 = position2 * local_anchor2; + let anchor1 = position1 * local_frame1; + let anchor2 = position2 * local_frame2; let ii1 = ii1_sqrt.squared(); let ii2 = ii2_sqrt.squared(); let r1 = anchor1.translation.vector - world_com1.coords; @@ -363,20 +363,20 @@ impl WFixedVelocityGroundConstraint { ]); let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset]; - let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] { - cparams[ii].local_anchor2 + let local_frame1 = Isometry::from(gather![|ii| if flipped[ii] { + cparams[ii].local_frame2 } else { - cparams[ii].local_anchor1 + cparams[ii].local_frame1 }]); - let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] { - cparams[ii].local_anchor1 + let local_frame2 = Isometry::from(gather![|ii| if flipped[ii] { + cparams[ii].local_frame1 } else { - cparams[ii].local_anchor2 + cparams[ii].local_frame2 }]); let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]); - let anchor1 = position1 * local_anchor1; - let anchor2 = position2 * local_anchor2; + let anchor1 = position1 * local_frame1; + let anchor2 = position2 * local_frame2; let ii2 = ii2_sqrt.squared(); let r1 = anchor1.translation.vector - world_com1.coords; let r2 = anchor2.translation.vector - world_com2.coords; |