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

Fix velocity constraints for ball joints involving bodies with non-uniform angular inertia.

Browse Source 
Fix #86
This commit is contained in:
Crozet Sébastien
2021-01-20 17:03:36 +01:00
parent 0ade350b5f
commit d69b5876f3
4 changed files with 55 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples3d/joints3.rs
View File

@@ -196,16 +196,16 @@ fn create_ball_joints(
}; };
let rigid_body = RigidBodyBuilder::new(status) let rigid_body = RigidBodyBuilder::new(status)
.translation(fk * shift, 0.0, fi * shift) .translation(fk * shift, 0.0, fi * shift * 2.0)
.build(); .build();
let child_handle = bodies.insert(rigid_body); let child_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::ball(rad).build(); let collider = ColliderBuilder::capsule_z(rad * 1.25, rad).build();
colliders.insert(collider, child_handle, bodies); colliders.insert(collider, child_handle, bodies);
// Vertical joint. // Vertical joint.
if i > 0 { if i > 0 {
let parent_handle = *body_handles.last().unwrap(); let parent_handle = *body_handles.last().unwrap();
let joint = BallJoint::new(Point3::origin(), Point3::new(0.0, 0.0, -shift)); let joint = BallJoint::new(Point3::origin(), Point3::new(0.0, 0.0, -shift * 2.0));
joints.insert(bodies, parent_handle, child_handle, joint); joints.insert(bodies, parent_handle, child_handle, joint);
} }

46
src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
View File

@@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{ use crate::dynamics::{
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
}; };
use crate::math::{Real, SdpMatrix, Vector}; use crate::math::{AngularInertia, Real, SdpMatrix, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
#[derive(Debug)] #[derive(Debug)]
@@ -15,13 +15,16 @@ pub(crate) struct BallVelocityConstraint {
rhs: Vector<Real>, rhs: Vector<Real>,
pub(crate) impulse: Vector<Real>, pub(crate) impulse: Vector<Real>,
gcross1: Vector<Real>, r1: Vector<Real>,
gcross2: Vector<Real>, r2: Vector<Real>,
inv_lhs: SdpMatrix<Real>, inv_lhs: SdpMatrix<Real>,
im1: Real, im1: Real,
im2: Real, im2: Real,
ii1_sqrt: AngularInertia<Real>,
ii2_sqrt: AngularInertia<Real>,
} }
impl BallVelocityConstraint { impl BallVelocityConstraint {
@@ -72,9 +75,6 @@ impl BallVelocityConstraint {
lhs = SdpMatrix::new(m11, m12, m22) lhs = SdpMatrix::new(m11, m12, m22)
} }
let gcross1 = rb1.world_inv_inertia_sqrt.transform_lin_vector(anchor1);
let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2);
let inv_lhs = lhs.inverse_unchecked(); let inv_lhs = lhs.inverse_unchecked();
BallVelocityConstraint { BallVelocityConstraint {
@@ -84,10 +84,12 @@ impl BallVelocityConstraint {
im1, im1,
im2, im2,
impulse: cparams.impulse * params.warmstart_coeff, impulse: cparams.impulse * params.warmstart_coeff,
gcross1, r1: anchor1,
gcross2, r2: anchor2,
rhs, rhs,
inv_lhs, inv_lhs,
ii1_sqrt: rb1.world_inv_inertia_sqrt,
ii2_sqrt: rb2.world_inv_inertia_sqrt,
} }
} }
@@ -96,9 +98,9 @@ impl BallVelocityConstraint {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
mj_lambda1.linear += self.im1 * self.impulse; mj_lambda1.linear += self.im1 * self.impulse;
mj_lambda1.angular += self.gcross1.gcross(self.impulse); mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(self.impulse));
mj_lambda2.linear -= self.im2 * self.impulse; mj_lambda2.linear -= self.im2 * self.impulse;
mj_lambda2.angular -= self.gcross2.gcross(self.impulse); mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse));
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
@@ -108,18 +110,20 @@ impl BallVelocityConstraint {
let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1); let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
let vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
let dvel = -vel1 + vel2 + self.rhs; let dvel = -vel1 + vel2 + self.rhs;
let impulse = self.inv_lhs * dvel; let impulse = self.inv_lhs * dvel;
self.impulse += impulse; self.impulse += impulse;
mj_lambda1.linear += self.im1 * impulse; mj_lambda1.linear += self.im1 * impulse;
mj_lambda1.angular += self.gcross1.gcross(impulse); mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(impulse));
mj_lambda2.linear -= self.im2 * impulse; mj_lambda2.linear -= self.im2 * impulse;
mj_lambda2.angular -= self.gcross2.gcross(impulse); mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
@@ -139,9 +143,10 @@ pub(crate) struct BallVelocityGroundConstraint {
joint_id: JointIndex, joint_id: JointIndex,
rhs: Vector<Real>, rhs: Vector<Real>,
impulse: Vector<Real>, impulse: Vector<Real>,
gcross2: Vector<Real>, r2: Vector<Real>,
inv_lhs: SdpMatrix<Real>, inv_lhs: SdpMatrix<Real>,
im2: Real, im2: Real,
ii2_sqrt: AngularInertia<Real>,
} }
impl BallVelocityGroundConstraint { impl BallVelocityGroundConstraint {
@@ -171,7 +176,6 @@ impl BallVelocityGroundConstraint {
let rhs = vel2 - vel1; let rhs = vel2 - vel1;
let cmat2 = anchor2.gcross_matrix(); let cmat2 = anchor2.gcross_matrix();
let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2);
let lhs; let lhs;
@@ -200,30 +204,32 @@ impl BallVelocityGroundConstraint {
mj_lambda2: rb2.active_set_offset, mj_lambda2: rb2.active_set_offset,
im2, im2,
impulse: cparams.impulse * params.warmstart_coeff, impulse: cparams.impulse * params.warmstart_coeff,
gcross2, r2: anchor2,
rhs, rhs,
inv_lhs, inv_lhs,
ii2_sqrt: rb2.world_inv_inertia_sqrt,
} }
} }
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) { pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
mj_lambda2.linear -= self.im2 * self.impulse; mj_lambda2.linear -= self.im2 * self.impulse;
mj_lambda2.angular -= self.gcross2.gcross(self.impulse); mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse));
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
} }
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) { pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
let vel2 = mj_lambda2.linear + angvel.gcross(self.r2);
let dvel = vel2 + self.rhs; let dvel = vel2 + self.rhs;
let impulse = self.inv_lhs * dvel; let impulse = self.inv_lhs * dvel;
self.impulse += impulse; self.impulse += impulse;
mj_lambda2.linear -= self.im2 * impulse; mj_lambda2.linear -= self.im2 * impulse;
mj_lambda2.angular -= self.gcross2.gcross(impulse); mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
} }

44
src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
View File

@@ -18,13 +18,16 @@ pub(crate) struct WBallVelocityConstraint {
rhs: Vector<SimdReal>, rhs: Vector<SimdReal>,
pub(crate) impulse: Vector<SimdReal>, pub(crate) impulse: Vector<SimdReal>,
gcross1: Vector<SimdReal>, r1: Vector<SimdReal>,
gcross2: Vector<SimdReal>, r2: Vector<SimdReal>,
inv_lhs: SdpMatrix<SimdReal>, inv_lhs: SdpMatrix<SimdReal>,
im1: SimdReal, im1: SimdReal,
im2: SimdReal, im2: SimdReal,
ii1_sqrt: AngularInertia<SimdReal>,
ii2_sqrt: AngularInertia<SimdReal>,
} }
impl WBallVelocityConstraint { impl WBallVelocityConstraint {
@@ -88,9 +91,6 @@ impl WBallVelocityConstraint {
lhs = SdpMatrix::new(m11, m12, m22) lhs = SdpMatrix::new(m11, m12, m22)
} }
let gcross1 = ii1_sqrt.transform_lin_vector(anchor1);
let gcross2 = ii2_sqrt.transform_lin_vector(anchor2);
let inv_lhs = lhs.inverse_unchecked(); let inv_lhs = lhs.inverse_unchecked();
WBallVelocityConstraint { WBallVelocityConstraint {
@@ -100,10 +100,12 @@ impl WBallVelocityConstraint {
im1, im1,
im2, im2,
impulse: impulse * SimdReal::splat(params.warmstart_coeff), impulse: impulse * SimdReal::splat(params.warmstart_coeff),
gcross1, r1: anchor1,
gcross2, r2: anchor2,
rhs, rhs,
inv_lhs, inv_lhs,
ii1_sqrt,
ii2_sqrt,
} }
} }
@@ -126,9 +128,9 @@ impl WBallVelocityConstraint {
}; };
mj_lambda1.linear += self.impulse * self.im1; mj_lambda1.linear += self.impulse * self.im1;
mj_lambda1.angular += self.gcross1.gcross(self.impulse); mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(self.impulse));
mj_lambda2.linear -= self.impulse * self.im2; mj_lambda2.linear -= self.impulse * self.im2;
mj_lambda2.angular -= self.gcross2.gcross(self.impulse); mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse));
for ii in 0..SIMD_WIDTH { for ii in 0..SIMD_WIDTH {
mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
@@ -158,18 +160,20 @@ impl WBallVelocityConstraint {
), ),
}; };
let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1); let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
let vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
let dvel = -vel1 + vel2 + self.rhs; let dvel = -vel1 + vel2 + self.rhs;
let impulse = self.inv_lhs * dvel; let impulse = self.inv_lhs * dvel;
self.impulse += impulse; self.impulse += impulse;
mj_lambda1.linear += impulse * self.im1; mj_lambda1.linear += impulse * self.im1;
mj_lambda1.angular += self.gcross1.gcross(impulse); mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(impulse));
mj_lambda2.linear -= impulse * self.im2; mj_lambda2.linear -= impulse * self.im2;
mj_lambda2.angular -= self.gcross2.gcross(impulse); mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
for ii in 0..SIMD_WIDTH { for ii in 0..SIMD_WIDTH {
mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
@@ -197,9 +201,10 @@ pub(crate) struct WBallVelocityGroundConstraint {
joint_id: [JointIndex; SIMD_WIDTH], joint_id: [JointIndex; SIMD_WIDTH],
rhs: Vector<SimdReal>, rhs: Vector<SimdReal>,
pub(crate) impulse: Vector<SimdReal>, pub(crate) impulse: Vector<SimdReal>,
gcross2: Vector<SimdReal>, r2: Vector<SimdReal>,
inv_lhs: SdpMatrix<SimdReal>, inv_lhs: SdpMatrix<SimdReal>,
im2: SimdReal, im2: SimdReal,
ii2_sqrt: AngularInertia<SimdReal>,
} }
impl WBallVelocityGroundConstraint { impl WBallVelocityGroundConstraint {
@@ -243,7 +248,6 @@ impl WBallVelocityGroundConstraint {
let lhs; let lhs;
let cmat2 = anchor2.gcross_matrix(); let cmat2 = anchor2.gcross_matrix();
let gcross2 = ii2_sqrt.transform_lin_vector(anchor2);
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
{ {
@@ -268,9 +272,10 @@ impl WBallVelocityGroundConstraint {
mj_lambda2, mj_lambda2,
im2, im2,
impulse: impulse * SimdReal::splat(params.warmstart_coeff), impulse: impulse * SimdReal::splat(params.warmstart_coeff),
gcross2, r2: anchor2,
rhs, rhs,
inv_lhs, inv_lhs,
ii2_sqrt,
} }
} }
@@ -285,7 +290,7 @@ impl WBallVelocityGroundConstraint {
}; };
mj_lambda2.linear -= self.impulse * self.im2; mj_lambda2.linear -= self.impulse * self.im2;
mj_lambda2.angular -= self.gcross2.gcross(self.impulse); mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse));
for ii in 0..SIMD_WIDTH { for ii in 0..SIMD_WIDTH {
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
@@ -303,14 +308,15 @@ impl WBallVelocityGroundConstraint {
), ),
}; };
let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
let vel2 = mj_lambda2.linear + angvel.gcross(self.r2);
let dvel = vel2 + self.rhs; let dvel = vel2 + self.rhs;
let impulse = self.inv_lhs * dvel; let impulse = self.inv_lhs * dvel;
self.impulse += impulse; self.impulse += impulse;
mj_lambda2.linear -= impulse * self.im2; mj_lambda2.linear -= impulse * self.im2;
mj_lambda2.angular -= self.gcross2.gcross(impulse); mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
for ii in 0..SIMD_WIDTH { for ii in 0..SIMD_WIDTH {
mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);

2
src_testbed/objects/capsule.rs
View File

@@ -32,7 +32,7 @@ impl Capsule {
base_color: color, base_color: color,
gfx: node, gfx: node,
collider, collider,
delta, delta: delta * capsule.transform_wrt_y(),
}; };
res.gfx.set_color(color.x, color.y, color.z); res.gfx.set_color(color.x, color.y, color.z);