nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request #276 from dimforge/lock-translation-axis

Browse Source 
Allow locking individual translational axes
This commit is contained in:
Sébastien Crozet
2022-01-16 07:52:19 -08:00
committed by GitHub
parent 0ccd15c4b1 8213e92f14
commit 4454a845e9
17 changed files with 217 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
src/dynamics/joint/multibody_joint/multibody.rs
View File

@@ -35,23 +35,26 @@ impl Force {
} }
#[cfg(feature = "dim2")] #[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(); let mut result = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
result[(0, 0)] = mass; result[(0, 0)] = mass.x;
result[(1, 1)] = mass; result[(1, 1)] = mass.y;
result[(2, 2)] = inertia; result[(2, 2)] = inertia;
result result
} }
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
fn concat_rb_mass_matrix( fn concat_rb_mass_matrix(
mass: Real, mass: Vector<Real>,
inertia: Matrix<Real>, inertia: Matrix<Real>,
) -> SMatrix<Real, SPATIAL_DIM, SPATIAL_DIM> { ) -> SMatrix<Real, SPATIAL_DIM, SPATIAL_DIM> {
let mut result = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros(); let mut result = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
result[(0, 0)] = mass; result[(0, 0)] = mass.x;
result[(1, 1)] = mass; result[(1, 1)] = mass.y;
result[(2, 2)] = mass; result[(2, 2)] = mass.z;
result result
.fixed_slice_mut::<ANG_DIM, ANG_DIM>(DIM, DIM) .fixed_slice_mut::<ANG_DIM, ANG_DIM>(DIM, DIM)
.copy_from(&inertia); .copy_from(&inertia);
@@ -422,7 +425,7 @@ impl Multibody {
} }
let external_forces = Force::new( 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, rb_forces.torque - gyroscopic - rb_inertia * acc.angvel,
); );
self.accelerations.gemv_tr( 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); 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.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")] #[cfg(feature = "dim2")]

66
src/dynamics/rigid_body.rs
View File

@@ -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? /// 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 { pub fn is_translation_locked(&self) -> bool {
self.rb_mprops self.rb_mprops
.flags .flags
@@ -633,7 +674,7 @@ impl RigidBody {
/// This does nothing on non-dynamic bodies. /// This does nothing on non-dynamic bodies.
pub fn apply_impulse(&mut self, impulse: Vector<Real>, wake_up: bool) { pub fn apply_impulse(&mut self, impulse: Vector<Real>, wake_up: bool) {
if self.rb_type == RigidBodyType::Dynamic { 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 { if wake_up {
self.wake_up(true); self.wake_up(true);
@@ -847,6 +888,29 @@ impl RigidBodyBuilder {
self 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. /// Prevents this rigid-body from rotating because of forces.
pub fn lock_rotations(mut self) -> Self { pub fn lock_rotations(mut self) -> Self {
self.mprops_flags self.mprops_flags

51
src/dynamics/rigid_body_components.rs
View File

@@ -203,14 +203,20 @@ bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// Flags affecting the behavior of the constraints solver for a given contact manifold. /// Flags affecting the behavior of the constraints solver for a given contact manifold.
pub struct RigidBodyMassPropsFlags: u8 { 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. /// 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. /// 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. /// 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. /// 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. /// 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; 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. /// The world-space center of mass of the rigid-body.
pub world_com: Point<Real>, pub world_com: Point<Real>,
/// The inverse mass taking into account translation locking. /// 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, /// The square-root of the world-space inverse angular inertia tensor of the rigid-body,
/// taking into account rotation locking. /// taking into account rotation locking.
pub effective_world_inv_inertia_sqrt: AngularInertia<Real>, pub effective_world_inv_inertia_sqrt: AngularInertia<Real>,
@@ -240,7 +246,7 @@ impl Default for RigidBodyMassProps {
flags: RigidBodyMassPropsFlags::empty(), flags: RigidBodyMassPropsFlags::empty(),
local_mprops: MassProperties::zero(), local_mprops: MassProperties::zero(),
world_com: Point::origin(), world_com: Point::origin(),
effective_inv_mass: 0.0, effective_inv_mass: Vector::zero(),
effective_world_inv_inertia_sqrt: AngularInertia::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 /// The effective mass (that takes the potential translation locking into account) of
/// this rigid-body. /// this rigid-body.
#[must_use] #[must_use]
pub fn effective_mass(&self) -> Real { pub fn effective_mass(&self) -> Vector<Real> {
crate::utils::inv(self.effective_inv_mass) self.effective_inv_mass.map(crate::utils::inv)
} }
/// The effective world-space angular inertia (that takes the potential rotation locking into account) of /// 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. /// 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>) { pub fn update_world_mass_properties(&mut self, position: &Isometry<Real>) {
self.world_com = self.local_mprops.world_com(&position); 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.effective_world_inv_inertia_sqrt =
self.local_mprops.world_inv_inertia_sqrt(&position.rotation); self.local_mprops.world_inv_inertia_sqrt(&position.rotation);
// Take into account translation/rotation locking. // Take into account translation/rotation locking.
if self if self
.flags .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")] #[cfg(feature = "dim2")]
@@ -536,7 +557,7 @@ impl RigidBodyVelocity {
/// The impulse is applied right away, changing the linear velocity. /// The impulse is applied right away, changing the linear velocity.
/// This does nothing on non-dynamic bodies. /// This does nothing on non-dynamic bodies.
pub fn apply_impulse(&mut self, rb_mprops: &RigidBodyMassProps, impulse: Vector<Real>) { 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. /// Applies an angular impulse at the center-of-mass of this rigid-body.
@@ -659,7 +680,7 @@ impl RigidBodyForces {
init_vels: &RigidBodyVelocity, init_vels: &RigidBodyVelocity,
mprops: &RigidBodyMassProps, mprops: &RigidBodyMassProps,
) -> RigidBodyVelocity { ) -> 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 let angular_acc = mprops.effective_world_inv_inertia_sqrt
* (mprops.effective_world_inv_inertia_sqrt * self.torque); * (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 /// Adds to `self` the gravitational force that would result in a gravitational acceleration
/// equal to `gravity`. /// equal to `gravity`.
pub fn add_gravity_acceleration(&mut self, gravity: &Vector<Real>, mass: Real) { pub fn add_gravity_acceleration(&mut self, gravity: &Vector<Real>, mass: &Vector<Real>) {
self.force += gravity * self.gravity_scale * mass; 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. /// Applies a force at the given world-space point of the rigid-body with the given mass properties.

22
src/dynamics/solver/generic_velocity_constraint.rs
View File

@@ -116,12 +116,12 @@ impl GenericVelocityConstraint {
im1: if rb_type1.is_dynamic() { im1: if rb_type1.is_dynamic() {
rb_mprops1.effective_inv_mass rb_mprops1.effective_inv_mass
} else { } else {
0.0 na::zero()
}, },
im2: if rb_type2.is_dynamic() { im2: if rb_type2.is_dynamic() {
rb_mprops2.effective_inv_mass rb_mprops2.effective_inv_mass
} else { } else {
0.0 na::zero()
}, },
limit: 0.0, limit: 0.0,
mj_lambda1, mj_lambda1,
@@ -175,7 +175,8 @@ impl GenericVelocityConstraint {
) )
.0 .0
} else if rb_type1.is_dynamic() { } 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 { } else {
0.0 0.0
}; };
@@ -193,7 +194,8 @@ impl GenericVelocityConstraint {
) )
.0 .0
} else if rb_type2.is_dynamic() { } 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 { } else {
0.0 0.0
}; };
@@ -258,7 +260,9 @@ impl GenericVelocityConstraint {
) )
.0 .0
} else if rb_type1.is_dynamic() { } 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 { } else {
0.0 0.0
}; };
@@ -276,7 +280,9 @@ impl GenericVelocityConstraint {
) )
.0 .0
} else if rb_type2.is_dynamic() { } 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 { } else {
0.0 0.0
}; };
@@ -345,8 +351,8 @@ impl GenericVelocityConstraint {
&self.velocity_constraint.dir1, &self.velocity_constraint.dir1,
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
&self.velocity_constraint.tangent1, &self.velocity_constraint.tangent1,
self.velocity_constraint.im1, &self.velocity_constraint.im1,
self.velocity_constraint.im2, &self.velocity_constraint.im2,
self.velocity_constraint.limit, self.velocity_constraint.limit,
self.ndofs1, self.ndofs1,
self.ndofs2, self.ndofs2,

16
src/dynamics/solver/generic_velocity_constraint_element.rs
View File

@@ -70,11 +70,11 @@ impl GenericRhs {
dir: &Vector<Real>, dir: &Vector<Real>,
gcross: &AngVector<Real>, gcross: &AngVector<Real>,
mj_lambdas: &mut DVector<Real>, mj_lambdas: &mut DVector<Real>,
inv_mass: Real, inv_mass: &Vector<Real>,
) { ) {
match self { match self {
GenericRhs::DeltaVel(rhs) => { GenericRhs::DeltaVel(rhs) => {
rhs.linear += dir * (inv_mass * impulse); rhs.linear += dir.component_mul(inv_mass) * impulse;
rhs.angular += gcross * impulse; rhs.angular += gcross * impulse;
} }
GenericRhs::GenericId(mj_lambda) => { GenericRhs::GenericId(mj_lambda) => {
@@ -94,8 +94,8 @@ impl VelocityConstraintTangentPart<Real> {
j_id: usize, j_id: usize,
jacobians: &DVector<Real>, jacobians: &DVector<Real>,
tangents1: [&Vector<Real>; DIM - 1], tangents1: [&Vector<Real>; DIM - 1],
im1: Real, im1: &Vector<Real>,
im2: Real, im2: &Vector<Real>,
ndofs1: usize, ndofs1: usize,
ndofs2: usize, ndofs2: usize,
limit: Real, limit: Real,
@@ -246,8 +246,8 @@ impl VelocityConstraintNormalPart<Real> {
j_id: usize, j_id: usize,
jacobians: &DVector<Real>, jacobians: &DVector<Real>,
dir1: &Vector<Real>, dir1: &Vector<Real>,
im1: Real, im1: &Vector<Real>,
im2: Real, im2: &Vector<Real>,
ndofs1: usize, ndofs1: usize,
ndofs2: usize, ndofs2: usize,
mj_lambda1: &mut GenericRhs, mj_lambda1: &mut GenericRhs,
@@ -296,8 +296,8 @@ impl VelocityConstraintElement<Real> {
jacobians: &DVector<Real>, jacobians: &DVector<Real>,
dir1: &Vector<Real>, dir1: &Vector<Real>,
#[cfg(feature = "dim3")] tangent1: &Vector<Real>, #[cfg(feature = "dim3")] tangent1: &Vector<Real>,
im1: Real, im1: &Vector<Real>,
im2: Real, im2: &Vector<Real>,
limit: Real, limit: Real,
// ndofs is 0 for a non-multibody body, or a multibody with zero // ndofs is 0 for a non-multibody body, or a multibody with zero
// degrees of freedom. // degrees of freedom.

2
src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs
View File

@@ -33,7 +33,7 @@ impl SolverBody<Real, 1> {
let mut out_invm_j = jacobians.fixed_rows_mut::<SPATIAL_DIM>(wj_id); let mut out_invm_j = jacobians.fixed_rows_mut::<SPATIAL_DIM>(wj_id);
out_invm_j out_invm_j
.fixed_rows_mut::<DIM>(0) .fixed_rows_mut::<DIM>(0)
.axpy(self.im, &unit_force, 0.0); .copy_from(&self.im.component_mul(&unit_force));
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
{ {

20
src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
View File

@@ -51,7 +51,7 @@ pub enum WritebackId {
pub struct SolverBody<N: SimdRealField, const LANES: usize> { pub struct SolverBody<N: SimdRealField, const LANES: usize> {
pub linvel: Vector<N>, pub linvel: Vector<N>,
pub angvel: AngVector<N>, pub angvel: AngVector<N>,
pub im: N, pub im: Vector<N>,
pub sqrt_ii: AngularInertia<N>, pub sqrt_ii: AngularInertia<N>,
pub world_com: Point<N>, pub world_com: Point<N>,
pub mj_lambda: [usize; LANES], pub mj_lambda: [usize; LANES],
@@ -74,8 +74,8 @@ pub struct JointVelocityConstraint<N: SimdRealField, const LANES: usize> {
pub rhs: N, pub rhs: N,
pub rhs_wo_bias: N, pub rhs_wo_bias: N,
pub im1: N, pub im1: Vector<N>,
pub im2: N, pub im2: Vector<N>,
pub writeback_id: WritebackId, pub writeback_id: WritebackId,
} }
@@ -94,8 +94,8 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
inv_lhs: N::zero(), inv_lhs: N::zero(),
rhs: N::zero(), rhs: N::zero(),
rhs_wo_bias: N::zero(), rhs_wo_bias: N::zero(),
im1: N::zero(), im1: na::zero(),
im2: N::zero(), im2: na::zero(),
writeback_id: WritebackId::Dof(0), 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_impulse1 = self.ang_jac1 * delta_impulse;
let ang_impulse2 = self.ang_jac2 * 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_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; mj_lambda2.angular -= ang_impulse2;
} }
@@ -353,7 +353,7 @@ pub struct JointVelocityGroundConstraint<N: SimdRealField, const LANES: usize> {
pub rhs: N, pub rhs: N,
pub rhs_wo_bias: N, pub rhs_wo_bias: N,
pub im2: N, pub im2: Vector<N>,
pub writeback_id: WritebackId, pub writeback_id: WritebackId,
} }
@@ -370,7 +370,7 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
inv_lhs: N::zero(), inv_lhs: N::zero(),
rhs: N::zero(), rhs: N::zero(),
rhs_wo_bias: N::zero(), rhs_wo_bias: N::zero(),
im2: N::zero(), im2: na::zero(),
writeback_id: WritebackId::Dof(0), 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 lin_impulse = self.lin_jac * delta_impulse;
let ang_impulse = self.ang_jac2 * 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; mj_lambda2.angular -= ang_impulse;
} }

11
src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
View File

@@ -355,7 +355,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
// Use the modified Gram-Schmidt orthogonalization. // Use the modified Gram-Schmidt orthogonalization.
for j in 0..len { for j in 0..len {
let c_j = &mut constraints[j]; 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_jac1.gdot(c_j.ang_jac1)
+ c_j.ang_jac2.gdot(c_j.ang_jac2); + c_j.ang_jac2.gdot(c_j.ang_jac2);
let inv_dot_jj = crate::utils::simd_inv(dot_jj); 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 { for i in (j + 1)..len {
let (c_i, c_j) = constraints.index_mut_const(i, j); 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_jac1.gdot(c_j.ang_jac1)
+ c_i.ang_jac2.gdot(c_j.ang_jac2); + c_i.ang_jac2.gdot(c_j.ang_jac2);
let coeff = dot_ij * inv_dot_jj; let coeff = dot_ij * inv_dot_jj;
@@ -672,7 +672,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
// Use the modified Gram-Schmidt orthogonalization. // Use the modified Gram-Schmidt orthogonalization.
for j in 0..len { for j in 0..len {
let c_j = &mut constraints[j]; 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); 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. 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 { for i in (j + 1)..len {
let (c_i, c_j) = constraints.index_mut_const(i, j); let (c_i, c_j) = constraints.index_mut_const(i, j);
let dot_ij = let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
c_i.lin_jac.dot(&c_j.lin_jac) * imsum + c_i.ang_jac2.gdot(c_j.ang_jac2); + c_i.ang_jac2.gdot(c_j.ang_jac2);
let coeff = dot_ij * inv_dot_jj; let coeff = dot_ij * inv_dot_jj;
c_i.lin_jac -= c_j.lin_jac * coeff; c_i.lin_jac -= c_j.lin_jac * coeff;

2
src/dynamics/solver/parallel_island_solver.rs
View File

@@ -247,7 +247,7 @@ impl ParallelIslandSolver {
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor: // by the square root of the inertia tensor:
dvel.angular += rb_mass_props.effective_world_inv_inertia_sqrt * rb_forces.torque * params.dt; 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;
} }
} }

16
src/dynamics/solver/velocity_constraint.rs
View File

@@ -94,8 +94,8 @@ pub(crate) struct VelocityConstraint {
pub dir1: Vector<Real>, // Non-penetration force direction for the first body. pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
pub tangent1: Vector<Real>, // One of the friction force directions. pub tangent1: Vector<Real>, // One of the friction force directions.
pub im1: Real, pub im1: Vector<Real>,
pub im2: Real, pub im2: Vector<Real>,
pub limit: Real, pub limit: Real,
pub mj_lambda1: usize, pub mj_lambda1: usize,
pub mj_lambda2: usize, pub mj_lambda2: usize,
@@ -235,9 +235,9 @@ impl VelocityConstraint {
.effective_world_inv_inertia_sqrt .effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1)); .transform_vector(dp2.gcross(-force_dir1));
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let r = 1.0 let r = 1.0
/ (mprops1.effective_inv_mass / (force_dir1.dot(&imsum.component_mul(&force_dir1))
+ mprops2.effective_inv_mass
+ gcross1.gdot(gcross1) + gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2)); + gcross2.gdot(gcross2));
@@ -274,9 +274,9 @@ impl VelocityConstraint {
let gcross2 = mprops2 let gcross2 = mprops2
.effective_world_inv_inertia_sqrt .effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j])); .transform_vector(dp2.gcross(-tangents1[j]));
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let r = 1.0 let r = 1.0
/ (mprops1.effective_inv_mass / (tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ mprops2.effective_inv_mass
+ gcross1.gdot(gcross1) + gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2)); + gcross2.gdot(gcross2));
let rhs = let rhs =
@@ -314,8 +314,8 @@ impl VelocityConstraint {
&self.dir1, &self.dir1,
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
&self.tangent1, &self.tangent1,
self.im1, &self.im1,
self.im2, &self.im2,
self.limit, self.limit,
&mut mj_lambda1, &mut mj_lambda1,
&mut mj_lambda2, &mut mj_lambda2,

28
src/dynamics/solver/velocity_constraint_element.rs
View File

@@ -30,8 +30,8 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
pub fn solve( pub fn solve(
&mut self, &mut self,
tangents1: [&Vector<N>; DIM - 1], tangents1: [&Vector<N>; DIM - 1],
im1: N, im1: &Vector<N>,
im2: N, im2: &Vector<N>,
limit: N, limit: N,
mj_lambda1: &mut DeltaVel<N>, mj_lambda1: &mut DeltaVel<N>,
mj_lambda2: &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]; let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse; 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_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; mj_lambda2.angular += self.gcross2[0] * dlambda;
} }
@@ -84,12 +84,12 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
let dlambda = new_impulse - self.impulse; let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse; self.impulse = new_impulse;
mj_lambda1.linear += mj_lambda1.linear += tangents1[0].component_mul(im1) * dlambda[0]
tangents1[0] * (im1 * dlambda[0]) + tangents1[1] * (im1 * dlambda[1]); + tangents1[1].component_mul(im1) * dlambda[1];
mj_lambda1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1]; mj_lambda1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1];
mj_lambda2.linear += mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
tangents1[0] * (-im2 * dlambda[0]) + tangents1[1] * (-im2 * dlambda[1]); + tangents1[1].component_mul(im2) * -dlambda[1];
mj_lambda2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * 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( pub fn solve(
&mut self, &mut self,
dir1: &Vector<N>, dir1: &Vector<N>,
im1: N, im1: &Vector<N>,
im2: N, im2: &Vector<N>,
mj_lambda1: &mut DeltaVel<N>, mj_lambda1: &mut DeltaVel<N>,
mj_lambda2: &mut DeltaVel<N>, mj_lambda2: &mut DeltaVel<N>,
) where ) where
@@ -136,10 +136,10 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
let dlambda = new_impulse - self.impulse; let dlambda = new_impulse - self.impulse;
self.impulse = new_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_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; mj_lambda2.angular += self.gcross2 * dlambda;
} }
} }
@@ -163,8 +163,8 @@ impl<N: SimdRealField + Copy> VelocityConstraintElement<N> {
elements: &mut [Self], elements: &mut [Self],
dir1: &Vector<N>, dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>, #[cfg(feature = "dim3")] tangent1: &Vector<N>,
im1: N, im1: &Vector<N>,
im2: N, im2: &Vector<N>,
limit: N, limit: N,
mj_lambda1: &mut DeltaVel<N>, mj_lambda1: &mut DeltaVel<N>,
mj_lambda2: &mut DeltaVel<N>, mj_lambda2: &mut DeltaVel<N>,

22
src/dynamics/solver/velocity_constraint_wide.rs
View File

@@ -20,8 +20,8 @@ pub(crate) struct WVelocityConstraint {
pub tangent1: Vector<SimdReal>, // One of the friction force directions. pub tangent1: Vector<SimdReal>, // One of the friction force directions.
pub elements: [VelocityConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS], pub elements: [VelocityConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
pub num_contacts: u8, pub num_contacts: u8,
pub im1: SimdReal, pub im1: Vector<SimdReal>,
pub im2: SimdReal, pub im2: Vector<SimdReal>,
pub limit: SimdReal, pub limit: SimdReal,
pub mj_lambda1: [usize; SIMD_WIDTH], pub mj_lambda1: [usize; SIMD_WIDTH],
pub mj_lambda2: [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 mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); 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> = let ii1: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]); 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 angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); 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> = let ii2: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]); 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 gcross1 = ii1.transform_vector(dp1.gcross(force_dir1));
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let imsum = im1 + im2;
let r = SimdReal::splat(1.0) 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 projected_velocity = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias = let mut rhs_wo_bias =
(SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
@@ -161,8 +164,11 @@ impl WVelocityConstraint {
for j in 0..DIM - 1 { for j in 0..DIM - 1 {
let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j])); let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let imsum = im1 + im2;
let r = SimdReal::splat(1.0) 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]); let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross1[j] = gcross1; constraint.elements[k].tangent_part.gcross1[j] = gcross1;
@@ -206,8 +212,8 @@ impl WVelocityConstraint {
&self.dir1, &self.dir1,
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
&self.tangent1, &self.tangent1,
self.im1, &self.im1,
self.im2, &self.im2,
self.limit, self.limit,
&mut mj_lambda1, &mut mj_lambda1,
&mut mj_lambda2, &mut mj_lambda2,

13
src/dynamics/solver/velocity_ground_constraint.rs
View File

@@ -17,7 +17,7 @@ pub(crate) struct VelocityGroundConstraint {
pub dir1: Vector<Real>, // Non-penetration force direction for the first body. pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
pub tangent1: Vector<Real>, // One of the friction force directions. pub tangent1: Vector<Real>, // One of the friction force directions.
pub im2: Real, pub im2: Vector<Real>,
pub limit: Real, pub limit: Real,
pub elements: [VelocityGroundConstraintElement<Real>; MAX_MANIFOLD_POINTS], pub elements: [VelocityGroundConstraintElement<Real>; MAX_MANIFOLD_POINTS],
@@ -153,7 +153,9 @@ impl VelocityGroundConstraint {
.effective_world_inv_inertia_sqrt .effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1)); .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_bouncy = manifold_point.is_bouncy() as u32 as Real;
let is_resting = 1.0 - is_bouncy; let is_resting = 1.0 - is_bouncy;
@@ -184,7 +186,10 @@ impl VelocityGroundConstraint {
let gcross2 = mprops2 let gcross2 = mprops2
.effective_world_inv_inertia_sqrt .effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j])); .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 let rhs = (vel1 - vel2
+ flipped_multiplier * manifold_point.tangent_velocity) + flipped_multiplier * manifold_point.tangent_velocity)
.dot(&tangents1[j]); .dot(&tangents1[j]);
@@ -219,7 +224,7 @@ impl VelocityGroundConstraint {
&self.dir1, &self.dir1,
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
&self.tangent1, &self.tangent1,
self.im2, &self.im2,
self.limit, self.limit,
&mut mj_lambda2, &mut mj_lambda2,
solve_normal, solve_normal,

14
src/dynamics/solver/velocity_ground_constraint_element.rs
View File

@@ -29,7 +29,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
pub fn solve( pub fn solve(
&mut self, &mut self,
tangents1: [&Vector<N>; DIM - 1], tangents1: [&Vector<N>; DIM - 1],
im2: N, im2: &Vector<N>,
limit: N, limit: N,
mj_lambda2: &mut DeltaVel<N>, mj_lambda2: &mut DeltaVel<N>,
) where ) where
@@ -45,7 +45,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
let dlambda = new_impulse - self.impulse[0]; let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse; 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; mj_lambda2.angular += self.gcross2[0] * dlambda;
} }
@@ -72,8 +72,8 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
self.impulse = new_impulse; self.impulse = new_impulse;
mj_lambda2.linear += mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
tangents1[0] * (-im2 * dlambda[0]) + tangents1[1] * (-im2 * dlambda[1]); + tangents1[1].component_mul(im2) * -dlambda[1];
mj_lambda2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1]; mj_lambda2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
} }
} }
@@ -101,7 +101,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
} }
#[inline] #[inline]
pub fn solve(&mut self, dir1: &Vector<N>, im2: N, mj_lambda2: &mut DeltaVel<N>) pub fn solve(&mut self, dir1: &Vector<N>, im2: &Vector<N>, mj_lambda2: &mut DeltaVel<N>)
where where
AngVector<N>: WDot<AngVector<N>, Result = N>, AngVector<N>: WDot<AngVector<N>, Result = N>,
{ {
@@ -111,7 +111,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
let dlambda = new_impulse - self.impulse; let dlambda = new_impulse - self.impulse;
self.impulse = new_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; mj_lambda2.angular += self.gcross2 * dlambda;
} }
} }
@@ -136,7 +136,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
elements: &mut [Self], elements: &mut [Self],
dir1: &Vector<N>, dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>, #[cfg(feature = "dim3")] tangent1: &Vector<N>,
im2: N, im2: &Vector<N>,
limit: N, limit: N,
mj_lambda2: &mut DeltaVel<N>, mj_lambda2: &mut DeltaVel<N>,
solve_normal: bool, solve_normal: bool,

13
src/dynamics/solver/velocity_ground_constraint_wide.rs
View File

@@ -21,7 +21,7 @@ pub(crate) struct WVelocityGroundConstraint {
pub tangent1: Vector<SimdReal>, // One of the friction force directions. pub tangent1: Vector<SimdReal>, // One of the friction force directions.
pub elements: [VelocityGroundConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS], pub elements: [VelocityGroundConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
pub num_contacts: u8, pub num_contacts: u8,
pub im2: SimdReal, pub im2: Vector<SimdReal>,
pub limit: SimdReal, pub limit: SimdReal,
pub mj_lambda2: [usize; SIMD_WIDTH], pub mj_lambda2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
@@ -76,7 +76,7 @@ impl WVelocityGroundConstraint {
let flipped_sign = SimdReal::from(flipped); 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<SimdReal> = let ii2: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]); 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 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 projected_velocity = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias = let mut rhs_wo_bias =
(SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
@@ -165,7 +166,9 @@ impl WVelocityGroundConstraint {
for j in 0..DIM - 1 { for j in 0..DIM - 1 {
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); 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]); let rhs = (vel1 - vel2 + tangent_velocity * flipped_sign).dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross2[j] = gcross2; constraint.elements[k].tangent_part.gcross2[j] = gcross2;
@@ -201,7 +204,7 @@ impl WVelocityGroundConstraint {
&self.dir1, &self.dir1,
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
&self.tangent1, &self.tangent1,
self.im2, &self.im2,
self.limit, self.limit,
&mut mj_lambda2, &mut mj_lambda2,
solve_normal, solve_normal,

2
src/dynamics/solver/velocity_solver.rs
View File

@@ -67,7 +67,7 @@ impl VelocitySolver {
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor: // by the square root of the inertia tensor:
dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt; 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() { for (_, multibody) in multibodies.multibodies.iter_mut() {

2
src/pipeline/physics_pipeline.rs
View File

@@ -223,7 +223,7 @@ impl PhysicsPipeline {
}) })
.unwrap(); .unwrap();
bodies.map_mut_internal(handle.0, |forces: &mut RigidBodyForces| { 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)
}); });
} }