Add prelude + use vectors for setting linvel/translation in builders

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;