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

Fix some solver issues

Browse Source 
- Fix the wrong codepath taken  by the solver for contacts involving a collider without parent.
- Properly adress the non-linear treatment of the friction direction
- Simplify the sleeping strategy
- Add an impulse resolution multiplier
This commit is contained in:
Sébastien Crozet
2022-01-16 16:40:59 +01:00
parent 4454a845e9
commit 0703e5527f
43 changed files with 936 additions and 229 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -11,17 +11,22 @@ pub(crate) struct VelocityGroundConstraintTangentPart<N: SimdRealField + Copy> {
pub impulse: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub impulse: na::Vector2<N>,
pub r: [N; DIM - 1],
#[cfg(feature = "dim2")]
pub r: [N; 1],
#[cfg(feature = "dim3")]
pub r: [N; DIM],
}
impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
#[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
fn zero() -> Self {
Self {
gcross2: [na::zero(); DIM - 1],
rhs: [na::zero(); DIM - 1],
impulse: na::zero(),
r: [na::zero(); DIM - 1],
#[cfg(feature = "dim2")]
r: [na::zero(); 1],
#[cfg(feature = "dim3")]
r: [na::zero(); DIM],
}
}
@@ -38,10 +43,10 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
{
#[cfg(feature = "dim2")]
{
let dimpulse = -tangents1[0].dot(&mj_lambda2.linear)
let dvel = -tangents1[0].dot(&mj_lambda2.linear)
+ self.gcross2[0].gdot(mj_lambda2.angular)
+ self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dimpulse).simd_clamp(-limit, limit);
let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
@@ -51,17 +56,22 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
#[cfg(feature = "dim3")]
{
let dimpulse_0 = -tangents1[0].dot(&mj_lambda2.linear)
let dvel_0 = -tangents1[0].dot(&mj_lambda2.linear)
+ self.gcross2[0].gdot(mj_lambda2.angular)
+ self.rhs[0];
let dimpulse_1 = -tangents1[1].dot(&mj_lambda2.linear)
let dvel_1 = -tangents1[1].dot(&mj_lambda2.linear)
+ self.gcross2[1].gdot(mj_lambda2.angular)
+ self.rhs[1];
let new_impulse = na::Vector2::new(
self.impulse[0] - self.r[0] * dimpulse_0,
self.impulse[1] - self.r[1] * dimpulse_1,
);
let dvel_00 = dvel_0 * dvel_0;
let dvel_11 = dvel_1 * dvel_1;
let dvel_01 = dvel_0 * dvel_1;
let inv_lhs = (dvel_00 + dvel_11)
* crate::utils::simd_inv(
dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2],
);
let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1];
let new_impulse = self.impulse - delta_impulse;
let new_impulse = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
@@ -69,7 +79,6 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
new_impulse.simd_cap_magnitude(limit)
};
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
@@ -89,7 +98,6 @@ pub(crate) struct VelocityGroundConstraintNormalPart<N: SimdRealField + Copy> {
}
impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
#[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
fn zero() -> Self {
Self {
gcross2: na::zero(),
@@ -105,9 +113,8 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
where
AngVector<N>: WDot<AngVector<N>, Result = N>,
{
let dimpulse =
-dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs;
let new_impulse = (self.impulse - self.r * dimpulse).simd_max(N::zero());
let dvel = -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs;
let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
@@ -123,7 +130,6 @@ pub(crate) struct VelocityGroundConstraintElement<N: SimdRealField + Copy> {
}
impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
#[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
pub fn zero() -> Self {
Self {
normal_part: VelocityGroundConstraintNormalPart::zero(),