From 8f330b2a00610e5b68c1acd9208120e8f750c7aa Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 21 Jan 2021 14:58:40 +0100 Subject: Rotation locking: apply filter only to the world inertia properties to fix the multi-collider case. --- src/dynamics/rigid_body.rs | 194 +++++++++------------ .../joint_constraint/ball_position_constraint.rs | 16 +- .../ball_position_constraint_wide.rs | 12 +- .../joint_constraint/ball_velocity_constraint.rs | 24 +-- .../ball_velocity_constraint_wide.rs | 12 +- .../joint_constraint/fixed_position_constraint.rs | 12 +- .../joint_constraint/fixed_velocity_constraint.rs | 18 +- .../fixed_velocity_constraint_wide.rs | 12 +- .../prismatic_position_constraint.rs | 8 +- .../prismatic_velocity_constraint.rs | 18 +- .../prismatic_velocity_constraint_wide.rs | 12 +- .../revolute_position_constraint.rs | 8 +- .../revolute_velocity_constraint.rs | 18 +- .../revolute_velocity_constraint_wide.rs | 12 +- src/dynamics/solver/parallel_island_solver.rs | 2 +- src/dynamics/solver/position_constraint.rs | 8 +- src/dynamics/solver/position_constraint_wide.rs | 14 +- src/dynamics/solver/position_ground_constraint.rs | 4 +- .../solver/position_ground_constraint_wide.rs | 7 +- src/dynamics/solver/velocity_constraint.rs | 24 +-- src/dynamics/solver/velocity_constraint_wide.rs | 14 +- src/dynamics/solver/velocity_ground_constraint.rs | 12 +- .../solver/velocity_ground_constraint_wide.rs | 7 +- src/dynamics/solver/velocity_solver.rs | 4 +- 24 files changed, 229 insertions(+), 243 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 20d04b0..9279e4f 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -31,10 +31,10 @@ bitflags::bitflags! { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// Flags affecting the behavior of the constraints solver for a given contact manifold. pub(crate) struct RigidBodyFlags: u8 { - const IGNORE_COLLIDER_MASS = 1 << 0; - const IGNORE_COLLIDER_ANGULAR_INERTIA_X = 1 << 1; - const IGNORE_COLLIDER_ANGULAR_INERTIA_Y = 1 << 2; - const IGNORE_COLLIDER_ANGULAR_INERTIA_Z = 1 << 3; + const TRANSLATION_LOCKED = 1 << 0; + const ROTATION_LOCKED_X = 1 << 1; + const ROTATION_LOCKED_Y = 1 << 2; + const ROTATION_LOCKED_Z = 1 << 3; } } @@ -62,8 +62,9 @@ pub struct RigidBody { pub(crate) mass_properties: MassProperties, /// The world-space center of mass of the rigid-body. pub world_com: Point, + pub effective_inv_mass: Real, /// The square-root of the inverse angular inertia tensor of the rigid-body. - pub world_inv_inertia_sqrt: AngularInertia, + pub effective_world_inv_inertia_sqrt: AngularInertia, /// The linear velocity of the rigid-body. pub(crate) linvel: Vector, /// The angular velocity of the rigid-body. @@ -98,7 +99,8 @@ impl RigidBody { predicted_position: Isometry::identity(), mass_properties: MassProperties::zero(), world_com: Point::origin(), - world_inv_inertia_sqrt: AngularInertia::zero(), + effective_inv_mass: 0.0, + effective_world_inv_inertia_sqrt: AngularInertia::zero(), linvel: Vector::zeros(), angvel: na::zero(), linacc: Vector::zeros(), @@ -130,14 +132,13 @@ impl RigidBody { } pub(crate) fn integrate_accelerations(&mut self, dt: Real, gravity: Vector) { - if self.mass_properties.inv_mass != 0.0 { + if self.effective_inv_mass != 0.0 { self.linvel += (gravity * self.gravity_scale + self.linacc) * dt; - self.angvel += self.angacc * dt; - - // Reset the accelerations. self.linacc = na::zero(); - self.angacc = na::zero(); } + + self.angvel += self.angacc * dt; + self.angacc = na::zero(); } /// The mass properties of this rigid-body. @@ -227,40 +228,10 @@ impl RigidBody { .mass_properties() .transform_by(coll.position_wrt_parent()); self.colliders.push(handle); - self.mass_properties += Self::filter_collider_mass_props(mass_properties, self.flags); + self.mass_properties += mass_properties; self.update_world_mass_properties(); } - fn filter_collider_mass_props( - mut props: MassProperties, - flags: RigidBodyFlags, - ) -> MassProperties { - if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_MASS) { - props.inv_mass = 0.0; - } - - #[cfg(feature = "dim2")] - { - if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z) { - props.inv_principal_inertia_sqrt = 0.0; - } - } - #[cfg(feature = "dim3")] - { - if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X) { - props.inv_principal_inertia_sqrt.x = 0.0; - } - if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y) { - props.inv_principal_inertia_sqrt.y = 0.0; - } - if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z) { - props.inv_principal_inertia_sqrt.z = 0.0; - } - } - - props - } - pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) { for handle in &self.colliders { let collider = &mut colliders[*handle]; @@ -277,7 +248,7 @@ impl RigidBody { let mass_properties = coll .mass_properties() .transform_by(coll.position_wrt_parent()); - self.mass_properties -= Self::filter_collider_mass_props(mass_properties, self.flags); + self.mass_properties -= mass_properties; self.update_world_mass_properties(); } } @@ -458,9 +429,41 @@ impl RigidBody { pub(crate) fn update_world_mass_properties(&mut self) { self.world_com = self.mass_properties.world_com(&self.position); - self.world_inv_inertia_sqrt = self + self.effective_inv_mass = self.mass_properties.inv_mass; + self.effective_world_inv_inertia_sqrt = self .mass_properties .world_inv_inertia_sqrt(&self.position.rotation); + + // Take into account translation/rotation locking. + if self.flags.contains(RigidBodyFlags::TRANSLATION_LOCKED) { + self.effective_inv_mass = 0.0; + } + + #[cfg(feature = "dim2")] + { + if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z) { + self.effective_world_inv_inertia_sqrt = 0.0; + } + } + #[cfg(feature = "dim3")] + { + if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_X) { + self.effective_world_inv_inertia_sqrt.m11 = 0.0; + self.effective_world_inv_inertia_sqrt.m12 = 0.0; + self.effective_world_inv_inertia_sqrt.m13 = 0.0; + } + + if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Y) { + self.effective_world_inv_inertia_sqrt.m22 = 0.0; + self.effective_world_inv_inertia_sqrt.m12 = 0.0; + self.effective_world_inv_inertia_sqrt.m23 = 0.0; + } + if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z) { + self.effective_world_inv_inertia_sqrt.m33 = 0.0; + self.effective_world_inv_inertia_sqrt.m13 = 0.0; + self.effective_world_inv_inertia_sqrt.m23 = 0.0; + } + } } /* @@ -469,7 +472,7 @@ impl RigidBody { /// Applies a force at the center-of-mass of this rigid-body. pub fn apply_force(&mut self, force: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { - self.linacc += force * self.mass_properties.inv_mass; + self.linacc += force * self.effective_inv_mass; if wake_up { self.wake_up(true); @@ -480,7 +483,7 @@ impl RigidBody { /// Applies an impulse at the center-of-mass of this rigid-body. pub fn apply_impulse(&mut self, impulse: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { - self.linvel += impulse * self.mass_properties.inv_mass; + self.linvel += impulse * self.effective_inv_mass; if wake_up { self.wake_up(true); @@ -492,7 +495,8 @@ impl RigidBody { #[cfg(feature = "dim2")] pub fn apply_torque(&mut self, torque: Real, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { - self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque); + self.angacc += self.effective_world_inv_inertia_sqrt + * (self.effective_world_inv_inertia_sqrt * torque); if wake_up { self.wake_up(true); @@ -504,7 +508,8 @@ impl RigidBody { #[cfg(feature = "dim3")] pub fn apply_torque(&mut self, torque: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { - self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque); + self.angacc += self.effective_world_inv_inertia_sqrt + * (self.effective_world_inv_inertia_sqrt * torque); if wake_up { self.wake_up(true); @@ -516,8 +521,8 @@ impl RigidBody { #[cfg(feature = "dim2")] pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { - self.angvel += - self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse); + self.angvel += self.effective_world_inv_inertia_sqrt + * (self.effective_world_inv_inertia_sqrt * torque_impulse); if wake_up { self.wake_up(true); @@ -529,8 +534,8 @@ impl RigidBody { #[cfg(feature = "dim3")] pub fn apply_torque_impulse(&mut self, torque_impulse: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { - self.angvel += - self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse); + self.angvel += self.effective_world_inv_inertia_sqrt + * (self.effective_world_inv_inertia_sqrt * torque_impulse); if wake_up { self.wake_up(true); @@ -679,16 +684,28 @@ impl RigidBodyBuilder { } /// Prevents this rigid-body from rotating because of forces. - /// - /// This is equivalent to `self.principal_inertia(0.0, false)` (in 2D) or - /// `self.principal_inertia(Vector3::zeros(), Vector3::repeat(false))` (in 3D). - /// - /// See the documentation of [`RigidBodyBuilder::principal_inertia`] for more details. - pub fn lock_rotations(self) -> Self { - #[cfg(feature = "dim2")] - return self.principal_angular_inertia(0.0, false); - #[cfg(feature = "dim3")] - return self.principal_angular_inertia(Vector::zeros(), Vector::repeat(false)); + pub fn lock_rotations(mut self) -> Self { + self.flags.set(RigidBodyFlags::ROTATION_LOCKED_X, true); + self.flags.set(RigidBodyFlags::ROTATION_LOCKED_Y, true); + self.flags.set(RigidBodyFlags::ROTATION_LOCKED_Z, true); + self + } + + /// Only allow rotations of this rigid-body around specific coordinate axes. + #[cfg(feature = "dim3")] + pub fn restrict_rotations( + mut self, + allow_rotations_x: bool, + allow_rotations_y: bool, + allow_rotations_z: bool, + ) -> Self { + self.flags + .set(RigidBodyFlags::ROTATION_LOCKED_X, !allow_rotations_x); + self.flags + .set(RigidBodyFlags::ROTATION_LOCKED_Y, !allow_rotations_y); + self.flags + .set(RigidBodyFlags::ROTATION_LOCKED_Z, !allow_rotations_z); + self } /// Sets the mass of the rigid-body being built. @@ -705,42 +722,23 @@ impl RigidBodyBuilder { pub fn mass(mut self, mass: Real, colliders_contribution_enabled: bool) -> Self { self.mass_properties.inv_mass = utils::inv(mass); self.flags.set( - RigidBodyFlags::IGNORE_COLLIDER_MASS, + RigidBodyFlags::TRANSLATION_LOCKED, !colliders_contribution_enabled, ); self } /// Sets the angular inertia of this rigid-body. - /// - /// In order to lock the rotations of this rigid-body (by - /// making them kinematic), call `.principal_inertia(0.0, false)`. - /// - /// If `colliders_contribution_enabled` is `false`, then the principal inertia specified here - /// will be the final principal inertia of the rigid-body created by this builder. - /// If `colliders_contribution_enabled` is `true`, then the final principal of the rigid-body - /// will depend on the initial principal inertia set by this method to which is added - /// the contributions of all the colliders with non-zero density attached to this rigid-body. #[cfg(feature = "dim2")] - pub fn principal_angular_inertia( - mut self, - inertia: Real, - colliders_contribution_enabled: bool, - ) -> Self { + pub fn principal_angular_inertia(mut self, inertia: Real) -> Self { self.mass_properties.inv_principal_inertia_sqrt = utils::inv(inertia); - self.flags.set( - RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X - | RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y - | RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z, - !colliders_contribution_enabled, - ); self } /// Use `self.principal_angular_inertia` instead. #[cfg(feature = "dim2")] #[deprecated(note = "renamed to `principal_angular_inertia`.")] - pub fn principal_inertia(self, inertia: Real, colliders_contribution_enabled: bool) -> Self { - self.principal_angular_inertia(inertia, colliders_contribution_enabled) + pub fn principal_inertia(self, inertia: Real) -> Self { + self.principal_angular_inertia(inertia) } /// Sets the principal angular inertia of this rigid-body. @@ -756,36 +754,16 @@ impl RigidBodyBuilder { /// to which is added the contributions of all the colliders with non-zero density /// attached to this rigid-body. #[cfg(feature = "dim3")] - pub fn principal_angular_inertia( - mut self, - inertia: AngVector, - colliders_contribution_enabled: AngVector, - ) -> Self { + pub fn principal_angular_inertia(mut self, inertia: AngVector) -> Self { self.mass_properties.inv_principal_inertia_sqrt = inertia.map(utils::inv); - self.flags.set( - RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X, - !colliders_contribution_enabled.x, - ); - self.flags.set( - RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y, - !colliders_contribution_enabled.y, - ); - self.flags.set( - RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z, - !colliders_contribution_enabled.z, - ); self } /// Use `self.principal_angular_inertia` instead. #[cfg(feature = "dim3")] #[deprecated(note = "renamed to `principal_angular_inertia`.")] - pub fn principal_inertia( - self, - inertia: AngVector, - colliders_contribution_enabled: AngVector, - ) -> Self { - self.principal_angular_inertia(inertia, colliders_contribution_enabled) + pub fn principal_inertia(self, inertia: AngVector) -> Self { + self.principal_angular_inertia(inertia) } /// Sets the damping factor for the linear part of the rigid-body motion. diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 8bc9072..744c00d 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -27,10 +27,10 @@ impl BallPositionConstraint { Self { local_com1: rb1.mass_properties.local_com, local_com2: rb2.mass_properties.local_com, - im1: rb1.mass_properties.inv_mass, - im2: rb2.mass_properties.inv_mass, - ii1: rb1.world_inv_inertia_sqrt.squared(), - ii2: rb2.world_inv_inertia_sqrt.squared(), + im1: rb1.effective_inv_mass, + im2: rb2.effective_inv_mass, + ii1: rb1.effective_world_inv_inertia_sqrt.squared(), + ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_anchor1: cparams.local_anchor1, local_anchor2: cparams.local_anchor2, position1: rb1.active_set_offset, @@ -115,8 +115,8 @@ impl BallPositionGroundConstraint { // already been flipped by the caller. Self { anchor1: rb1.predicted_position * cparams.local_anchor2, - im2: rb2.mass_properties.inv_mass, - ii2: rb2.world_inv_inertia_sqrt.squared(), + im2: rb2.effective_inv_mass, + ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor1, position2: rb2.active_set_offset, local_com2: rb2.mass_properties.local_com, @@ -124,8 +124,8 @@ impl BallPositionGroundConstraint { } else { Self { anchor1: rb1.predicted_position * cparams.local_anchor1, - im2: rb2.mass_properties.inv_mass, - ii2: rb2.world_inv_inertia_sqrt.squared(), + im2: rb2.effective_inv_mass, + ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor2, position2: rb2.active_set_offset, local_com2: rb2.mass_properties.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 2f2ffc3..043eea7 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -31,14 +31,14 @@ impl WBallPositionConstraint { ) -> Self { let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]); let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); - let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); let ii1 = AngularInertia::::from( - array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ) .squared(); let ii2 = AngularInertia::::from( - array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ) .squared(); let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); @@ -141,9 +141,9 @@ impl WBallPositionGroundConstraint { } else { cparams[ii].local_anchor1 }; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); let ii2 = AngularInertia::::from( - array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ) .squared(); let local_anchor2 = Point::from(array![|ii| if flipped[ii] { diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index 75e19d3..e75f978 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -40,8 +40,8 @@ impl BallVelocityConstraint { let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); - let im1 = rb1.mass_properties.inv_mass; - let im2 = rb2.mass_properties.inv_mass; + let im1 = rb1.effective_inv_mass; + let im2 = rb2.effective_inv_mass; let rhs = -(vel1 - vel2); let lhs; @@ -52,12 +52,12 @@ impl BallVelocityConstraint { #[cfg(feature = "dim3")] { lhs = rb2 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat2) .add_diagonal(im2) + rb1 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat1) .add_diagonal(im1); @@ -67,8 +67,8 @@ impl BallVelocityConstraint { // it's just easier that way. #[cfg(feature = "dim2")] { - let ii1 = rb1.world_inv_inertia_sqrt.squared(); - let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2; let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2; let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2; @@ -88,8 +88,8 @@ impl BallVelocityConstraint { r2: anchor2, rhs, inv_lhs, - ii1_sqrt: rb1.world_inv_inertia_sqrt, - ii2_sqrt: rb2.world_inv_inertia_sqrt, + ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, } } @@ -170,7 +170,7 @@ impl BallVelocityGroundConstraint { ) }; - let im2 = rb2.mass_properties.inv_mass; + let im2 = rb2.effective_inv_mass; let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); let rhs = vel2 - vel1; @@ -182,7 +182,7 @@ impl BallVelocityGroundConstraint { #[cfg(feature = "dim3")] { lhs = rb2 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat2) .add_diagonal(im2); @@ -190,7 +190,7 @@ impl BallVelocityGroundConstraint { #[cfg(feature = "dim2")] { - let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let m11 = im2 + cmat2.x * cmat2.x * ii2; let m12 = cmat2.x * cmat2.y * ii2; let m22 = im2 + cmat2.y * cmat2.y * ii2; @@ -207,7 +207,7 @@ impl BallVelocityGroundConstraint { r2: anchor2, rhs, inv_lhs, - ii2_sqrt: rb2.world_inv_inertia_sqrt, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, } } diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs index 697221b..95b0bb5 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -42,9 +42,9 @@ impl WBallVelocityConstraint { let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); let ii1_sqrt = AngularInertia::::from( - array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; @@ -52,9 +52,9 @@ impl WBallVelocityConstraint { let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -228,9 +228,9 @@ impl WBallVelocityGroundConstraint { let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index cf9dcb7..7e8fc97 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -21,10 +21,10 @@ pub(crate) struct FixedPositionConstraint { impl FixedPositionConstraint { pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self { - let ii1 = rb1.world_inv_inertia_sqrt.squared(); - let ii2 = rb2.world_inv_inertia_sqrt.squared(); - let im1 = rb1.mass_properties.inv_mass; - let im2 = rb2.mass_properties.inv_mass; + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let im1 = rb1.effective_inv_mass; + let im2 = rb2.effective_inv_mass; let lin_inv_lhs = 1.0 / (im1 + im2); let ang_inv_lhs = (ii1 + ii2).inverse(); @@ -111,8 +111,8 @@ impl FixedPositionGroundConstraint { anchor1, local_anchor2, position2: rb2.active_set_offset, - im2: rb2.mass_properties.inv_mass, - ii2: rb2.world_inv_inertia_sqrt.squared(), + im2: rb2.effective_inv_mass, + ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_com2: rb2.mass_properties.local_com, impulse: 0.0, } diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs index 715b7dd..2868d4b 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -51,10 +51,10 @@ impl FixedVelocityConstraint { ) -> Self { let anchor1 = rb1.position * cparams.local_anchor1; let anchor2 = rb2.position * cparams.local_anchor2; - let im1 = rb1.mass_properties.inv_mass; - let im2 = rb2.mass_properties.inv_mass; - let ii1 = rb1.world_inv_inertia_sqrt.squared(); - let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let im1 = rb1.effective_inv_mass; + let im2 = rb2.effective_inv_mass; + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1.translation.vector - rb1.world_com.coords; let r2 = anchor2.translation.vector - rb2.world_com.coords; let rmat1 = r1.gcross_matrix(); @@ -118,8 +118,8 @@ impl FixedVelocityConstraint { im2, ii1, ii2, - ii1_sqrt: rb1.world_inv_inertia_sqrt, - ii2_sqrt: rb2.world_inv_inertia_sqrt, + ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, impulse: cparams.impulse * params.warmstart_coeff, inv_lhs, r1, @@ -248,8 +248,8 @@ impl FixedVelocityGroundConstraint { let r1 = anchor1.translation.vector - rb1.world_com.coords; - let im2 = rb2.mass_properties.inv_mass; - let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let im2 = rb2.effective_inv_mass; + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let r2 = anchor2.translation.vector - rb2.world_com.coords; let rmat2 = r2.gcross_matrix(); @@ -304,7 +304,7 @@ impl FixedVelocityGroundConstraint { mj_lambda2: rb2.active_set_offset, im2, ii2, - ii2_sqrt: rb2.world_inv_inertia_sqrt, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, impulse: cparams.impulse * params.warmstart_coeff, inv_lhs, r2, 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 48a6cba..a8d7e91 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -61,9 +61,9 @@ impl WFixedVelocityConstraint { let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); let ii1_sqrt = AngularInertia::::from( - array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; @@ -71,9 +71,9 @@ impl WFixedVelocityConstraint { let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -315,9 +315,9 @@ impl WFixedVelocityGroundConstraint { let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs index 2616f6b..ea7e927 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs @@ -28,10 +28,10 @@ pub(crate) struct PrismaticPositionConstraint { impl PrismaticPositionConstraint { pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &PrismaticJoint) -> Self { - let ii1 = rb1.world_inv_inertia_sqrt.squared(); - let ii2 = rb2.world_inv_inertia_sqrt.squared(); - let im1 = rb1.mass_properties.inv_mass; - let im2 = rb2.mass_properties.inv_mass; + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let im1 = rb1.effective_inv_mass; + let im2 = rb2.effective_inv_mass; let lin_inv_lhs = 1.0 / (im1 + im2); let ang_inv_lhs = (ii1 + ii2).inverse(); diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index 78dc1b5..9b82ea5 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -92,13 +92,13 @@ impl PrismaticVelocityConstraint { // simplifications of the computation without introducing // much instabilities. - let im1 = rb1.mass_properties.inv_mass; - let ii1 = rb1.world_inv_inertia_sqrt.squared(); + let im1 = rb1.effective_inv_mass; + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1 - rb1.world_com; let r1_mat = r1.gcross_matrix(); - let im2 = rb2.mass_properties.inv_mass; - let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let im2 = rb2.effective_inv_mass; + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let r2 = anchor2 - rb2.world_com; let r2_mat = r2.gcross_matrix(); @@ -176,9 +176,9 @@ impl PrismaticVelocityConstraint { mj_lambda1: rb1.active_set_offset, mj_lambda2: rb2.active_set_offset, im1, - ii1_sqrt: rb1.world_inv_inertia_sqrt, + ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, im2, - ii2_sqrt: rb2.world_inv_inertia_sqrt, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, impulse: cparams.impulse * params.warmstart_coeff, limits_impulse: limits_impulse * params.warmstart_coeff, limits_forcedirs, @@ -388,8 +388,8 @@ impl PrismaticVelocityGroundConstraint { // simplifications of the computation without introducing // much instabilities. - let im2 = rb2.mass_properties.inv_mass; - let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let im2 = rb2.effective_inv_mass; + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1 - rb1.world_com; let r2 = anchor2 - rb2.world_com; let r2_mat = r2.gcross_matrix(); @@ -465,7 +465,7 @@ impl PrismaticVelocityGroundConstraint { joint_id, mj_lambda2: rb2.active_set_offset, im2, - ii2_sqrt: rb2.world_inv_inertia_sqrt, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, impulse: cparams.impulse * params.warmstart_coeff, limits_impulse: limits_impulse * params.warmstart_coeff, basis1, diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index 8e0a1ab..04ac182 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -73,9 +73,9 @@ impl WPrismaticVelocityConstraint { let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); let ii1_sqrt = AngularInertia::::from( - array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; @@ -83,9 +83,9 @@ impl WPrismaticVelocityConstraint { let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -431,9 +431,9 @@ impl WPrismaticVelocityGroundConstraint { let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index e46e0e4..19dd451 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -26,10 +26,10 @@ pub(crate) struct RevolutePositionConstraint { impl RevolutePositionConstraint { pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &RevoluteJoint) -> Self { - let ii1 = rb1.world_inv_inertia_sqrt.squared(); - let ii2 = rb2.world_inv_inertia_sqrt.squared(); - let im1 = rb1.mass_properties.inv_mass; - let im2 = rb2.mass_properties.inv_mass; + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); + let im1 = rb1.effective_inv_mass; + let im2 = rb2.effective_inv_mass; let lin_inv_lhs = 1.0 / (im1 + im2); let ang_inv_lhs = (ii1 + ii2).inverse(); diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index e60bd8a..38f56d9 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -52,14 +52,14 @@ impl RevoluteVelocityConstraint { // let basis2 = r21 * basis1; // NOTE: to simplify, we use basis2 = basis1. // Though we may want to test if that does not introduce any instability. - let im1 = rb1.mass_properties.inv_mass; - let im2 = rb2.mass_properties.inv_mass; + let im1 = rb1.effective_inv_mass; + let im2 = rb2.effective_inv_mass; - let ii1 = rb1.world_inv_inertia_sqrt.squared(); + let ii1 = rb1.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1 - rb1.world_com; let r1_mat = r1.gcross_matrix(); - let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let r2 = anchor2 - rb2.world_com; let r2_mat = r2.gcross_matrix(); @@ -87,10 +87,10 @@ impl RevoluteVelocityConstraint { mj_lambda1: rb1.active_set_offset, mj_lambda2: rb2.active_set_offset, im1, - ii1_sqrt: rb1.world_inv_inertia_sqrt, + ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, basis1, im2, - ii2_sqrt: rb2.world_inv_inertia_sqrt, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, impulse: cparams.impulse * params.warmstart_coeff, inv_lhs, rhs, @@ -212,8 +212,8 @@ impl RevoluteVelocityGroundConstraint { // .to_rotation_matrix() // .into_inner(); // let basis2 = /*r21 * */ basis1; - let im2 = rb2.mass_properties.inv_mass; - let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let im2 = rb2.effective_inv_mass; + let ii2 = rb2.effective_world_inv_inertia_sqrt.squared(); let r1 = anchor1 - rb1.world_com; let r2 = anchor2 - rb2.world_com; let r2_mat = r2.gcross_matrix(); @@ -240,7 +240,7 @@ impl RevoluteVelocityGroundConstraint { joint_id, mj_lambda2: rb2.active_set_offset, im2, - ii2_sqrt: rb2.world_inv_inertia_sqrt, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, impulse: cparams.impulse * params.warmstart_coeff, basis1, inv_lhs, diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs index 1b8af80..822c2ac 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -43,9 +43,9 @@ impl WRevoluteVelocityConstraint { let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); let ii1_sqrt = AngularInertia::::from( - array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; @@ -53,9 +53,9 @@ impl WRevoluteVelocityConstraint { let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -262,9 +262,9 @@ impl WRevoluteVelocityGroundConstraint { let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); let ii2_sqrt = AngularInertia::::from( - array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index 9a02330..389b6e6 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -250,7 +250,7 @@ impl ParallelIslandSolver { let rb = &mut bodies[handle.0]; let dvel = mj_lambdas[rb.active_set_offset]; rb.linvel += dvel.linear; - rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular); + rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular); rb.integrate(params.dt()); positions[rb.active_set_offset] = rb.position; } diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 9aa10bb..f846989 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -94,10 +94,10 @@ impl PositionConstraint { local_p2, local_n1: rb1.position.inverse_transform_vector(&manifold.data.normal), dists, - im1: rb1.mass_properties.inv_mass, - im2: rb2.mass_properties.inv_mass, - ii1: rb1.world_inv_inertia_sqrt.squared(), - ii2: rb2.world_inv_inertia_sqrt.squared(), + im1: rb1.effective_inv_mass, + im2: rb2.effective_inv_mass, + ii1: rb1.effective_world_inv_inertia_sqrt.squared(), + ii2: rb2.effective_world_inv_inertia_sqrt.squared(), num_contacts: manifold_points.len() as u8, erp: params.erp, max_linear_correction: params.max_linear_correction, diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index 2bb74c9..67b5cdb 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -38,12 +38,14 @@ impl WPositionConstraint { let rbs1 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH]; let rbs2 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH]; - let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let sqrt_ii1: AngularInertia = - AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let sqrt_ii2: AngularInertia = - AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); + let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); + let sqrt_ii1: AngularInertia = AngularInertia::from( + array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); + let sqrt_ii2: AngularInertia = AngularInertia::from( + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs index a8523f5..4ab07eb 100644 --- a/src/dynamics/solver/position_ground_constraint.rs +++ b/src/dynamics/solver/position_ground_constraint.rs @@ -63,8 +63,8 @@ impl PositionGroundConstraint { local_p2, n1, dists, - im2: rb2.mass_properties.inv_mass, - ii2: rb2.world_inv_inertia_sqrt.squared(), + im2: rb2.effective_inv_mass, + ii2: rb2.effective_world_inv_inertia_sqrt.squared(), num_contacts: manifold_contacts.len() as u8, erp: params.erp, max_linear_correction: params.max_linear_correction, diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index c6f1c3f..f52b3f4 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -45,9 +45,10 @@ impl WPositionGroundConstraint { } } - let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let sqrt_ii2: AngularInertia = - AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); + let sqrt_ii2: AngularInertia = AngularInertia::from( + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); let n1 = Vector::from( array![|ii| if flipped[ii] { -manifolds[ii].data.normal } else { manifolds[ii].data.normal }; SIMD_WIDTH], diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 2686c4d..c2aa6f5 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -161,8 +161,8 @@ impl VelocityConstraint { let mut constraint = VelocityConstraint { dir1: force_dir1, elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im1: rb1.mass_properties.inv_mass, - im2: rb2.mass_properties.inv_mass, + im1: rb1.effective_inv_mass, + im2: rb2.effective_inv_mass, limit: 0.0, mj_lambda1, mj_lambda2, @@ -204,8 +204,8 @@ impl VelocityConstraint { #[cfg(target_arch = "wasm32")] { constraint.dir1 = force_dir1; - constraint.im1 = rb1.mass_properties.inv_mass; - constraint.im2 = rb2.mass_properties.inv_mass; + constraint.im1 = rb1.effective_inv_mass; + constraint.im2 = rb2.effective_inv_mass; constraint.limit = manifold.data.friction; constraint.mj_lambda1 = mj_lambda1; constraint.mj_lambda2 = mj_lambda2; @@ -226,15 +226,15 @@ impl VelocityConstraint { // Normal part. { let gcross1 = rb1 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .transform_vector(dp1.gcross(force_dir1)); let gcross2 = rb2 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-force_dir1)); let r = 1.0 - / (rb1.mass_properties.inv_mass - + rb2.mass_properties.inv_mass + / (rb1.effective_inv_mass + + rb2.effective_inv_mass + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); @@ -263,14 +263,14 @@ impl VelocityConstraint { for j in 0..DIM - 1 { let gcross1 = rb1 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .transform_vector(dp1.gcross(tangents1[j])); let gcross2 = rb2 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-tangents1[j])); let r = 1.0 - / (rb1.mass_properties.inv_mass - + rb2.mass_properties.inv_mass + / (rb1.effective_inv_mass + + rb2.effective_inv_mass + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2).dot(&tangents1[j]); diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index b86e6a6..a8384de 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -71,18 +71,20 @@ impl WVelocityConstraint { let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; - let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1: AngularInertia = - AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); + let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]); + let ii1: AngularInertia = AngularInertia::from( + array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); - let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2: AngularInertia = - AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); + let ii2: AngularInertia = AngularInertia::from( + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 37d4e3a..7ea4fbb 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -87,7 +87,7 @@ impl VelocityGroundConstraint { let mut constraint = VelocityGroundConstraint { dir1: force_dir1, elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im2: rb2.mass_properties.inv_mass, + im2: rb2.effective_inv_mass, limit: 0.0, mj_lambda2, manifold_id, @@ -128,7 +128,7 @@ impl VelocityGroundConstraint { #[cfg(target_arch = "wasm32")] { constraint.dir1 = force_dir1; - constraint.im2 = rb2.mass_properties.inv_mass; + constraint.im2 = rb2.effective_inv_mass; constraint.limit = manifold.data.friction; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; @@ -148,10 +148,10 @@ impl VelocityGroundConstraint { // Normal part. { let gcross2 = rb2 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-force_dir1)); - let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2)); + let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2)); let mut rhs = (vel1 - vel2).dot(&force_dir1); @@ -177,9 +177,9 @@ impl VelocityGroundConstraint { for j in 0..DIM - 1 { let gcross2 = rb2 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-tangents1[j])); - let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2)); + let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2)); let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]); #[cfg(feature = "dim2")] let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff; diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 1c85a90..c2d2c4f 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -75,9 +75,10 @@ impl WVelocityGroundConstraint { } } - let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2: AngularInertia = - AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); + let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); + let ii2: AngularInertia = AngularInertia::from( + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], + ); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index dfb97b0..89cf34e 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -60,7 +60,9 @@ impl VelocitySolver { bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { let dvel = self.mj_lambdas[rb.active_set_offset]; rb.linvel += dvel.linear; - rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular); + rb.angvel += rb + .effective_world_inv_inertia_sqrt + .transform_vector(dvel.angular); }); // Write impulses back into the manifold structures. -- cgit