diff options
Diffstat (limited to 'src/dynamics/solver')
39 files changed, 1982 insertions, 2933 deletions
diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index 9ddf791..c920b69 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -1,35 +1,7 @@ use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet}; -use crate::geometry::{ContactManifold, ContactManifoldIndex, KinematicsCategory}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; -pub(crate) fn categorize_position_contacts( - bodies: &RigidBodySet, - manifolds: &[&mut ContactManifold], - manifold_indices: &[ContactManifoldIndex], - out_point_point_ground: &mut Vec<ContactManifoldIndex>, - out_plane_point_ground: &mut Vec<ContactManifoldIndex>, - out_point_point: &mut Vec<ContactManifoldIndex>, - out_plane_point: &mut Vec<ContactManifoldIndex>, -) { - for manifold_i in manifold_indices { - let manifold = &manifolds[*manifold_i]; - let rb1 = &bodies[manifold.body_pair.body1]; - let rb2 = &bodies[manifold.body_pair.body2]; - - if !rb1.is_dynamic() || !rb2.is_dynamic() { - match manifold.kinematics.category { - KinematicsCategory::PointPoint => out_point_point_ground.push(*manifold_i), - KinematicsCategory::PlanePoint => out_plane_point_ground.push(*manifold_i), - } - } else { - match manifold.kinematics.category { - KinematicsCategory::PointPoint => out_point_point.push(*manifold_i), - KinematicsCategory::PlanePoint => out_plane_point.push(*manifold_i), - } - } - } -} - -pub(crate) fn categorize_velocity_contacts( +pub(crate) fn categorize_contacts( bodies: &RigidBodySet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], @@ -38,8 +10,8 @@ pub(crate) fn categorize_velocity_contacts( ) { for manifold_i in manifold_indices { let manifold = &manifolds[*manifold_i]; - let rb1 = &bodies[manifold.body_pair.body1]; - let rb2 = &bodies[manifold.body_pair.body2]; + let rb1 = &bodies[manifold.data.body_pair.body1]; + let rb2 = &bodies[manifold.data.body_pair.body2]; if !rb1.is_dynamic() || !rb2.is_dynamic() { out_ground.push(*manifold_i) diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs index c4a424b..378d302 100644 --- a/src/dynamics/solver/delta_vel.rs +++ b/src/dynamics/solver/delta_vel.rs @@ -3,7 +3,7 @@ use na::{Scalar, SimdRealField}; #[derive(Copy, Clone, Debug)] //#[repr(align(64))] -pub(crate) struct DeltaVel<N: Scalar> { +pub(crate) struct DeltaVel<N: Scalar + Copy> { pub linear: Vector<N>, pub angular: AngVector<N>, } diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 04acaaf..21cc642 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -12,7 +12,7 @@ pub(crate) trait PairInteraction { impl<'a> PairInteraction for &'a mut ContactManifold { fn body_pair(&self) -> BodyPair { - self.body_pair + self.data.body_pair } } @@ -338,7 +338,7 @@ impl InteractionGroups { let mut occupied_mask = 0u128; let max_interaction_points = interaction_indices .iter() - .map(|i| interactions[*i].num_active_contacts()) + .map(|i| interactions[*i].data.num_active_contacts()) .max() .unwrap_or(1); @@ -351,12 +351,12 @@ impl InteractionGroups { // FIXME: how could we avoid iterating // on each interaction at every iteration on k? - if interaction.num_active_contacts() != k { + if interaction.data.num_active_contacts() != k { continue; } - let body1 = &bodies[interaction.body_pair.body1]; - let body2 = &bodies[interaction.body_pair.body2]; + let body1 = &bodies[interaction.data.body_pair.body1]; + let body2 = &bodies[interaction.data.body_pair.body2]; let is_static1 = !body1.is_dynamic(); let is_static2 = !body2.is_dynamic(); diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index b548c6b..deed8c2 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -1,9 +1,15 @@ use super::{PositionSolver, VelocitySolver}; use crate::counters::Counters; +use crate::dynamics::solver::{ + AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint, + AnyVelocityConstraint, SolverConstraints, +}; use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; pub struct IslandSolver { + contact_constraints: SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint>, + joint_constraints: SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint>, velocity_solver: VelocitySolver, position_solver: PositionSolver, } @@ -11,6 +17,8 @@ pub struct IslandSolver { impl IslandSolver { pub fn new() -> Self { Self { + contact_constraints: SolverConstraints::new(), + joint_constraints: SolverConstraints::new(), velocity_solver: VelocitySolver::new(), position_solver: PositionSolver::new(), } @@ -29,33 +37,23 @@ impl IslandSolver { ) { if manifold_indices.len() != 0 || joint_indices.len() != 0 { counters.solver.velocity_assembly_time.resume(); - self.velocity_solver.init_constraints( - island_id, - params, - bodies, - manifolds, - &manifold_indices, - joints, - &joint_indices, - ); + self.contact_constraints + .init(island_id, params, bodies, manifolds, manifold_indices); + self.joint_constraints + .init(island_id, params, bodies, joints, joint_indices); counters.solver.velocity_assembly_time.pause(); counters.solver.velocity_resolution_time.resume(); - self.velocity_solver - .solve_constraints(island_id, params, bodies, manifolds, joints); - counters.solver.velocity_resolution_time.pause(); - - counters.solver.position_assembly_time.resume(); - self.position_solver.init_constraints( + self.velocity_solver.solve( island_id, params, bodies, manifolds, - &manifold_indices, joints, - &joint_indices, + &mut self.contact_constraints.velocity_constraints, + &mut self.joint_constraints.velocity_constraints, ); - counters.solver.position_assembly_time.pause(); + counters.solver.velocity_resolution_time.pause(); } counters.solver.velocity_update_time.resume(); @@ -64,8 +62,13 @@ impl IslandSolver { if manifold_indices.len() != 0 || joint_indices.len() != 0 { counters.solver.position_resolution_time.resume(); - self.position_solver - .solve_constraints(island_id, params, bodies); + self.position_solver.solve( + island_id, + params, + bodies, + &self.contact_constraints.position_constraints, + &self.joint_constraints.position_constraints, + ); counters.solver.position_resolution_time.pause(); } } 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_c |