diff options
17 files changed, 217 insertions, 106 deletions
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<Real, SPATIAL_DIM, SPATIAL_DIM> { +fn concat_rb_mass_matrix( + mass: Vector<Real>, + inertia: Real, +) -> SMatrix<Real, SPATIAL_DIM, SPATIAL_DIM> { let mut result = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::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<Real>, inertia: Matrix<Real>, ) -> SMatrix<Real, SPATIAL_DIM, SPATIAL_DIM> { let mut result = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::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::<ANG_DIM, ANG_DIM>(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::<DIM>(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<Real>, 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<Real>, /// The inverse mass taking into account translation locking. - pub effective_inv_mass: Real, + pub effective_inv_mass: Vector<Real>, /// 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<Real>, @@ -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<Real> { + 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<Real>) { 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<Real>) { - 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<Real>, mass: Real) { - self.force += gravity * self.gravity_scale * mass; + pub fn add_gravity_acceleration(&mut self, gravity: &Vector<Real>, mass: &Vector<Real>) { + 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<Real>, gcross: &AngVector<Real>, mj_lambdas: &mut DVector<Real>, - inv_mass: Real, + inv_mass: &Vector<Real>, ) { 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<Real> { j_id: usize, jacobians: &DVector<Real>, tangents1: [&Vector<Real>; DIM - 1], - im1: Real, - im2: Real, + im1: &Vector<Real>, + im2: &Vector<Real>, ndofs1: usize, ndofs2: usize, limit: Real, @@ -246,8 +246,8 @@ impl VelocityConstraintNormalPart<Real> { j_id: usize, jacobians: &DVector<Real>, dir1: &Vector<Real>, - im1: Real, - im2: Real, + im1: &Vector<Real>, + im2: &Vector<Real>, ndofs1: usize, ndofs2: usize, mj_lambda1: &mut GenericRhs, @@ -296,8 +296,8 @@ impl VelocityConstraintElement<Real> { jacobians: &DVector<Real>, dir1: &Vector<Real>, #[cfg(feature = "dim3")] tangent1: &Vector<Real>, - im1: Real, - im2: Real, + im1: &Vector<Real>, + im2: &Vector<Real>, 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<Real, 1> { let mut out_invm_j = jacobians.fixed_rows_mut::<SPATIAL_DIM>(wj_id); out_invm_j .fixed_rows_mut::<DIM>(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<N: SimdRealField, const LANES: usize> { pub linvel: Vector<N>, pub angvel: AngVector<N>, - pub im: N, + pub im: Vector<N>, pub sqrt_ii: AngularInertia<N>, pub world_com: Point<N>, pub mj_lambda: [usize; LANES], @@ -74,8 +74,8 @@ pub struct JointVelocityConstraint<N: SimdRealField, const LANES: usize> { pub rhs: N, pub rhs_wo_bias: N, - pub im1: N, - pub im2: N, + pub im1: Vector<N>, + pub im2: Vector<N>, pub writeback_id: WritebackId, } @@ -94,8 +94,8 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> { 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<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> { 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<N: SimdRealField, const LANES: usize> { pub rhs: N, pub rhs_wo_bias: N, - pub im2: N, + pub im2: Vector<N>, pub writeback_id: WritebackId, } @@ -370,7 +370,7 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> { 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<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> { 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<N: WReal> JointVelocityConstraintBuilder<N> { // 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<N: WReal> JointVelocityConstraintBuilder<N> { 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<N: WReal> JointVelocityConstraintBuilder<N> { // 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<N: WReal> JointVelocityConstraintBuilder<N> { 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/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; } } 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<Real>, // Non-penetration force direction for the first body. #[cfg(feature = "dim3")] pub tangent1: Vector<Real>, // One of the friction force directions. - pub im1: Real, - pub im2: Real, + pub im1: Vector<Real>, + pub im2: Vector<Real>, 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<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> { pub fn solve( &mut self, tangents1: [&Vector<N>; DIM - 1], - im1: N, - im2: N, + im1: &Vector<N>, + im2: &Vector<N>, limit: N, mj_lambda1: &mut DeltaVel<N>, mj_lambda2: &mut DeltaVel<N>, @@ -50,10 +50,10 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> { 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<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> { 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<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> { pub fn solve( &mut self, dir1: &Vector<N>, - im1: N, - im2: N, + im1: &Vector<N>, + im2: &Vector<N>, mj_lambda1: &mut DeltaVel<N>, mj_lambda2: &mut DeltaVel<N>, ) where @@ -136,10 +136,10 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> { 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<N: SimdRealField + Copy> VelocityConstraintElement<N> { elements: &mut [Self], dir1: &Vector<N>, #[cfg(feature = "dim3")] tangent1: &Vector<N>, - im1: N, - im2: N, + im1: &Vector<N>, + im2: &Vector<N>, limit: N, mj_lambda1: &mut DeltaVel<N>, mj_lambda2: &mut DeltaVel<N>, 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<SimdReal>, // One of the friction force directions. pub elements: [VelocityConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS], pub num_contacts: u8, - pub im1: SimdReal, - pub im2: SimdReal, + pub im1: Vector<SimdReal>, + pub im2: Vector<SimdReal>, 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<SimdReal> = AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]); @@ -70,7 +70,7 @@ impl WVelocityConstraint { let angvel1 = AngVector::<SimdReal>::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<SimdReal> = 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, |