diff options
Diffstat (limited to 'src/dynamics/solver')
9 files changed, 54 insertions, 54 deletions
diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index 1e65341..0c4c323 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -113,7 +113,7 @@ impl IslandSolver { let mut new_poss = *poss; let new_vels = vels.apply_damping(params.dt, damping); new_poss.next_position = - vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com); + vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com); bodies.set_internal(handle.0, new_vels); bodies.set_internal(handle.0, new_poss); @@ -140,7 +140,7 @@ impl IslandSolver { .integrate(params.dt, vels, mprops) .apply_damping(params.dt, &damping); new_poss.next_position = - vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com); + vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com); bodies.set_internal(handle.0, new_vels); bodies.set_internal(handle.0, new_poss); diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 159cc77..0227960 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -34,8 +34,8 @@ impl BallPositionConstraint { let (mprops2, ids2) = rb2; Self { - local_com1: mprops1.mass_properties.local_com, - local_com2: mprops2.mass_properties.local_com, + local_com1: mprops1.local_mprops.local_com, + local_com2: mprops2.local_mprops.local_com, im1: mprops1.effective_inv_mass, im2: mprops2.effective_inv_mass, ii1: mprops1.effective_world_inv_inertia_sqrt.squared(), @@ -131,7 +131,7 @@ impl BallPositionGroundConstraint { ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor1, position2: ids2.active_set_offset, - local_com2: mprops2.mass_properties.local_com, + local_com2: mprops2.local_mprops.local_com, } } else { Self { @@ -140,7 +140,7 @@ impl BallPositionGroundConstraint { ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor2, position2: ids2.active_set_offset, - local_com2: mprops2.mass_properties.local_com, + local_com2: mprops2.local_mprops.local_com, } } } diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs index 6b00639..ea462ed 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -40,8 +40,8 @@ impl WBallPositionConstraint { let (mprops1, ids1) = rbs1; let (mprops2, ids2) = rbs2; - let local_com1 = Point::from(gather![|ii| mprops1[ii].mass_properties.local_com]); - let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]); + let local_com1 = Point::from(gather![|ii| mprops1[ii].local_mprops.local_com]); + let local_com2 = Point::from(gather![|ii| mprops2[ii].local_mprops.local_com]); let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); let ii1 = AngularInertia::<SimdReal>::from(gather![ @@ -169,7 +169,7 @@ impl WBallPositionGroundConstraint { cparams[ii].local_anchor2 }]); let position2 = gather![|ii| ids2[ii].active_set_offset]; - let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]); + let local_com2 = Point::from(gather![|ii| mprops2[ii].local_mprops.local_com]); Self { anchor1, diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index 239138f..3ab13f7 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,16 +38,16 @@ 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, im2, ii1, ii2, - local_com1: mprops1.mass_properties.local_com, - local_com2: mprops2.mass_properties.local_com, + local_com1: mprops1.local_mprops.local_com, + local_com2: mprops2.local_mprops.local_com, lin_inv_lhs, ang_inv_lhs, } @@ -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,23 +109,23 @@ 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(), - local_com2: mprops2.mass_properties.local_com, + local_com2: mprops2.local_mprops.local_com, impulse: 0.0, } } @@ -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; diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs index 76901b1..a7b5ea0 100644 --- a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs @@ -41,8 +41,8 @@ impl GenericPositionConstraint { im2, ii1, ii2, - local_com1: rb1.mass_properties.local_com, - local_com2: rb2.mass_properties.local_com, + local_com1: rb1.local_mprops.local_com, + local_com2: rb2.local_mprops.local_com, joint: *joint, } } @@ -215,7 +215,7 @@ impl GenericPositionGroundConstraint { position2: rb2.active_set_offset, im2: rb2.effective_inv_mass, ii2: rb2.effective_world_inv_inertia_sqrt.squared(), - local_com2: rb2.mass_properties.local_com, + local_com2: rb2.local_mprops.local_com, joint: *joint, } } diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index 1b3e23a..731d0e2 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -51,8 +51,8 @@ impl RevolutePositionConstraint { ii1, ii2, ang_inv_lhs, - local_com1: mprops1.mass_properties.local_com, - local_com2: mprops2.mass_properties.local_com, + local_com1: mprops1.local_mprops.local_com, + local_com2: mprops2.local_mprops.local_com, local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, local_axis1: cparams.local_axis1, @@ -183,7 +183,7 @@ impl RevolutePositionGroundConstraint { local_anchor2, im2: mprops2.effective_inv_mass, ii2: mprops2.effective_world_inv_inertia_sqrt.squared(), - local_com2: mprops2.mass_properties.local_com, + local_com2: mprops2.local_mprops.local_com, axis1, local_axis2, position2: ids2.active_set_offset, diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index 9d565c1..e12e7af 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -392,7 +392,7 @@ impl ParallelIslandSolver { let new_rb_vels = new_rb_vels.apply_damping(params.dt, rb_damping); new_rb_pos.next_position = - new_rb_vels.integrate(params.dt, &rb_pos.position, &rb_mprops.mass_properties.local_com); + new_rb_vels.integrate(params.dt, &rb_pos.position, &rb_mprops.local_mprops.local_com); bodies.set_internal(handle.0, new_rb_vels); bodies.set_internal(handle.0, new_rb_pos); |