From 9bf1321f8f1d2e116f44c2461a53f302c4ef4171 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 8 Dec 2020 17:31:49 +0100 Subject: Outsource the contact manifold, SAT, and some shapes. --- src/dynamics/solver/position_ground_constraint_wide.rs | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) (limited to 'src/dynamics/solver/position_ground_constraint_wide.rs') diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index e423c0a..1609709 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -32,8 +32,10 @@ impl WPositionGroundConstraint { out_constraints: &mut Vec, push: bool, ) { - let mut rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH]; - let mut rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH]; + let mut rbs1 = + array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH]; + let mut rbs2 = + array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH]; let mut flipped = [false; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { @@ -55,10 +57,10 @@ impl WPositionGroundConstraint { ); let delta1 = Isometry::from( - array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH], + array![|ii| if flipped[ii] { manifolds[ii].data.delta2 } else { manifolds[ii].data.delta1 }; SIMD_WIDTH], ); let delta2 = Isometry::from( - array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH], + array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH], ); let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); @@ -112,10 +114,10 @@ impl WPositionGroundConstraint { } } else { if manifolds[0].kinematics.category == KinematicsCategory::PointPoint { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyPositionConstraint::GroupedPointPointGround(constraint); } else { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyPositionConstraint::GroupedPlanePointGround(constraint); } } -- cgit From cc6d1b973002b4d366bc81ec6bf9e8240ad7b404 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 14 Dec 2020 15:51:43 +0100 Subject: Outsource the Shape trait, wquadtree, and shape types. --- .../solver/position_ground_constraint_wide.rs | 40 +++++++++++----------- 1 file changed, 20 insertions(+), 20 deletions(-) (limited to 'src/dynamics/solver/position_ground_constraint_wide.rs') diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index 1609709..2531b2e 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -2,7 +2,7 @@ use super::AnyPositionConstraint; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::{ContactManifold, KinematicsCategory}; use crate::math::{ - AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS, + AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -13,14 +13,14 @@ use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue}; pub(crate) struct WPositionGroundConstraint { pub rb2: [usize; SIMD_WIDTH], // NOTE: the points are relative to the center of masses. - pub p1: [Point; MAX_MANIFOLD_POINTS], - pub local_p2: [Point; MAX_MANIFOLD_POINTS], - pub n1: Vector, - pub radius: SimdFloat, - pub im2: SimdFloat, - pub ii2: AngularInertia, - pub erp: SimdFloat, - pub max_linear_correction: SimdFloat, + pub p1: [Point; MAX_MANIFOLD_POINTS], + pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub n1: Vector, + pub radius: SimdReal, + pub im2: SimdReal, + pub ii2: AngularInertia, + pub erp: SimdReal, + pub max_linear_correction: SimdReal, pub num_contacts: u8, } @@ -45,8 +45,8 @@ impl WPositionGroundConstraint { } } - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let sqrt_ii2: AngularInertia = + let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let sqrt_ii2: AngularInertia = AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); let local_n1 = Vector::from( @@ -63,15 +63,15 @@ impl WPositionGroundConstraint { array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH], ); - let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); - let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); + let radius1 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); + let radius2 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); let coll_pos1 = delta1 * Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]); let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/; + let radius = radius1 + radius2 /*- SimdReal::splat(params.allowed_linear_error)*/; let n1 = coll_pos1 * local_n1; @@ -87,8 +87,8 @@ impl WPositionGroundConstraint { radius, im2, ii2: sqrt_ii2.squared(), - erp: SimdFloat::splat(params.erp), - max_linear_correction: SimdFloat::splat(params.max_linear_correction), + erp: SimdReal::splat(params.erp), + max_linear_correction: SimdReal::splat(params.max_linear_correction), num_contacts: num_points as u8, }; @@ -131,7 +131,7 @@ impl WPositionGroundConstraint { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); - let allowed_err = SimdFloat::splat(params.allowed_linear_error); + let allowed_err = SimdReal::splat(params.allowed_linear_error); let target_dist = self.radius - allowed_err; for k in 0..self.num_contacts as usize { @@ -145,7 +145,7 @@ impl WPositionGroundConstraint { let dist = sqdist.simd_sqrt(); let n = dpos / dist; let err = ((dist - target_dist) * self.erp) - .simd_clamp(-self.max_linear_correction, SimdFloat::zero()); + .simd_clamp(-self.max_linear_correction, SimdReal::zero()); let dp2 = p2.coords - pos2.translation.vector; let gcross2 = -dp2.gcross(n); let ii_gcross2 = self.ii2.transform_vector(gcross2); @@ -173,7 +173,7 @@ impl WPositionGroundConstraint { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); - let allowed_err = SimdFloat::splat(params.allowed_linear_error); + let allowed_err = SimdReal::splat(params.allowed_linear_error); let target_dist = self.radius - allowed_err; for k in 0..self.num_contacts as usize { @@ -186,7 +186,7 @@ impl WPositionGroundConstraint { // NOTE: this condition does not seem to be useful perfomancewise? if dist.simd_lt(target_dist).any() { let err = ((dist - target_dist) * self.erp) - .simd_clamp(-self.max_linear_correction, SimdFloat::zero()); + .simd_clamp(-self.max_linear_correction, SimdReal::zero()); let dp2 = p2.coords - pos2.translation.vector; let gcross2 = -dp2.gcross(n1); -- cgit From 43628c8846c8805d2f835dda4182b7240292900c Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 30 Dec 2020 17:30:07 +0100 Subject: Try using solver contacts again, but in a more cache-coherent way. --- .../solver/position_ground_constraint_wide.rs | 51 +++++++--------------- 1 file changed, 15 insertions(+), 36 deletions(-) (limited to 'src/dynamics/solver/position_ground_constraint_wide.rs') diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index 2531b2e..40d5a68 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -15,8 +15,8 @@ pub(crate) struct WPositionGroundConstraint { // NOTE: the points are relative to the center of masses. pub p1: [Point; MAX_MANIFOLD_POINTS], pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub dists: [SimdReal; MAX_MANIFOLD_POINTS], pub n1: Vector, - pub radius: SimdReal, pub im2: SimdReal, pub ii2: AngularInertia, pub erp: SimdReal, @@ -49,34 +49,17 @@ impl WPositionGroundConstraint { let sqrt_ii2: AngularInertia = AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); - let local_n1 = Vector::from( - array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH], - ); - let local_n2 = Vector::from( - array![|ii| if flipped[ii] { manifolds[ii].local_n1 } else { manifolds[ii].local_n2 }; SIMD_WIDTH], - ); - - let delta1 = Isometry::from( - array![|ii| if flipped[ii] { manifolds[ii].data.delta2 } else { manifolds[ii].data.delta1 }; SIMD_WIDTH], - ); - let delta2 = Isometry::from( - array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH], + let n1 = Vector::from( + array![|ii| if flipped[ii] { -manifolds[ii].data.normal } else { manifolds[ii].data.normal }; SIMD_WIDTH], ); - let radius1 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); - let radius2 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); - - let coll_pos1 = - delta1 * Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]); - + let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let radius = radius1 + radius2 /*- SimdReal::splat(params.allowed_linear_error)*/; - - let n1 = coll_pos1 * local_n1; + let num_active_contacts = manifolds[0].num_active_contacts(); - for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; + for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { + let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let mut constraint = WPositionGroundConstraint { @@ -84,7 +67,7 @@ impl WPositionGroundConstraint { p1: [Point::origin(); MAX_MANIFOLD_POINTS], local_p2: [Point::origin(); MAX_MANIFOLD_POINTS], n1, - radius, + dists: [SimdReal::zero(); MAX_MANIFOLD_POINTS], im2, ii2: sqrt_ii2.squared(), erp: SimdReal::splat(params.erp), @@ -93,15 +76,11 @@ impl WPositionGroundConstraint { }; for i in 0..num_points { - let local_p1 = Point::from( - array![|ii| if flipped[ii] { manifold_points[ii][i].local_p2 } else { manifold_points[ii][i].local_p1 }; SIMD_WIDTH], - ); - let local_p2 = Point::from( - array![|ii| if flipped[ii] { manifold_points[ii][i].local_p1 } else { manifold_points[ii][i].local_p2 }; SIMD_WIDTH], - ); - - constraint.p1[i] = coll_pos1 * local_p1 - n1 * radius1; - constraint.local_p2[i] = delta2 * (local_p2 - local_n2 * radius2); + let point = Point::from(array![|ii| manifold_points[ii][i].point; SIMD_WIDTH]); + let dist = SimdReal::from(array![|ii| manifold_points[ii][i].dist; SIMD_WIDTH]); + constraint.p1[i] = point; + constraint.local_p2[i] = pos2.inverse_transform_point(&point); + constraint.dists[i] = dist; } if push { @@ -132,9 +111,9 @@ impl WPositionGroundConstraint { // Compute jacobians. let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); let allowed_err = SimdReal::splat(params.allowed_linear_error); - let target_dist = self.radius - allowed_err; for k in 0..self.num_contacts as usize { + let target_dist = -self.dists[k] - allowed_err; let p1 = self.p1[k]; let p2 = pos2 * self.local_p2[k]; @@ -174,9 +153,9 @@ impl WPositionGroundConstraint { // Compute jacobians. let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); let allowed_err = SimdReal::splat(params.allowed_linear_error); - let target_dist = self.radius - allowed_err; for k in 0..self.num_contacts as usize { + let target_dist = -self.dists[k] - allowed_err; let n1 = self.n1; let p1 = self.p1[k]; let p2 = pos2 * self.local_p2[k]; -- cgit From 348a339fe350aff6d885cb5a857a0bb6afbea990 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 31 Dec 2020 10:02:51 +0100 Subject: Remove code related to point-point kinematics. --- .../solver/position_ground_constraint_wide.rs | 66 ++-------------------- 1 file changed, 5 insertions(+), 61 deletions(-) (limited to 'src/dynamics/solver/position_ground_constraint_wide.rs') diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index 40d5a68..76c78d8 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -1,6 +1,6 @@ use super::AnyPositionConstraint; use crate::dynamics::{IntegrationParameters, RigidBodySet}; -use crate::geometry::{ContactManifold, KinematicsCategory}; +use crate::geometry::ContactManifold; use crate::math::{ AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS, SIMD_WIDTH, @@ -84,71 +84,15 @@ impl WPositionGroundConstraint { } if push { - if manifolds[0].kinematics.category == KinematicsCategory::PointPoint { - out_constraints - .push(AnyPositionConstraint::GroupedPointPointGround(constraint)); - } else { - out_constraints - .push(AnyPositionConstraint::GroupedPlanePointGround(constraint)); - } + out_constraints.push(AnyPositionConstraint::GroupedGround(constraint)); } else { - if manifolds[0].kinematics.category == KinematicsCategory::PointPoint { - out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = - AnyPositionConstraint::GroupedPointPointGround(constraint); - } else { - out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = - AnyPositionConstraint::GroupedPlanePointGround(constraint); - } + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = + AnyPositionConstraint::GroupedGround(constraint); } } } - pub fn solve_point_point( - &self, - params: &IntegrationParameters, - positions: &mut [Isometry], - ) { - // FIXME: can we avoid most of the multiplications by pos1/pos2? - // Compute jacobians. - let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); - let allowed_err = SimdReal::splat(params.allowed_linear_error); - - for k in 0..self.num_contacts as usize { - let target_dist = -self.dists[k] - allowed_err; - let p1 = self.p1[k]; - let p2 = pos2 * self.local_p2[k]; - - let dpos = p2 - p1; - let sqdist = dpos.norm_squared(); - - if sqdist.simd_lt(target_dist * target_dist).any() { - let dist = sqdist.simd_sqrt(); - let n = dpos / dist; - let err = ((dist - target_dist) * self.erp) - .simd_clamp(-self.max_linear_correction, SimdReal::zero()); - let dp2 = p2.coords - pos2.translation.vector; - let gcross2 = -dp2.gcross(n); - let ii_gcross2 = self.ii2.transform_vector(gcross2); - - // Compute impulse. - let inv_r = self.im2 + gcross2.gdot(ii_gcross2); - let impulse = err / inv_r; - // Apply impulse. - pos2.translation = Translation::from(n * (-impulse * self.im2)) * pos2.translation; - pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation; - } - } - - for ii in 0..SIMD_WIDTH { - positions[self.rb2[ii]] = pos2.extract(ii); - } - } - - pub fn solve_plane_point( - &self, - params: &IntegrationParameters, - positions: &mut [Isometry], - ) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); -- cgit From 967145a9492175be59e8db33299b1687d69d84e2 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 31 Dec 2020 11:16:03 +0100 Subject: Perform contact sorting in the narrow-phase directly. --- src/dynamics/solver/position_ground_constraint_wide.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/dynamics/solver/position_ground_constraint_wide.rs') diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index 76c78d8..dbea1db 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -8,7 +8,7 @@ use crate::math::{ use crate::utils::{WAngularInertia, WCross, WDot}; use num::Zero; -use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue}; +use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; pub(crate) struct WPositionGroundConstraint { pub rb2: [usize; SIMD_WIDTH], @@ -56,10 +56,10 @@ impl WPositionGroundConstraint { let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let num_active_contacts = manifolds[0].num_active_contacts(); + let num_active_contacts = manifolds[0].data.num_active_contacts(); for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH]; + let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let mut constraint = WPositionGroundConstraint { -- cgit From aa61fe65e3ff0289ecab57b4053a3410cf6d4a87 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 4 Jan 2021 15:14:25 +0100 Subject: Add support of 64-bits reals. --- src/dynamics/solver/position_ground_constraint_wide.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/dynamics/solver/position_ground_constraint_wide.rs') diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index dbea1db..c6f1c3f 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -2,8 +2,8 @@ use super::AnyPositionConstraint; use crate::dynamics::{IntegrationParameters, RigidBodySet}; use crate::geometry::ContactManifold; use crate::math::{ - AngularInertia, Isometry, Point, Rotation, SimdReal, Translation, Vector, MAX_MANIFOLD_POINTS, - SIMD_WIDTH, + AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector, + MAX_MANIFOLD_POINTS, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -92,7 +92,7 @@ impl WPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { // FIXME: can we avoid most of the multiplications by pos1/pos2? // Compute jacobians. let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); -- cgit From 8f330b2a00610e5b68c1acd9208120e8f750c7aa Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 21 Jan 2021 14:58:40 +0100 Subject: Rotation locking: apply filter only to the world inertia properties to fix the multi-collider case. --- src/dynamics/solver/position_ground_constraint_wide.rs | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'src/dynamics/solver/position_ground_constraint_wide.rs') diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index c6f1c3f..f52b3f4 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -45,9 +45,10 @@ impl WPositionGroundConstraint { } } - let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let sqrt_ii2: AngularInertia = - 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 = 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], -- cgit