From b631fe9193a2e769e5ca1c5c8c4ac9843da870ac Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 9 Jan 2022 22:15:36 +0100 Subject: Allow locking individual translational axes --- src/dynamics/joint/multibody_joint/multibody.rs | 23 +++++--- src/dynamics/rigid_body.rs | 66 +++++++++++++++++++++- src/dynamics/rigid_body_components.rs | 51 ++++++++++++----- src/dynamics/solver/generic_velocity_constraint.rs | 22 +++++--- .../solver/generic_velocity_constraint_element.rs | 16 +++--- .../joint_generic_velocity_constraint_builder.rs | 2 +- .../joint_constraint/joint_velocity_constraint.rs | 20 +++---- .../joint_velocity_constraint_builder.rs | 11 ++-- src/dynamics/solver/velocity_constraint.rs | 16 +++--- src/dynamics/solver/velocity_constraint_element.rs | 28 ++++----- src/dynamics/solver/velocity_constraint_wide.rs | 22 +++++--- src/dynamics/solver/velocity_ground_constraint.rs | 13 +++-- .../solver/velocity_ground_constraint_element.rs | 14 ++--- .../solver/velocity_ground_constraint_wide.rs | 13 +++-- src/dynamics/solver/velocity_solver.rs | 2 +- src/pipeline/physics_pipeline.rs | 2 +- 16 files changed, 216 insertions(+), 105 deletions(-) (limited to 'src') diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 7414077..1b88245 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -35,23 +35,26 @@ impl Force { } #[cfg(feature = "dim2")] -fn concat_rb_mass_matrix(mass: Real, inertia: Real) -> SMatrix { +fn concat_rb_mass_matrix( + mass: Vector, + inertia: Real, +) -> SMatrix { let mut result = SMatrix::::zeros(); - result[(0, 0)] = mass; - result[(1, 1)] = mass; + result[(0, 0)] = mass.x; + result[(1, 1)] = mass.y; result[(2, 2)] = inertia; result } #[cfg(feature = "dim3")] fn concat_rb_mass_matrix( - mass: Real, + mass: Vector, inertia: Matrix, ) -> SMatrix { let mut result = SMatrix::::zeros(); - result[(0, 0)] = mass; - result[(1, 1)] = mass; - result[(2, 2)] = mass; + result[(0, 0)] = mass.x; + result[(1, 1)] = mass.y; + result[(2, 2)] = mass.z; result .fixed_slice_mut::(DIM, DIM) .copy_from(&inertia); @@ -422,7 +425,7 @@ impl Multibody { } let external_forces = Force::new( - rb_forces.force - rb_mass * acc.linvel, + rb_forces.force - rb_mass.component_mul(&acc.linvel), rb_forces.torque - gyroscopic - rb_inertia * acc.angvel, ); self.accelerations.gemv_tr( @@ -699,7 +702,9 @@ impl Multibody { { let mut i_coriolis_dt_v = self.i_coriolis_dt.fixed_rows_mut::(0); i_coriolis_dt_v.copy_from(coriolis_v); - i_coriolis_dt_v *= rb_mass * dt; + i_coriolis_dt_v + .column_iter_mut() + .for_each(|mut c| c.component_mul_assign(&(rb_mass * dt))); } #[cfg(feature = "dim2")] diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 0b41981..e5dcd04 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -195,7 +195,48 @@ impl RigidBody { } } + #[inline] + /// Locks or unlocks rotations of this rigid-body along each cartesian axes. + pub fn restrict_translations( + &mut self, + allow_translation_x: bool, + allow_translation_y: bool, + #[cfg(feature = "dim3")] allow_translation_z: bool, + wake_up: bool, + ) { + if self.is_dynamic() { + if wake_up { + self.wake_up(true); + } + + self.rb_mprops.flags.set( + RigidBodyMassPropsFlags::TRANSLATION_LOCKED_X, + !allow_translation_x, + ); + self.rb_mprops.flags.set( + RigidBodyMassPropsFlags::TRANSLATION_LOCKED_Y, + !allow_translation_y, + ); + #[cfg(feature = "dim3")] + self.rb_mprops.flags.set( + RigidBodyMassPropsFlags::TRANSLATION_LOCKED_Z, + !allow_translation_z, + ); + self.update_world_mass_properties(); + } + } + + /// Are the translations of this rigid-body locked? + #[cfg(feature = "dim2")] + pub fn is_translation_locked(&self) -> bool { + self.rb_mprops.flags.contains( + RigidBodyMassPropsFlags::TRANSLATION_LOCKED_X + | RigidBodyMassPropsFlags::TRANSLATION_LOCKED_Y, + ) + } + /// Are the translations of this rigid-body locked? + #[cfg(feature = "dim3")] pub fn is_translation_locked(&self) -> bool { self.rb_mprops .flags @@ -633,7 +674,7 @@ impl RigidBody { /// This does nothing on non-dynamic bodies. pub fn apply_impulse(&mut self, impulse: Vector, wake_up: bool) { if self.rb_type == RigidBodyType::Dynamic { - self.rb_vels.linvel += impulse * self.rb_mprops.effective_inv_mass; + self.rb_vels.linvel += impulse.component_mul(&self.rb_mprops.effective_inv_mass); if wake_up { self.wake_up(true); @@ -847,6 +888,29 @@ impl RigidBodyBuilder { self } + /// Only allow translations of this rigid-body around specific coordinate axes. + pub fn restrict_translations( + mut self, + allow_translations_x: bool, + allow_translations_y: bool, + #[cfg(feature = "dim3")] allow_translations_z: bool, + ) -> Self { + self.mprops_flags.set( + RigidBodyMassPropsFlags::TRANSLATION_LOCKED_X, + !allow_translations_x, + ); + self.mprops_flags.set( + RigidBodyMassPropsFlags::TRANSLATION_LOCKED_Y, + !allow_translations_y, + ); + #[cfg(feature = "dim3")] + self.mprops_flags.set( + RigidBodyMassPropsFlags::TRANSLATION_LOCKED_Z, + !allow_translations_z, + ); + self + } + /// Prevents this rigid-body from rotating because of forces. pub fn lock_rotations(mut self) -> Self { self.mprops_flags diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index 23ff7a8..ff3a614 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -203,14 +203,20 @@ bitflags::bitflags! { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// Flags affecting the behavior of the constraints solver for a given contact manifold. pub struct RigidBodyMassPropsFlags: u8 { + /// Flag indicating that the rigid-body cannot translate along the `X` axis. + const TRANSLATION_LOCKED_X = 1 << 0; + /// Flag indicating that the rigid-body cannot translate along the `Y` axis. + const TRANSLATION_LOCKED_Y = 1 << 1; + /// Flag indicating that the rigid-body cannot translate along the `Z` axis. + const TRANSLATION_LOCKED_Z = 1 << 2; /// Flag indicating that the rigid-body cannot translate along any direction. - const TRANSLATION_LOCKED = 1 << 0; + const TRANSLATION_LOCKED = Self::TRANSLATION_LOCKED_X.bits | Self::TRANSLATION_LOCKED_Y.bits | Self::TRANSLATION_LOCKED_Z.bits; /// Flag indicating that the rigid-body cannot rotate along the `X` axis. - const ROTATION_LOCKED_X = 1 << 1; + const ROTATION_LOCKED_X = 1 << 3; /// Flag indicating that the rigid-body cannot rotate along the `Y` axis. - const ROTATION_LOCKED_Y = 1 << 2; + const ROTATION_LOCKED_Y = 1 << 4; /// Flag indicating that the rigid-body cannot rotate along the `Z` axis. - const ROTATION_LOCKED_Z = 1 << 3; + const ROTATION_LOCKED_Z = 1 << 5; /// Combination of flags indicating that the rigid-body cannot rotate along any axis. const ROTATION_LOCKED = Self::ROTATION_LOCKED_X.bits | Self::ROTATION_LOCKED_Y.bits | Self::ROTATION_LOCKED_Z.bits; } @@ -228,7 +234,7 @@ pub struct RigidBodyMassProps { /// The world-space center of mass of the rigid-body. pub world_com: Point, /// The inverse mass taking into account translation locking. - pub effective_inv_mass: Real, + pub effective_inv_mass: Vector, /// The square-root of the world-space inverse angular inertia tensor of the rigid-body, /// taking into account rotation locking. pub effective_world_inv_inertia_sqrt: AngularInertia, @@ -240,7 +246,7 @@ impl Default for RigidBodyMassProps { flags: RigidBodyMassPropsFlags::empty(), local_mprops: MassProperties::zero(), world_com: Point::origin(), - effective_inv_mass: 0.0, + effective_inv_mass: Vector::zero(), effective_world_inv_inertia_sqrt: AngularInertia::zero(), } } @@ -274,8 +280,8 @@ impl RigidBodyMassProps { /// The effective mass (that takes the potential translation locking into account) of /// this rigid-body. #[must_use] - pub fn effective_mass(&self) -> Real { - crate::utils::inv(self.effective_inv_mass) + pub fn effective_mass(&self) -> Vector { + self.effective_inv_mass.map(crate::utils::inv) } /// The effective world-space angular inertia (that takes the potential rotation locking into account) of @@ -288,16 +294,31 @@ impl RigidBodyMassProps { /// Update the world-space mass properties of `self`, taking into account the new position. pub fn update_world_mass_properties(&mut self, position: &Isometry) { self.world_com = self.local_mprops.world_com(&position); - self.effective_inv_mass = self.local_mprops.inv_mass; + self.effective_inv_mass = Vector::repeat(self.local_mprops.inv_mass); self.effective_world_inv_inertia_sqrt = self.local_mprops.world_inv_inertia_sqrt(&position.rotation); // Take into account translation/rotation locking. if self .flags - .contains(RigidBodyMassPropsFlags::TRANSLATION_LOCKED) + .contains(RigidBodyMassPropsFlags::TRANSLATION_LOCKED_X) { - self.effective_inv_mass = 0.0; + self.effective_inv_mass.x = 0.0; + } + + if self + .flags + .contains(RigidBodyMassPropsFlags::TRANSLATION_LOCKED_Y) + { + self.effective_inv_mass.y = 0.0; + } + + #[cfg(feature = "dim3")] + if self + .flags + .contains(RigidBodyMassPropsFlags::TRANSLATION_LOCKED_Z) + { + self.effective_inv_mass.z = 0.0; } #[cfg(feature = "dim2")] @@ -536,7 +557,7 @@ impl RigidBodyVelocity { /// The impulse is applied right away, changing the linear velocity. /// This does nothing on non-dynamic bodies. pub fn apply_impulse(&mut self, rb_mprops: &RigidBodyMassProps, impulse: Vector) { - self.linvel += impulse * rb_mprops.effective_inv_mass; + self.linvel += impulse.component_mul(&rb_mprops.effective_inv_mass); } /// Applies an angular impulse at the center-of-mass of this rigid-body. @@ -659,7 +680,7 @@ impl RigidBodyForces { init_vels: &RigidBodyVelocity, mprops: &RigidBodyMassProps, ) -> RigidBodyVelocity { - let linear_acc = self.force * mprops.effective_inv_mass; + let linear_acc = self.force.component_mul(&mprops.effective_inv_mass); let angular_acc = mprops.effective_world_inv_inertia_sqrt * (mprops.effective_world_inv_inertia_sqrt * self.torque); @@ -671,8 +692,8 @@ impl RigidBodyForces { /// Adds to `self` the gravitational force that would result in a gravitational acceleration /// equal to `gravity`. - pub fn add_gravity_acceleration(&mut self, gravity: &Vector, mass: Real) { - self.force += gravity * self.gravity_scale * mass; + pub fn add_gravity_acceleration(&mut self, gravity: &Vector, mass: &Vector) { + self.force += gravity.component_mul(&mass) * self.gravity_scale; } /// Applies a force at the given world-space point of the rigid-body with the given mass properties. diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs index fb06335..8c93511 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -116,12 +116,12 @@ impl GenericVelocityConstraint { im1: if rb_type1.is_dynamic() { rb_mprops1.effective_inv_mass } else { - 0.0 + na::zero() }, im2: if rb_type2.is_dynamic() { rb_mprops2.effective_inv_mass } else { - 0.0 + na::zero() }, limit: 0.0, mj_lambda1, @@ -175,7 +175,8 @@ impl GenericVelocityConstraint { ) .0 } else if rb_type1.is_dynamic() { - rb_mprops1.effective_inv_mass + gcross1.gdot(gcross1) + force_dir1.dot(&rb_mprops1.effective_inv_mass.component_mul(&force_dir1)) + + gcross1.gdot(gcross1) } else { 0.0 }; @@ -193,7 +194,8 @@ impl GenericVelocityConstraint { ) .0 } else if rb_type2.is_dynamic() { - rb_mprops2.effective_inv_mass + gcross2.gdot(gcross2) + force_dir1.dot(&rb_mprops2.effective_inv_mass.component_mul(&force_dir1)) + + gcross2.gdot(gcross2) } else { 0.0 }; @@ -258,7 +260,9 @@ impl GenericVelocityConstraint { ) .0 } else if rb_type1.is_dynamic() { - rb_mprops1.effective_inv_mass + gcross1.gdot(gcross1) + force_dir1 + .dot(&rb_mprops1.effective_inv_mass.component_mul(&force_dir1)) + + gcross1.gdot(gcross1) } else { 0.0 }; @@ -276,7 +280,9 @@ impl GenericVelocityConstraint { ) .0 } else if rb_type2.is_dynamic() { - rb_mprops2.effective_inv_mass + gcross2.gdot(gcross2) + force_dir1 + .dot(&rb_mprops2.effective_inv_mass.component_mul(&force_dir1)) + + gcross2.gdot(gcross2) } else { 0.0 }; @@ -345,8 +351,8 @@ impl GenericVelocityConstraint { &self.velocity_constraint.dir1, #[cfg(feature = "dim3")] &self.velocity_constraint.tangent1, - self.velocity_constraint.im1, - self.velocity_constraint.im2, + &self.velocity_constraint.im1, + &self.velocity_constraint.im2, self.velocity_constraint.limit, self.ndofs1, self.ndofs2, diff --git a/src/dynamics/solver/generic_velocity_constraint_element.rs b/src/dynamics/solver/generic_velocity_constraint_element.rs index 150be8b..c31bbfb 100644 --- a/src/dynamics/solver/generic_velocity_constraint_element.rs +++ b/src/dynamics/solver/generic_velocity_constraint_element.rs @@ -70,11 +70,11 @@ impl GenericRhs { dir: &Vector, gcross: &AngVector, mj_lambdas: &mut DVector, - inv_mass: Real, + inv_mass: &Vector, ) { match self { GenericRhs::DeltaVel(rhs) => { - rhs.linear += dir * (inv_mass * impulse); + rhs.linear += dir.component_mul(inv_mass) * impulse; rhs.angular += gcross * impulse; } GenericRhs::GenericId(mj_lambda) => { @@ -94,8 +94,8 @@ impl VelocityConstraintTangentPart { j_id: usize, jacobians: &DVector, tangents1: [&Vector; DIM - 1], - im1: Real, - im2: Real, + im1: &Vector, + im2: &Vector, ndofs1: usize, ndofs2: usize, limit: Real, @@ -246,8 +246,8 @@ impl VelocityConstraintNormalPart { j_id: usize, jacobians: &DVector, dir1: &Vector, - im1: Real, - im2: Real, + im1: &Vector, + im2: &Vector, ndofs1: usize, ndofs2: usize, mj_lambda1: &mut GenericRhs, @@ -296,8 +296,8 @@ impl VelocityConstraintElement { jacobians: &DVector, dir1: &Vector, #[cfg(feature = "dim3")] tangent1: &Vector, - im1: Real, - im2: Real, + im1: &Vector, + im2: &Vector, limit: Real, // ndofs is 0 for a non-multibody body, or a multibody with zero // degrees of freedom. diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs index 92bcc9f..5edc815 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs @@ -33,7 +33,7 @@ impl SolverBody { let mut out_invm_j = jacobians.fixed_rows_mut::(wj_id); out_invm_j .fixed_rows_mut::(0) - .axpy(self.im, &unit_force, 0.0); + .copy_from(&self.im.component_mul(&unit_force)); #[cfg(feature = "dim2")] { diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index 3e31256..a4ec313 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -51,7 +51,7 @@ pub enum WritebackId { pub struct SolverBody { pub linvel: Vector, pub angvel: AngVector, - pub im: N, + pub im: Vector, pub sqrt_ii: AngularInertia, pub world_com: Point, pub mj_lambda: [usize; LANES], @@ -74,8 +74,8 @@ pub struct JointVelocityConstraint { pub rhs: N, pub rhs_wo_bias: N, - pub im1: N, - pub im2: N, + pub im1: Vector, + pub im2: Vector, pub writeback_id: WritebackId, } @@ -94,8 +94,8 @@ impl JointVelocityConstraint { inv_lhs: N::zero(), rhs: N::zero(), rhs_wo_bias: N::zero(), - im1: N::zero(), - im2: N::zero(), + im1: na::zero(), + im2: na::zero(), writeback_id: WritebackId::Dof(0), } } @@ -115,9 +115,9 @@ impl JointVelocityConstraint { let ang_impulse1 = self.ang_jac1 * delta_impulse; let ang_impulse2 = self.ang_jac2 * delta_impulse; - mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.linear += lin_impulse.component_mul(&self.im1); mj_lambda1.angular += ang_impulse1; - mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.linear -= lin_impulse.component_mul(&self.im2); mj_lambda2.angular -= ang_impulse2; } @@ -353,7 +353,7 @@ pub struct JointVelocityGroundConstraint { pub rhs: N, pub rhs_wo_bias: N, - pub im2: N, + pub im2: Vector, pub writeback_id: WritebackId, } @@ -370,7 +370,7 @@ impl JointVelocityGroundConstraint { inv_lhs: N::zero(), rhs: N::zero(), rhs_wo_bias: N::zero(), - im2: N::zero(), + im2: na::zero(), writeback_id: WritebackId::Dof(0), } } @@ -388,7 +388,7 @@ impl JointVelocityGroundConstraint { let lin_impulse = self.lin_jac * delta_impulse; let ang_impulse = self.ang_jac2 * delta_impulse; - mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.linear -= lin_impulse.component_mul(&self.im2); mj_lambda2.angular -= ang_impulse; } diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs index dd17ecf..55a112a 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs @@ -355,7 +355,7 @@ impl JointVelocityConstraintBuilder { // Use the modified Gram-Schmidt orthogonalization. for j in 0..len { let c_j = &mut constraints[j]; - let dot_jj = c_j.lin_jac.norm_squared() * imsum + let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac)) + c_j.ang_jac1.gdot(c_j.ang_jac1) + c_j.ang_jac2.gdot(c_j.ang_jac2); let inv_dot_jj = crate::utils::simd_inv(dot_jj); @@ -370,7 +370,7 @@ impl JointVelocityConstraintBuilder { for i in (j + 1)..len { let (c_i, c_j) = constraints.index_mut_const(i, j); - let dot_ij = c_i.lin_jac.dot(&c_j.lin_jac) * imsum + let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac)) + c_i.ang_jac1.gdot(c_j.ang_jac1) + c_i.ang_jac2.gdot(c_j.ang_jac2); let coeff = dot_ij * inv_dot_jj; @@ -672,7 +672,8 @@ impl JointVelocityConstraintBuilder { // Use the modified Gram-Schmidt orthogonalization. for j in 0..len { let c_j = &mut constraints[j]; - let dot_jj = c_j.lin_jac.norm_squared() * imsum + c_j.ang_jac2.gdot(c_j.ang_jac2); + let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac)) + + c_j.ang_jac2.gdot(c_j.ang_jac2); let inv_dot_jj = crate::utils::simd_inv(dot_jj); c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs. @@ -685,8 +686,8 @@ impl JointVelocityConstraintBuilder { for i in (j + 1)..len { let (c_i, c_j) = constraints.index_mut_const(i, j); - let dot_ij = - c_i.lin_jac.dot(&c_j.lin_jac) * imsum + c_i.ang_jac2.gdot(c_j.ang_jac2); + let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac)) + + c_i.ang_jac2.gdot(c_j.ang_jac2); let coeff = dot_ij * inv_dot_jj; c_i.lin_jac -= c_j.lin_jac * coeff; diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 719468d..0a841b8 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -94,8 +94,8 @@ pub(crate) struct VelocityConstraint { pub dir1: Vector, // Non-penetration force direction for the first body. #[cfg(feature = "dim3")] pub tangent1: Vector, // One of the friction force directions. - pub im1: Real, - pub im2: Real, + pub im1: Vector, + pub im2: Vector, pub limit: Real, pub mj_lambda1: usize, pub mj_lambda2: usize, @@ -235,9 +235,9 @@ impl VelocityConstraint { .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-force_dir1)); + let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; let r = 1.0 - / (mprops1.effective_inv_mass - + mprops2.effective_inv_mass + / (force_dir1.dot(&imsum.component_mul(&force_dir1)) + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); @@ -274,9 +274,9 @@ impl VelocityConstraint { let gcross2 = mprops2 .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-tangents1[j])); + let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; let r = 1.0 - / (mprops1.effective_inv_mass - + mprops2.effective_inv_mass + / (tangents1[j].dot(&imsum.component_mul(&tangents1[j])) + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); let rhs = @@ -314,8 +314,8 @@ impl VelocityConstraint { &self.dir1, #[cfg(feature = "dim3")] &self.tangent1, - self.im1, - self.im2, + &self.im1, + &self.im2, self.limit, &mut mj_lambda1, &mut mj_lambda2, 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 VelocityConstraintTangentPart { pub fn solve( &mut self, tangents1: [&Vector; DIM - 1], - im1: N, - im2: N, + im1: &Vector, + im2: &Vector, limit: N, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel, @@ -50,10 +50,10 @@ impl VelocityConstraintTangentPart { 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 VelocityConstraintTangentPart { 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 VelocityConstraintNormalPart { pub fn solve( &mut self, dir1: &Vector, - im1: N, - im2: N, + im1: &Vector, + im2: &Vector, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel, ) where @@ -136,10 +136,10 @@ impl VelocityConstraintNormalPart { 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 VelocityConstraintElement { elements: &mut [Self], dir1: &Vector, #[cfg(feature = "dim3")] tangent1: &Vector, - im1: N, - im2: N, + im1: &Vector, + im2: &Vector, limit: N, mj_lambda1: &mut DeltaVel, mj_lambda2: &mut DeltaVel, diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 0e2e36a..f072ad8 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -20,8 +20,8 @@ pub(crate) struct WVelocityConstraint { pub tangent1: Vector, // One of the friction force directions. pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS], pub num_contacts: u8, - pub im1: SimdReal, - pub im2: SimdReal, + pub im1: Vector, + pub im2: Vector, pub limit: SimdReal, pub mj_lambda1: [usize; SIMD_WIDTH], pub mj_lambda2: [usize; SIMD_WIDTH], @@ -62,7 +62,7 @@ impl WVelocityConstraint { let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)]; let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); - let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]); + let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]); let ii1: AngularInertia = AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]); @@ -70,7 +70,7 @@ impl WVelocityConstraint { let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); - let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let im2 = Vector::from(gather![|ii| mprops2[ii].effective_inv_mass]); let ii2: AngularInertia = AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]); @@ -135,8 +135,11 @@ impl WVelocityConstraint { let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1)); let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); + let imsum = im1 + im2; let r = SimdReal::splat(1.0) - / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); + / (force_dir1.dot(&imsum.component_mul(&force_dir1)) + + gcross1.gdot(gcross1) + + gcross2.gdot(gcross2)); let projected_velocity = (vel1 - vel2).dot(&force_dir1); let mut rhs_wo_bias = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; @@ -161,8 +164,11 @@ impl WVelocityConstraint { for j in 0..DIM - 1 { let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j])); let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); + let imsum = im1 + im2; let r = SimdReal::splat(1.0) - / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); + / (tangents1[j].dot(&imsum.component_mul(&tangents1[j])) + + gcross1.gdot(gcross1) + + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]); constraint.elements[k].tangent_part.gcross1[j] = gcross1; @@ -206,8 +212,8 @@ impl WVelocityConstraint { &self.dir1, #[cfg(feature = "dim3")] &self.tangent1, - self.im1, - self.im2, + &self.im1, + &self.im2, self.limit, &mut mj_lambda1, &mut mj_lambda2, diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 87865b3..bd60633 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -17,7 +17,7 @@ pub(crate) struct VelocityGroundConstraint { pub dir1: Vector, // Non-penetration force direction for the first body. #[cfg(feature = "dim3")] pub tangent1: Vector, // One of the friction force directions. - pub im2: Real, + pub im2: Vector, pub limit: Real, pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], @@ -153,7 +153,9 @@ impl VelocityGroundConstraint { .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-force_dir1)); - let r = 1.0 / (mprops2.effective_inv_mass + gcross2.gdot(gcross2)); + let r = 1.0 + / (force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + + gcross2.gdot(gcross2)); let is_bouncy = manifold_point.is_bouncy() as u32 as Real; let is_resting = 1.0 - is_bouncy; @@ -184,7 +186,10 @@ impl VelocityGroundConstraint { let gcross2 = mprops2 .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-tangents1[j])); - let r = 1.0 / (mprops2.effective_inv_mass + gcross2.gdot(gcross2)); + let r = 1.0 + / (tangents1[j] + .dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j])) + + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2 + flipped_multiplier * manifold_point.tangent_velocity) .dot(&tangents1[j]); @@ -219,7 +224,7 @@ impl VelocityGroundConstraint { &self.dir1, #[cfg(feature = "dim3")] &self.tangent1, - self.im2, + &self.im2, self.limit, &mut mj_lambda2, solve_normal, diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs index adfcfda..4d4d3c3 100644 --- a/src/dynamics/solver/velocity_ground_constraint_element.rs +++ b/src/dynamics/solver/velocity_ground_constraint_element.rs @@ -29,7 +29,7 @@ impl VelocityGroundConstraintTangentPart { pub fn solve( &mut self, tangents1: [&Vector; DIM - 1], - im2: N, + im2: &Vector, limit: N, mj_lambda2: &mut DeltaVel, ) where @@ -45,7 +45,7 @@ impl VelocityGroundConstraintTangentPart { let dlambda = new_impulse - self.impulse[0]; self.impulse[0] = new_impulse; - mj_lambda2.linear += tangents1[0] * (-im2 * dlambda); + mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda; mj_lambda2.angular += self.gcross2[0] * dlambda; } @@ -72,8 +72,8 @@ impl VelocityGroundConstraintTangentPart { self.impulse = new_impulse; - 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]; } } @@ -101,7 +101,7 @@ impl VelocityGroundConstraintNormalPart { } #[inline] - pub fn solve(&mut self, dir1: &Vector, im2: N, mj_lambda2: &mut DeltaVel) + pub fn solve(&mut self, dir1: &Vector, im2: &Vector, mj_lambda2: &mut DeltaVel) where AngVector: WDot, Result = N>, { @@ -111,7 +111,7 @@ impl VelocityGroundConstraintNormalPart { let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; - mj_lambda2.linear += dir1 * (-im2 * dlambda); + mj_lambda2.linear += dir1.component_mul(im2) * -dlambda; mj_lambda2.angular += self.gcross2 * dlambda; } } @@ -136,7 +136,7 @@ impl VelocityGroundConstraintElement { elements: &mut [Self], dir1: &Vector, #[cfg(feature = "dim3")] tangent1: &Vector, - im2: N, + im2: &Vector, limit: N, mj_lambda2: &mut DeltaVel, solve_normal: bool, diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index e1ea8f6..2e4812b 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -21,7 +21,7 @@ pub(crate) struct WVelocityGroundConstraint { pub tangent1: Vector, // One of the friction force directions. pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], pub num_contacts: u8, - pub im2: SimdReal, + pub im2: Vector, pub limit: SimdReal, pub mj_lambda2: [usize; SIMD_WIDTH], pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], @@ -76,7 +76,7 @@ impl WVelocityGroundConstraint { let flipped_sign = SimdReal::from(flipped); - let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]); + let im2 = Vector::from(gather![|ii| mprops2[ii].effective_inv_mass]); let ii2: AngularInertia = AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]); @@ -142,7 +142,8 @@ impl WVelocityGroundConstraint { { let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); - let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2)); + let r = SimdReal::splat(1.0) + / (force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2)); let projected_velocity = (vel1 - vel2).dot(&force_dir1); let mut rhs_wo_bias = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; @@ -165,7 +166,9 @@ impl WVelocityGroundConstraint { for j in 0..DIM - 1 { let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); - let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2)); + let r = SimdReal::splat(1.0) + / (tangents1[j].dot(&im2.component_mul(&tangents1[j])) + + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2 + tangent_velocity * flipped_sign).dot(&tangents1[j]); constraint.elements[k].tangent_part.gcross2[j] = gcross2; @@ -201,7 +204,7 @@ impl WVelocityGroundConstraint { &self.dir1, #[cfg(feature = "dim3")] &self.tangent1, - self.im2, + &self.im2, self.limit, &mut mj_lambda2, solve_normal, diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index ed1fe3a..91b6be9 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -67,7 +67,7 @@ impl VelocitySolver { // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied // by the square root of the inertia tensor: dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt; - dvel.linear += forces.force * (mprops.effective_inv_mass * params.dt); + dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt; } for (_, multibody) in multibodies.multibodies.iter_mut() { diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 125b144..a529aee 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -223,7 +223,7 @@ impl PhysicsPipeline { }) .unwrap(); bodies.map_mut_internal(handle.0, |forces: &mut RigidBodyForces| { - forces.add_gravity_acceleration(&gravity, effective_inv_mass) + forces.add_gravity_acceleration(&gravity, &effective_inv_mass) }); } -- cgit From 8213e92f146fab618a406e0f8fed8a15ebd9228c Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 16 Jan 2022 16:37:44 +0100 Subject: Fix parallel build. --- src/dynamics/solver/parallel_island_solver.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index e695b3d..6d5debe 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -247,7 +247,7 @@ impl ParallelIslandSolver { // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied // by the square root of the inertia tensor: dvel.angular += rb_mass_props.effective_world_inv_inertia_sqrt * rb_forces.torque * params.dt; - dvel.linear += rb_forces.force * (rb_mass_props.effective_inv_mass * params.dt); + dvel.linear += rb_forces.force.component_mul(&rb_mass_props.effective_inv_mass) * params.dt; } } -- cgit