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

Rotation locking: apply filter only to the world inertia properties to fix the multi-collider case.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-01-21 14:58:40 +01:00
parent d69b5876f3
commit 8f330b2a00
25 changed files with 232 additions and 244 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
src/dynamics/solver/joint_constraint/ball_position_constraint.rs
View File

@@ -27,10 +27,10 @@ impl BallPositionConstraint {
Self {
local_com1: rb1.mass_properties.local_com,
local_com2: rb2.mass_properties.local_com,
im1: rb1.mass_properties.inv_mass,
im2: rb2.mass_properties.inv_mass,
ii1: rb1.world_inv_inertia_sqrt.squared(),
ii2: rb2.world_inv_inertia_sqrt.squared(),
im1: rb1.effective_inv_mass,
im2: rb2.effective_inv_mass,
ii1: rb1.effective_world_inv_inertia_sqrt.squared(),
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_anchor1: cparams.local_anchor1,
local_anchor2: cparams.local_anchor2,
position1: rb1.active_set_offset,
@@ -115,8 +115,8 @@ impl BallPositionGroundConstraint {
// already been flipped by the caller.
Self {
anchor1: rb1.predicted_position * cparams.local_anchor2,
im2: rb2.mass_properties.inv_mass,
ii2: rb2.world_inv_inertia_sqrt.squared(),
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_anchor2: cparams.local_anchor1,
position2: rb2.active_set_offset,
local_com2: rb2.mass_properties.local_com,
@@ -124,8 +124,8 @@ impl BallPositionGroundConstraint {
} else {
Self {
anchor1: rb1.predicted_position * cparams.local_anchor1,
im2: rb2.mass_properties.inv_mass,
ii2: rb2.world_inv_inertia_sqrt.squared(),
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_anchor2: cparams.local_anchor2,
position2: rb2.active_set_offset,
local_com2: rb2.mass_properties.local_com,

12
src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
View File

@@ -31,14 +31,14 @@ impl WBallPositionConstraint {
) -> Self {
let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]);
let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1 = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
)
.squared();
let ii2 = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
)
.squared();
let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
@@ -141,9 +141,9 @@ impl WBallPositionGroundConstraint {
} else {
cparams[ii].local_anchor1
}; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2 = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
)
.squared();
let local_anchor2 = Point::from(array![|ii| if flipped[ii] {

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

@@ -40,8 +40,8 @@ impl BallVelocityConstraint {
let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
let im1 = rb1.mass_properties.inv_mass;
let im2 = rb2.mass_properties.inv_mass;
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let rhs = -(vel1 - vel2);
let lhs;
@@ -52,12 +52,12 @@ impl BallVelocityConstraint {
#[cfg(feature = "dim3")]
{
lhs = rb2
.world_inv_inertia_sqrt
.effective_world_inv_inertia_sqrt
.squared()
.quadform(&cmat2)
.add_diagonal(im2)
+ rb1
.world_inv_inertia_sqrt
.effective_world_inv_inertia_sqrt
.squared()
.quadform(&cmat1)
.add_diagonal(im1);
@@ -67,8 +67,8 @@ impl BallVelocityConstraint {
// it's just easier that way.
#[cfg(feature = "dim2")]
{
let ii1 = rb1.world_inv_inertia_sqrt.squared();
let ii2 = rb2.world_inv_inertia_sqrt.squared();
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2;
let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2;
let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2;
@@ -88,8 +88,8 @@ impl BallVelocityConstraint {
r2: anchor2,
rhs,
inv_lhs,
ii1_sqrt: rb1.world_inv_inertia_sqrt,
ii2_sqrt: rb2.world_inv_inertia_sqrt,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
}
}
@@ -170,7 +170,7 @@ impl BallVelocityGroundConstraint {
)
};
let im2 = rb2.mass_properties.inv_mass;
let im2 = rb2.effective_inv_mass;
let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
let rhs = vel2 - vel1;
@@ -182,7 +182,7 @@ impl BallVelocityGroundConstraint {
#[cfg(feature = "dim3")]
{
lhs = rb2
.world_inv_inertia_sqrt
.effective_world_inv_inertia_sqrt
.squared()
.quadform(&cmat2)
.add_diagonal(im2);
@@ -190,7 +190,7 @@ impl BallVelocityGroundConstraint {
#[cfg(feature = "dim2")]
{
let ii2 = rb2.world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let m11 = im2 + cmat2.x * cmat2.x * ii2;
let m12 = cmat2.x * cmat2.y * ii2;
let m22 = im2 + cmat2.y * cmat2.y * ii2;
@@ -207,7 +207,7 @@ impl BallVelocityGroundConstraint {
r2: anchor2,
rhs,
inv_lhs,
ii2_sqrt: rb2.world_inv_inertia_sqrt,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
}
}

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

@@ -42,9 +42,9 @@ impl WBallVelocityConstraint {
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
@@ -52,9 +52,9 @@ impl WBallVelocityConstraint {
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -228,9 +228,9 @@ impl WBallVelocityGroundConstraint {
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];

12
src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
View File

@@ -21,10 +21,10 @@ pub(crate) struct FixedPositionConstraint {
impl FixedPositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self {
let ii1 = rb1.world_inv_inertia_sqrt.squared();
let ii2 = rb2.world_inv_inertia_sqrt.squared();
let im1 = rb1.mass_properties.inv_mass;
let im2 = rb2.mass_properties.inv_mass;
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let lin_inv_lhs = 1.0 / (im1 + im2);
let ang_inv_lhs = (ii1 + ii2).inverse();
@@ -111,8 +111,8 @@ impl FixedPositionGroundConstraint {
anchor1,
local_anchor2,
position2: rb2.active_set_offset,
im2: rb2.mass_properties.inv_mass,
ii2: rb2.world_inv_inertia_sqrt.squared(),
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_com2: rb2.mass_properties.local_com,
impulse: 0.0,
}

18
src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
View File

@@ -51,10 +51,10 @@ impl FixedVelocityConstraint {
) -> Self {
let anchor1 = rb1.position * cparams.local_anchor1;
let anchor2 = rb2.position * cparams.local_anchor2;
let im1 = rb1.mass_properties.inv_mass;
let im2 = rb2.mass_properties.inv_mass;
let ii1 = rb1.world_inv_inertia_sqrt.squared();
let ii2 = rb2.world_inv_inertia_sqrt.squared();
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1.translation.vector - rb1.world_com.coords;
let r2 = anchor2.translation.vector - rb2.world_com.coords;
let rmat1 = r1.gcross_matrix();
@@ -118,8 +118,8 @@ impl FixedVelocityConstraint {
im2,
ii1,
ii2,
ii1_sqrt: rb1.world_inv_inertia_sqrt,
ii2_sqrt: rb2.world_inv_inertia_sqrt,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
impulse: cparams.impulse * params.warmstart_coeff,
inv_lhs,
r1,
@@ -248,8 +248,8 @@ impl FixedVelocityGroundConstraint {
let r1 = anchor1.translation.vector - rb1.world_com.coords;
let im2 = rb2.mass_properties.inv_mass;
let ii2 = rb2.world_inv_inertia_sqrt.squared();
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2.translation.vector - rb2.world_com.coords;
let rmat2 = r2.gcross_matrix();
@@ -304,7 +304,7 @@ impl FixedVelocityGroundConstraint {
mj_lambda2: rb2.active_set_offset,
im2,
ii2,
ii2_sqrt: rb2.world_inv_inertia_sqrt,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
impulse: cparams.impulse * params.warmstart_coeff,
inv_lhs,
r2,

12
src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
View File

@@ -61,9 +61,9 @@ impl WFixedVelocityConstraint {
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
@@ -71,9 +71,9 @@ impl WFixedVelocityConstraint {
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -315,9 +315,9 @@ impl WFixedVelocityGroundConstraint {
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];

8
src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
View File

@@ -28,10 +28,10 @@ pub(crate) struct PrismaticPositionConstraint {
impl PrismaticPositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &PrismaticJoint) -> Self {
let ii1 = rb1.world_inv_inertia_sqrt.squared();
let ii2 = rb2.world_inv_inertia_sqrt.squared();
let im1 = rb1.mass_properties.inv_mass;
let im2 = rb2.mass_properties.inv_mass;
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let lin_inv_lhs = 1.0 / (im1 + im2);
let ang_inv_lhs = (ii1 + ii2).inverse();

18
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
View File

@@ -92,13 +92,13 @@ impl PrismaticVelocityConstraint {
// simplifications of the computation without introducing
// much instabilities.
let im1 = rb1.mass_properties.inv_mass;
let ii1 = rb1.world_inv_inertia_sqrt.squared();
let im1 = rb1.effective_inv_mass;
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let r1_mat = r1.gcross_matrix();
let im2 = rb2.mass_properties.inv_mass;
let ii2 = rb2.world_inv_inertia_sqrt.squared();
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2 - rb2.world_com;
let r2_mat = r2.gcross_matrix();
@@ -176,9 +176,9 @@ impl PrismaticVelocityConstraint {
mj_lambda1: rb1.active_set_offset,
mj_lambda2: rb2.active_set_offset,
im1,
ii1_sqrt: rb1.world_inv_inertia_sqrt,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
im2,
ii2_sqrt: rb2.world_inv_inertia_sqrt,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
impulse: cparams.impulse * params.warmstart_coeff,
limits_impulse: limits_impulse * params.warmstart_coeff,
limits_forcedirs,
@@ -388,8 +388,8 @@ impl PrismaticVelocityGroundConstraint {
// simplifications of the computation without introducing
// much instabilities.
let im2 = rb2.mass_properties.inv_mass;
let ii2 = rb2.world_inv_inertia_sqrt.squared();
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let r2 = anchor2 - rb2.world_com;
let r2_mat = r2.gcross_matrix();
@@ -465,7 +465,7 @@ impl PrismaticVelocityGroundConstraint {
joint_id,
mj_lambda2: rb2.active_set_offset,
im2,
ii2_sqrt: rb2.world_inv_inertia_sqrt,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
impulse: cparams.impulse * params.warmstart_coeff,
limits_impulse: limits_impulse * params.warmstart_coeff,
basis1,

12
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
View File

@@ -73,9 +73,9 @@ impl WPrismaticVelocityConstraint {
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
@@ -83,9 +83,9 @@ impl WPrismaticVelocityConstraint {
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -431,9 +431,9 @@ impl WPrismaticVelocityGroundConstraint {
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];

8
src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
View File

@@ -26,10 +26,10 @@ pub(crate) struct RevolutePositionConstraint {
impl RevolutePositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &RevoluteJoint) -> Self {
let ii1 = rb1.world_inv_inertia_sqrt.squared();
let ii2 = rb2.world_inv_inertia_sqrt.squared();
let im1 = rb1.mass_properties.inv_mass;
let im2 = rb2.mass_properties.inv_mass;
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let lin_inv_lhs = 1.0 / (im1 + im2);
let ang_inv_lhs = (ii1 + ii2).inverse();

18
src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
View File

@@ -52,14 +52,14 @@ impl RevoluteVelocityConstraint {
// let basis2 = r21 * basis1;
// NOTE: to simplify, we use basis2 = basis1.
// Though we may want to test if that does not introduce any instability.
let im1 = rb1.mass_properties.inv_mass;
let im2 = rb2.mass_properties.inv_mass;
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let ii1 = rb1.world_inv_inertia_sqrt.squared();
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let r1_mat = r1.gcross_matrix();
let ii2 = rb2.world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2 - rb2.world_com;
let r2_mat = r2.gcross_matrix();
@@ -87,10 +87,10 @@ impl RevoluteVelocityConstraint {
mj_lambda1: rb1.active_set_offset,
mj_lambda2: rb2.active_set_offset,
im1,
ii1_sqrt: rb1.world_inv_inertia_sqrt,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
basis1,
im2,
ii2_sqrt: rb2.world_inv_inertia_sqrt,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
impulse: cparams.impulse * params.warmstart_coeff,
inv_lhs,
rhs,
@@ -212,8 +212,8 @@ impl RevoluteVelocityGroundConstraint {
// .to_rotation_matrix()
// .into_inner();
// let basis2 = /*r21 * */ basis1;
let im2 = rb2.mass_properties.inv_mass;
let ii2 = rb2.world_inv_inertia_sqrt.squared();
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let r2 = anchor2 - rb2.world_com;
let r2_mat = r2.gcross_matrix();
@@ -240,7 +240,7 @@ impl RevoluteVelocityGroundConstraint {
joint_id,
mj_lambda2: rb2.active_set_offset,
im2,
ii2_sqrt: rb2.world_inv_inertia_sqrt,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
impulse: cparams.impulse * params.warmstart_coeff,
basis1,
inv_lhs,

12
src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
View File

@@ -43,9 +43,9 @@ impl WRevoluteVelocityConstraint {
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
@@ -53,9 +53,9 @@ impl WRevoluteVelocityConstraint {
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -262,9 +262,9 @@ impl WRevoluteVelocityGroundConstraint {
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);