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

4
examples3d/locked_rotations3.rs
View File

@@ -33,7 +33,7 @@ pub fn init_world(testbed: &mut Testbed) {
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(0.0, 3.0, 0.0)
.lock_translations()
.principal_angular_inertia(Vector3::zeros(), Vector3::new(true, false, false))
.restrict_rotations(true, false, false)
.build();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(0.2, 0.6, 2.0).build();
@@ -50,6 +50,8 @@ pub fn init_world(testbed: &mut Testbed) {
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::capsule_y(0.6, 0.4).build();
colliders.insert(collider, handle, &mut bodies);
let collider = ColliderBuilder::capsule_x(0.6, 0.4).build();
colliders.insert(collider, handle, &mut bodies);
/*
* Set up the testbed.

194
src/dynamics/rigid_body.rs
View File

@@ -31,10 +31,10 @@ bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// Flags affecting the behavior of the constraints solver for a given contact manifold.
pub(crate) struct RigidBodyFlags: u8 {
const IGNORE_COLLIDER_MASS = 1 << 0;
const IGNORE_COLLIDER_ANGULAR_INERTIA_X = 1 << 1;
const IGNORE_COLLIDER_ANGULAR_INERTIA_Y = 1 << 2;
const IGNORE_COLLIDER_ANGULAR_INERTIA_Z = 1 << 3;
const TRANSLATION_LOCKED = 1 << 0;
const ROTATION_LOCKED_X = 1 << 1;
const ROTATION_LOCKED_Y = 1 << 2;
const ROTATION_LOCKED_Z = 1 << 3;
}
}
@@ -62,8 +62,9 @@ pub struct RigidBody {
pub(crate) mass_properties: MassProperties,
/// The world-space center of mass of the rigid-body.
pub world_com: Point<Real>,
pub effective_inv_mass: Real,
/// The square-root of the inverse angular inertia tensor of the rigid-body.
pub world_inv_inertia_sqrt: AngularInertia<Real>,
pub effective_world_inv_inertia_sqrt: AngularInertia<Real>,
/// The linear velocity of the rigid-body.
pub(crate) linvel: Vector<Real>,
/// The angular velocity of the rigid-body.
@@ -98,7 +99,8 @@ impl RigidBody {
predicted_position: Isometry::identity(),
mass_properties: MassProperties::zero(),
world_com: Point::origin(),
world_inv_inertia_sqrt: AngularInertia::zero(),
effective_inv_mass: 0.0,
effective_world_inv_inertia_sqrt: AngularInertia::zero(),
linvel: Vector::zeros(),
angvel: na::zero(),
linacc: Vector::zeros(),
@@ -130,14 +132,13 @@ impl RigidBody {
}
pub(crate) fn integrate_accelerations(&mut self, dt: Real, gravity: Vector<Real>) {
if self.mass_properties.inv_mass != 0.0 {
if self.effective_inv_mass != 0.0 {
self.linvel += (gravity * self.gravity_scale + self.linacc) * dt;
self.angvel += self.angacc * dt;
// Reset the accelerations.
self.linacc = na::zero();
self.angacc = na::zero();
}
self.angvel += self.angacc * dt;
self.angacc = na::zero();
}
/// The mass properties of this rigid-body.
@@ -227,40 +228,10 @@ impl RigidBody {
.mass_properties()
.transform_by(coll.position_wrt_parent());
self.colliders.push(handle);
self.mass_properties += Self::filter_collider_mass_props(mass_properties, self.flags);
self.mass_properties += mass_properties;
self.update_world_mass_properties();
}
fn filter_collider_mass_props(
mut props: MassProperties,
flags: RigidBodyFlags,
) -> MassProperties {
if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_MASS) {
props.inv_mass = 0.0;
}
#[cfg(feature = "dim2")]
{
if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z) {
props.inv_principal_inertia_sqrt = 0.0;
}
}
#[cfg(feature = "dim3")]
{
if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X) {
props.inv_principal_inertia_sqrt.x = 0.0;
}
if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y) {
props.inv_principal_inertia_sqrt.y = 0.0;
}
if flags.contains(RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z) {
props.inv_principal_inertia_sqrt.z = 0.0;
}
}
props
}
pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) {
for handle in &self.colliders {
let collider = &mut colliders[*handle];
@@ -277,7 +248,7 @@ impl RigidBody {
let mass_properties = coll
.mass_properties()
.transform_by(coll.position_wrt_parent());
self.mass_properties -= Self::filter_collider_mass_props(mass_properties, self.flags);
self.mass_properties -= mass_properties;
self.update_world_mass_properties();
}
}
@@ -458,9 +429,41 @@ impl RigidBody {
pub(crate) fn update_world_mass_properties(&mut self) {
self.world_com = self.mass_properties.world_com(&self.position);
self.world_inv_inertia_sqrt = self
self.effective_inv_mass = self.mass_properties.inv_mass;
self.effective_world_inv_inertia_sqrt = self
.mass_properties
.world_inv_inertia_sqrt(&self.position.rotation);
// Take into account translation/rotation locking.
if self.flags.contains(RigidBodyFlags::TRANSLATION_LOCKED) {
self.effective_inv_mass = 0.0;
}
#[cfg(feature = "dim2")]
{
if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z) {
self.effective_world_inv_inertia_sqrt = 0.0;
}
}
#[cfg(feature = "dim3")]
{
if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_X) {
self.effective_world_inv_inertia_sqrt.m11 = 0.0;
self.effective_world_inv_inertia_sqrt.m12 = 0.0;
self.effective_world_inv_inertia_sqrt.m13 = 0.0;
}
if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Y) {
self.effective_world_inv_inertia_sqrt.m22 = 0.0;
self.effective_world_inv_inertia_sqrt.m12 = 0.0;
self.effective_world_inv_inertia_sqrt.m23 = 0.0;
}
if self.flags.contains(RigidBodyFlags::ROTATION_LOCKED_Z) {
self.effective_world_inv_inertia_sqrt.m33 = 0.0;
self.effective_world_inv_inertia_sqrt.m13 = 0.0;
self.effective_world_inv_inertia_sqrt.m23 = 0.0;
}
}
}
/*
@@ -469,7 +472,7 @@ impl RigidBody {
/// Applies a force at the center-of-mass of this rigid-body.
pub fn apply_force(&mut self, force: Vector<Real>, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.linacc += force * self.mass_properties.inv_mass;
self.linacc += force * self.effective_inv_mass;
if wake_up {
self.wake_up(true);
@@ -480,7 +483,7 @@ impl RigidBody {
/// Applies an impulse at the center-of-mass of this rigid-body.
pub fn apply_impulse(&mut self, impulse: Vector<Real>, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.linvel += impulse * self.mass_properties.inv_mass;
self.linvel += impulse * self.effective_inv_mass;
if wake_up {
self.wake_up(true);
@@ -492,7 +495,8 @@ impl RigidBody {
#[cfg(feature = "dim2")]
pub fn apply_torque(&mut self, torque: Real, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
self.angacc += self.effective_world_inv_inertia_sqrt
* (self.effective_world_inv_inertia_sqrt * torque);
if wake_up {
self.wake_up(true);
@@ -504,7 +508,8 @@ impl RigidBody {
#[cfg(feature = "dim3")]
pub fn apply_torque(&mut self, torque: Vector<Real>, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
self.angacc += self.effective_world_inv_inertia_sqrt
* (self.effective_world_inv_inertia_sqrt * torque);
if wake_up {
self.wake_up(true);
@@ -516,8 +521,8 @@ impl RigidBody {
#[cfg(feature = "dim2")]
pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.angvel +=
self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse);
self.angvel += self.effective_world_inv_inertia_sqrt
* (self.effective_world_inv_inertia_sqrt * torque_impulse);
if wake_up {
self.wake_up(true);
@@ -529,8 +534,8 @@ impl RigidBody {
#[cfg(feature = "dim3")]
pub fn apply_torque_impulse(&mut self, torque_impulse: Vector<Real>, wake_up: bool) {
if self.body_status == BodyStatus::Dynamic {
self.angvel +=
self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse);
self.angvel += self.effective_world_inv_inertia_sqrt
* (self.effective_world_inv_inertia_sqrt * torque_impulse);
if wake_up {
self.wake_up(true);
@@ -679,16 +684,28 @@ impl RigidBodyBuilder {
}
/// Prevents this rigid-body from rotating because of forces.
///
/// This is equivalent to `self.principal_inertia(0.0, false)` (in 2D) or
/// `self.principal_inertia(Vector3::zeros(), Vector3::repeat(false))` (in 3D).
///
/// See the documentation of [`RigidBodyBuilder::principal_inertia`] for more details.
pub fn lock_rotations(self) -> Self {
#[cfg(feature = "dim2")]
return self.principal_angular_inertia(0.0, false);
#[cfg(feature = "dim3")]
return self.principal_angular_inertia(Vector::zeros(), Vector::repeat(false));
pub fn lock_rotations(mut self) -> Self {
self.flags.set(RigidBodyFlags::ROTATION_LOCKED_X, true);
self.flags.set(RigidBodyFlags::ROTATION_LOCKED_Y, true);
self.flags.set(RigidBodyFlags::ROTATION_LOCKED_Z, true);
self
}
/// Only allow rotations of this rigid-body around specific coordinate axes.
#[cfg(feature = "dim3")]
pub fn restrict_rotations(
mut self,
allow_rotations_x: bool,
allow_rotations_y: bool,
allow_rotations_z: bool,
) -> Self {
self.flags
.set(RigidBodyFlags::ROTATION_LOCKED_X, !allow_rotations_x);
self.flags
.set(RigidBodyFlags::ROTATION_LOCKED_Y, !allow_rotations_y);
self.flags
.set(RigidBodyFlags::ROTATION_LOCKED_Z, !allow_rotations_z);
self
}
/// Sets the mass of the rigid-body being built.
@@ -705,42 +722,23 @@ impl RigidBodyBuilder {
pub fn mass(mut self, mass: Real, colliders_contribution_enabled: bool) -> Self {
self.mass_properties.inv_mass = utils::inv(mass);
self.flags.set(
RigidBodyFlags::IGNORE_COLLIDER_MASS,
RigidBodyFlags::TRANSLATION_LOCKED,
!colliders_contribution_enabled,
);
self
}
/// Sets the angular inertia of this rigid-body.
///
/// In order to lock the rotations of this rigid-body (by
/// making them kinematic), call `.principal_inertia(0.0, false)`.
///
/// If `colliders_contribution_enabled` is `false`, then the principal inertia specified here
/// will be the final principal inertia of the rigid-body created by this builder.
/// If `colliders_contribution_enabled` is `true`, then the final principal of the rigid-body
/// will depend on the initial principal inertia set by this method to which is added
/// the contributions of all the colliders with non-zero density attached to this rigid-body.
#[cfg(feature = "dim2")]
pub fn principal_angular_inertia(
mut self,
inertia: Real,
colliders_contribution_enabled: bool,
) -> Self {
pub fn principal_angular_inertia(mut self, inertia: Real) -> Self {
self.mass_properties.inv_principal_inertia_sqrt = utils::inv(inertia);
self.flags.set(
RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X
| RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y
| RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z,
!colliders_contribution_enabled,
);
self
}
/// Use `self.principal_angular_inertia` instead.
#[cfg(feature = "dim2")]
#[deprecated(note = "renamed to `principal_angular_inertia`.")]
pub fn principal_inertia(self, inertia: Real, colliders_contribution_enabled: bool) -> Self {
self.principal_angular_inertia(inertia, colliders_contribution_enabled)
pub fn principal_inertia(self, inertia: Real) -> Self {
self.principal_angular_inertia(inertia)
}
/// Sets the principal angular inertia of this rigid-body.
@@ -756,36 +754,16 @@ impl RigidBodyBuilder {
/// to which is added the contributions of all the colliders with non-zero density
/// attached to this rigid-body.
#[cfg(feature = "dim3")]
pub fn principal_angular_inertia(
mut self,
inertia: AngVector<Real>,
colliders_contribution_enabled: AngVector<bool>,
) -> Self {
pub fn principal_angular_inertia(mut self, inertia: AngVector<Real>) -> Self {
self.mass_properties.inv_principal_inertia_sqrt = inertia.map(utils::inv);
self.flags.set(
RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_X,
!colliders_contribution_enabled.x,
);
self.flags.set(
RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Y,
!colliders_contribution_enabled.y,
);
self.flags.set(
RigidBodyFlags::IGNORE_COLLIDER_ANGULAR_INERTIA_Z,
!colliders_contribution_enabled.z,
);
self
}
/// Use `self.principal_angular_inertia` instead.
#[cfg(feature = "dim3")]
#[deprecated(note = "renamed to `principal_angular_inertia`.")]
pub fn principal_inertia(
self,
inertia: AngVector<Real>,
colliders_contribution_enabled: AngVector<bool>,
) -> Self {
self.principal_angular_inertia(inertia, colliders_contribution_enabled)
pub fn principal_inertia(self, inertia: AngVector<Real>) -> Self {
self.principal_angular_inertia(inertia)
}
/// Sets the damping factor for the linear part of the rigid-body motion.

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

2
src/dynamics/solver/parallel_island_solver.rs
View File

@@ -250,7 +250,7 @@ impl ParallelIslandSolver {
let rb = &mut bodies[handle.0];
let dvel = mj_lambdas[rb.active_set_offset];
rb.linvel += dvel.linear;
rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular);
rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular);
rb.integrate(params.dt());
positions[rb.active_set_offset] = rb.position;
}

8
src/dynamics/solver/position_constraint.rs
View File

@@ -94,10 +94,10 @@ impl PositionConstraint {
local_p2,
local_n1: rb1.position.inverse_transform_vector(&manifold.data.normal),
dists,
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(),
num_contacts: manifold_points.len() as u8,
erp: params.erp,
max_linear_correction: params.max_linear_correction,

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

@@ -38,12 +38,14 @@ impl WPositionConstraint {
let rbs1 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH];
let rbs2 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH];
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii1: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let sqrt_ii1: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);

4
src/dynamics/solver/position_ground_constraint.rs
View File

@@ -63,8 +63,8 @@ impl PositionGroundConstraint {
local_p2,
n1,
dists,
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(),
num_contacts: manifold_contacts.len() as u8,
erp: params.erp,
max_linear_correction: params.max_linear_correction,

7
src/dynamics/solver/position_ground_constraint_wide.rs
View File

@@ -45,9 +45,10 @@ impl WPositionGroundConstraint {
}
}
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let n1 = Vector::from(
array![|ii| if flipped[ii] { -manifolds[ii].data.normal } else { manifolds[ii].data.normal }; SIMD_WIDTH],

24
src/dynamics/solver/velocity_constraint.rs
View File

@@ -161,8 +161,8 @@ impl VelocityConstraint {
let mut constraint = VelocityConstraint {
dir1: force_dir1,
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1: rb1.mass_properties.inv_mass,
im2: rb2.mass_properties.inv_mass,
im1: rb1.effective_inv_mass,
im2: rb2.effective_inv_mass,
limit: 0.0,
mj_lambda1,
mj_lambda2,
@@ -204,8 +204,8 @@ impl VelocityConstraint {
#[cfg(target_arch = "wasm32")]
{
constraint.dir1 = force_dir1;
constraint.im1 = rb1.mass_properties.inv_mass;
constraint.im2 = rb2.mass_properties.inv_mass;
constraint.im1 = rb1.effective_inv_mass;
constraint.im2 = rb2.effective_inv_mass;
constraint.limit = manifold.data.friction;
constraint.mj_lambda1 = mj_lambda1;
constraint.mj_lambda2 = mj_lambda2;
@@ -226,15 +226,15 @@ impl VelocityConstraint {
// Normal part.
{
let gcross1 = rb1
.world_inv_inertia_sqrt
.effective_world_inv_inertia_sqrt
.transform_vector(dp1.gcross(force_dir1));
let gcross2 = rb2
.world_inv_inertia_sqrt
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let r = 1.0
/ (rb1.mass_properties.inv_mass
+ rb2.mass_properties.inv_mass
/ (rb1.effective_inv_mass
+ rb2.effective_inv_mass
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
@@ -263,14 +263,14 @@ impl VelocityConstraint {
for j in 0..DIM - 1 {
let gcross1 = rb1
.world_inv_inertia_sqrt
.effective_world_inv_inertia_sqrt
.transform_vector(dp1.gcross(tangents1[j]));
let gcross2 = rb2
.world_inv_inertia_sqrt
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let r = 1.0
/ (rb1.mass_properties.inv_mass
+ rb2.mass_properties.inv_mass
/ (rb1.effective_inv_mass
+ rb2.effective_inv_mass
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
let rhs = (vel1 - vel2).dot(&tangents1[j]);

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

@@ -71,18 +71,20 @@ impl WVelocityConstraint {
let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
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 im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);

12
src/dynamics/solver/velocity_ground_constraint.rs
View File

@@ -87,7 +87,7 @@ impl VelocityGroundConstraint {
let mut constraint = VelocityGroundConstraint {
dir1: force_dir1,
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: rb2.mass_properties.inv_mass,
im2: rb2.effective_inv_mass,
limit: 0.0,
mj_lambda2,
manifold_id,
@@ -128,7 +128,7 @@ impl VelocityGroundConstraint {
#[cfg(target_arch = "wasm32")]
{
constraint.dir1 = force_dir1;
constraint.im2 = rb2.mass_properties.inv_mass;
constraint.im2 = rb2.effective_inv_mass;
constraint.limit = manifold.data.friction;
constraint.mj_lambda2 = mj_lambda2;
constraint.manifold_id = manifold_id;
@@ -148,10 +148,10 @@ impl VelocityGroundConstraint {
// Normal part.
{
let gcross2 = rb2
.world_inv_inertia_sqrt
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2));
let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
let mut rhs = (vel1 - vel2).dot(&force_dir1);
@@ -177,9 +177,9 @@ impl VelocityGroundConstraint {
for j in 0..DIM - 1 {
let gcross2 = rb2
.world_inv_inertia_sqrt
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2));
let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]);
#[cfg(feature = "dim2")]
let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff;

7
src/dynamics/solver/velocity_ground_constraint_wide.rs
View File

@@ -75,9 +75,10 @@ impl WVelocityGroundConstraint {
}
}
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);

4
src/dynamics/solver/velocity_solver.rs
View File

@@ -60,7 +60,9 @@ impl VelocitySolver {
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
let dvel = self.mj_lambdas[rb.active_set_offset];
rb.linvel += dvel.linear;
rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular);
rb.angvel += rb
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
});
// Write impulses back into the manifold structures.