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

Allow locking individual translational axes

Browse Source
This commit is contained in:
Sébastien Crozet
2022-01-09 22:15:36 +01:00
parent 2bfceadf06
commit b631fe9193
16 changed files with 216 additions and 105 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")]
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")]

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?
#[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

51
src/dynamics/rigid_body_components.rs
View File

@@ -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.

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

@@ -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,

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

@@ -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.

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);
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")]
{

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 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;
}

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.
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;

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.
#[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,

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

@@ -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>,

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 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,

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.
#[cfg(feature = "dim3")]
pub tangent1: Vector<Real>, // One of the friction force directions.
pub im2: Real,
pub im2: Vector<Real>,
pub limit: Real,
pub elements: [VelocityGroundConstraintElement<Real>; 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,

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

@@ -29,7 +29,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
pub fn solve(
&mut self,
tangents1: [&Vector<N>; DIM - 1],
im2: N,
im2: &Vector<N>,
limit: N,
mj_lambda2: &mut DeltaVel<N>,
) where
@@ -45,7 +45,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
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<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
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<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
}
#[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
AngVector<N>: WDot<AngVector<N>, Result = N>,
{
@@ -111,7 +111,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
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<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
elements: &mut [Self],
dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>,
im2: N,
im2: &Vector<N>,
limit: N,
mj_lambda2: &mut DeltaVel<N>,
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 elements: [VelocityGroundConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub im2: SimdReal,
pub im2: Vector<SimdReal>,
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<SimdReal> =
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,

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
// 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() {