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

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