diff options
Diffstat (limited to 'src/dynamics/solver/joint_constraint')
19 files changed, 749 insertions, 523 deletions
diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 21a537e..744c00d 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -1,7 +1,7 @@ use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; -use crate::math::{AngularInertia, Isometry, Point, Rotation}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[derive(Debug)] @@ -9,17 +9,17 @@ pub(crate) struct BallPositionConstraint { position1: usize, position2: usize, - local_com1: Point<f32>, - local_com2: Point<f32>, + local_com1: Point<Real>, + local_com2: Point<Real>, - im1: f32, - im2: f32, + im1: Real, + im2: Real, - ii1: AngularInertia<f32>, - ii2: AngularInertia<f32>, + ii1: AngularInertia<Real>, + ii2: AngularInertia<Real>, - local_anchor1: Point<f32>, - local_anchor2: Point<f32>, + local_anchor1: Point<Real>, + local_anchor2: Point<Real>, } impl BallPositionConstraint { @@ -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, @@ -38,7 +38,7 @@ impl BallPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) { let mut position1 = positions[self.position1 as usize]; let mut position2 = positions[self.position2 as usize]; @@ -95,11 +95,11 @@ impl BallPositionConstraint { #[derive(Debug)] pub(crate) struct BallPositionGroundConstraint { position2: usize, - anchor1: Point<f32>, - im2: f32, - ii2: AngularInertia<f32>, - local_anchor2: Point<f32>, - local_com2: Point<f32>, + anchor1: Point<Real>, + im2: Real, + ii2: AngularInertia<Real>, + local_anchor2: Point<Real>, + local_com2: Point<Real>, } impl BallPositionGroundConstraint { @@ -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, @@ -133,7 +133,7 @@ impl BallPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) { let mut position2 = positions[self.position2 as usize]; let anchor2 = position2 * self.local_anchor2; diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs index c552d57..043eea7 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -1,7 +1,7 @@ use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; #[cfg(feature = "dim2")] use crate::math::SdpMatrix; -use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdFloat, SIMD_WIDTH}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use simba::simd::SimdValue; @@ -10,17 +10,17 @@ pub(crate) struct WBallPositionConstraint { position1: [usize; SIMD_WIDTH], position2: [usize; SIMD_WIDTH], - local_com1: Point<SimdFloat>, - local_com2: Point<SimdFloat>, + local_com1: Point<SimdReal>, + local_com2: Point<SimdReal>, - im1: SimdFloat, - im2: SimdFloat, + im1: SimdReal, + im2: SimdReal, - ii1: AngularInertia<SimdFloat>, - ii2: AngularInertia<SimdFloat>, + ii1: AngularInertia<SimdReal>, + ii2: AngularInertia<SimdReal>, - local_anchor1: Point<SimdFloat>, - local_anchor2: Point<SimdFloat>, + local_anchor1: Point<SimdReal>, + local_anchor2: Point<SimdReal>, } impl WBallPositionConstraint { @@ -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 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1 = AngularInertia::<SimdFloat>::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 im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]); + let ii1 = AngularInertia::<SimdReal>::from( + array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ) .squared(); - let ii2 = AngularInertia::<SimdFloat>::from( - array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + let ii2 = AngularInertia::<SimdReal>::from( + 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]); @@ -60,7 +60,7 @@ impl WBallPositionConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) { let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]); let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); @@ -97,7 +97,7 @@ impl WBallPositionConstraint { }; let inv_lhs = lhs.inverse_unchecked(); - let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp)); + let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp)); position1.translation.vector += impulse * self.im1; position2.translation.vector -= impulse * self.im2; @@ -120,11 +120,11 @@ impl WBallPositionConstraint { #[derive(Debug)] pub(crate) struct WBallPositionGroundConstraint { position2: [usize; SIMD_WIDTH], - anchor1: Point<SimdFloat>, - im2: SimdFloat, - ii2: AngularInertia<SimdFloat>, - local_anchor2: Point<SimdFloat>, - local_com2: Point<SimdFloat>, + anchor1: Point<SimdReal>, + im2: SimdReal, + ii2: AngularInertia<SimdReal>, + local_anchor2: Point<SimdReal>, + local_com2: Point<SimdReal>, } impl WBallPositionGroundConstraint { @@ -141,9 +141,9 @@ impl WBallPositionGroundConstraint { } else { cparams[ii].local_anchor1 }; SIMD_WIDTH]); - let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2 = AngularInertia::<SimdFloat>::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>::from( + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ) .squared(); let local_anchor2 = Point::from(array![|ii| if flipped[ii] { @@ -164,7 +164,7 @@ impl WBallPositionGroundConstraint { } } - pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) { let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); let anchor2 = position2 * self.local_anchor2; @@ -186,7 +186,7 @@ impl WBallPositionGroundConstraint { }; let inv_lhs = lhs.inverse_unchecked(); - let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp)); + let impulse = inv_lhs * -(err * SimdReal::splat(params.joint_erp)); position2.translation.vector -= impulse * self.im2; let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs index 97ba244..e75f978 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel; use crate::dynamics::{ BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; -use crate::math::{SdpMatrix, Vector}; +use crate::math::{AngularInertia, Real, SdpMatrix, Vector}; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; #[derive(Debug)] @@ -12,16 +12,19 @@ pub(crate) struct BallVelocityConstraint { joint_id: JointIndex, - rhs: Vector<f32>, - pub(crate) impulse: Vector<f32>, + rhs: Vector<Real>, + pub(crate) impulse: Vector<Real>, - gcross1: Vector<f32>, - gcross2: Vector<f32>, + r1: Vector<Real>, + r2: Vector<Real>, - inv_lhs: SdpMatrix<f32>, + inv_lhs: SdpMatrix<Real>, - im1: f32, - im2: f32, + im1: Real, + im2: Real, + + ii1_sqrt: AngularInertia<Real>, + ii2_sqrt: AngularInertia<Real>, } impl BallVelocityConstraint { @@ -37,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; @@ -49,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); @@ -64,17 +67,14 @@ 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; 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(); BallVelocityConstraint { @@ -84,42 +84,46 @@ impl BallVelocityConstraint { im1, im2, impulse: cparams.impulse * params.warmstart_coeff, - gcross1, - gcross2, + r1: anchor1, + r2: anchor2, rhs, inv_lhs, + ii1_sqrt: rb1.effective_world_inv_inertia_sqrt, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; 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.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_lambda2 as usize] = mj_lambda2; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) { let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 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 vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + 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 impulse = self.inv_lhs * dvel; self.impulse += 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.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_lambda2 as usize] = mj_lambda2; @@ -137,11 +141,12 @@ impl BallVelocityConstraint { pub(crate) struct BallVelocityGroundConstraint { mj_lambda2: usize, joint_id: JointIndex, - rhs: Vector<f32>, - impulse: Vector<f32>, - gcross2: Vector<f32>, - inv_lhs: SdpMatrix<f32>, - im2: f32, + rhs: Vector<Real>, + impulse: Vector<Real>, + r2: Vector<Real>, + inv_lhs: SdpMatrix<Real>, + im2: Real, + ii2_sqrt: AngularInertia<Real>, } impl BallVelocityGroundConstraint { @@ -165,20 +170,19 @@ 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; let cmat2 = anchor2.gcross_matrix(); - let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2); let lhs; #[cfg(feature = "dim3")] { lhs = rb2 - .world_inv_inertia_sqrt + .effective_world_inv_inertia_sqrt .squared() .quadform(&cmat2) .add_diagonal(im2); @@ -186,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; @@ -200,30 +204,32 @@ impl BallVelocityGroundConstraint { mj_lambda2: rb2.active_set_offset, im2, impulse: cparams.impulse * params.warmstart_coeff, - gcross2, + r2: anchor2, rhs, inv_lhs, + ii2_sqrt: rb2.effective_world_inv_inertia_sqrt, } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; 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; } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) { 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 impulse = self.inv_lhs * dvel; self.impulse += 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; } diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs index b96f3b8..95b0bb5 100644 --- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -3,7 +3,7 @@ use crate::dynamics::{ BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, }; use crate::math::{ - AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdFloat, Vector, SIMD_WIDTH, + AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH, }; use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use simba::simd::SimdValue; @@ -15,16 +15,19 @@ pub(crate) struct WBallVelocityConstraint { joint_id: [JointIndex; SIMD_WIDTH], - rhs: Vector<SimdFloat>, - pub(crate) impulse: Vector<SimdFloat>, + rhs: Vector<SimdReal>, + pub(crate) impulse: Vector<SimdReal>, - gcross1: Vector<SimdFloat>, - gcross2: Vector<SimdFloat>, + r1: Vector<SimdReal>, + r2: Vector<SimdReal>, - inv_lhs: SdpMatrix<SimdFloat>, + inv_lhs: SdpMatrix<SimdReal>, - im1: SimdFloat, - im2: SimdFloat, + im1: SimdReal, + im2: SimdReal, + + ii1_sqrt: AngularInertia<SimdReal>, + ii2_sqrt: AngularInertia<SimdReal>, } impl WBallVelocityConstraint { @@ -37,21 +40,21 @@ impl WBallVelocityConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; 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 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii1_sqrt = AngularInertia::<SimdFloat>::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_sqrt = AngularInertia::<SimdReal>::from( + array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; 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 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::<SimdFloat>::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_sqrt = AngularInertia::<SimdReal>::from( + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -62,8 +65,8 @@ impl WBallVelocityConstraint { let anchor1 = position1 * local_anchor1 - world_com1; let anchor2 = position2 * local_anchor2 - world_com2; - let vel1: Vector<SimdFloat> = linvel1 + angvel1.gcross(anchor1); - let vel2: Vector<SimdFloat> = linvel2 + angvel2.gcross(anchor2); + let vel1: Vector<SimdReal> = linvel1 + angvel1.gcross(anchor1); + let vel2: Vector<SimdReal> = linvel2 + angvel2.gcross(anchor2); let rhs = -(vel1 - vel2); let lhs; @@ -88,9 +91,6 @@ impl WBallVelocityConstraint { 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(); WBallVelocityConstraint { @@ -99,15 +99,17 @@ impl WBallVelocityConstraint { mj_lambda2, im1, im2, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), - gcross1, - gcross2, + impulse: impulse * SimdReal::splat(params.warmstart_coeff), + r1: anchor1, + r2: anchor2, rhs, inv_lhs, + ii1_sqrt, + ii2_sqrt, } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) { let mut mj_lambda1 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], @@ -126,9 +128,9 @@ impl WBallVelocityConstraint { }; 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.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 { mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); @@ -140,8 +142,8 @@ impl WBallVelocityConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) { - let mut mj_lambda1: DeltaVel<SimdFloat> = DeltaVel { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) { + let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], ), @@ -149,7 +151,7 @@ impl WBallVelocityConstraint { array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], ), }; - let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel { + let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], ), @@ -158,18 +160,20 @@ impl WBallVelocityConstraint { ), }; - let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1); - let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + 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 impulse = self.inv_lhs * dvel; self.impulse += impulse; 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.angular -= self.gcross2.gcross(impulse); + mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse)); for ii in 0..SIMD_WIDTH { mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); @@ -195,11 +199,12 @@ impl WBallVelocityConstraint { pub(crate) struct WBallVelocityGroundConstraint { mj_lambda2: [usize; SIMD_WIDTH], joint_id: [JointIndex; SIMD_WIDTH], - rhs: Vector<SimdFloat>, - pub(crate) impulse: Vector<SimdFloat>, - gcross2: Vector<SimdFloat>, - inv_lhs: SdpMatrix<SimdFloat>, - im2: SimdFloat, + rhs: Vector<SimdReal>, + pub(crate) impulse: Vector<SimdReal>, + r2: Vector<SimdReal>, + inv_lhs: SdpMatrix<SimdReal>, + im2: SimdReal, + ii2_sqrt: AngularInertia<SimdReal>, } impl WBallVelocityGroundConstraint { @@ -213,7 +218,7 @@ impl WBallVelocityGroundConstraint { ) -> Self { let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); - let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; 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 local_anchor1 = Point::from( array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], @@ -221,11 +226,11 @@ impl WBallVelocityGroundConstraint { let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); - let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; 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 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); - let ii2_sqrt = AngularInertia::<SimdFloat>::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_sqrt = AngularInertia::<SimdReal>::from( + array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH], ); let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -237,13 +242,12 @@ impl WBallVelocityGroundConstraint { let anchor1 = position1 * local_anchor1 - world_com1; let anchor2 = position2 * local_anchor2 - world_com2; - let vel1: Vector<SimdFloat> = linvel1 + angvel1.gcross(anchor1); - let vel2: Vector<SimdFloat> = linvel2 + angvel2.gcross(anchor2); + let vel1: Vector<SimdReal> = linvel1 + angvel1.gcross(anchor1); + let vel2: Vector<SimdReal> = linvel2 + angvel2.gcross(anchor2); let rhs = vel2 - vel1; let lhs; let cmat2 = anchor2.gcross_matrix(); - let gcross2 = ii2_sqrt.transform_lin_vector(anchor2); #[cfg(feature = "dim3")] { @@ -267,14 +271,15 @@ impl WBallVelocityGroundConstraint { joint_id, mj_lambda2, im2, - impulse: impulse * SimdFloat::splat(params.warmstart_coeff), - gcross2, + impulse: impulse * SimdReal::splat(params.warmstart_coeff), + r2: anchor2, rhs, inv_lhs, + ii2_sqrt, } } - pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) { + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) { let mut mj_lambda2 = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], @@ -285,7 +290,7 @@ impl WBallVelocityGroundConstraint { }; 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 { mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); @@ -293,8 +298,8 @@ impl WBallVelocityGroundConstraint { } } - pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) { - let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel { + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) { + let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel { linear: Vector::from( array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], ), @@ -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 impulse = self.inv_lhs * dvel; self.imp |