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/rigid_body_set.rs | 2 +- src/dynamics/solver/categorization.rs | 8 ++--- src/dynamics/solver/interaction_groups.rs | 12 ++++---- src/dynamics/solver/parallel_velocity_solver.rs | 4 +-- src/dynamics/solver/position_constraint.rs | 12 ++++---- src/dynamics/solver/position_constraint_wide.rs | 12 ++++---- src/dynamics/solver/position_ground_constraint.rs | 16 +++++----- .../solver/position_ground_constraint_wide.rs | 14 +++++---- src/dynamics/solver/velocity_constraint.rs | 30 +++++++++---------- src/dynamics/solver/velocity_constraint_wide.rs | 28 +++++++++--------- src/dynamics/solver/velocity_ground_constraint.rs | 34 +++++++++++----------- .../solver/velocity_ground_constraint_wide.rs | 28 +++++++++--------- 12 files changed, 101 insertions(+), 99 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index b8667bc..c32c402 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -479,7 +479,7 @@ impl RigidBodySet { if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) { for inter in contacts { for manifold in &inter.2.manifolds { - if manifold.num_active_contacts() > 0 { + if manifold.num_active_contacts > 0 { let other = crate::utils::other_handle( (inter.0, inter.1), *collider_handle, diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index 9ddf791..d0a2d0f 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -12,8 +12,8 @@ pub(crate) fn categorize_position_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() { match manifold.kinematics.category { @@ -38,8 +38,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/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 04acaaf..18e846b 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 } } @@ -77,7 +77,7 @@ impl ParallelInteractionGroups { .iter() .zip(self.interaction_colors.iter_mut()) { - let body_pair = interactions[*interaction_id].body_pair(); + let body_pair = interactions[*interaction_id].data.body_pair(); let rb1 = &bodies[body_pair.body1]; let rb2 = &bodies[body_pair.body2]; @@ -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].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.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/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs index 303a18f..451dd32 100644 --- a/src/dynamics/solver/parallel_velocity_solver.rs +++ b/src/dynamics/solver/parallel_velocity_solver.rs @@ -247,13 +247,13 @@ impl ParallelVelocitySolverPart { VelocityConstraintDesc::NongroundGrouped(joint_id) => { let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH]; let constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies); - self.constraints[joints[0].constraint_index] = constraint; + self.constraints[joints[0].data.constraint_index] = constraint; } #[cfg(feature = "simd-is-enabled")] VelocityConstraintDesc::GroundGrouped(joint_id) => { let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH]; let constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies); - self.constraints[joints[0].constraint_index] = constraint; + self.constraints[joints[0].data.constraint_index] = constraint; } } } diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 69fcf57..b329e39 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -88,8 +88,8 @@ impl PositionConstraint { out_constraints: &mut Vec, push: bool, ) { - 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]; let shift1 = manifold.local_n1 * -manifold.kinematics.radius1; let shift2 = manifold.local_n2 * -manifold.kinematics.radius2; let radius = @@ -104,8 +104,8 @@ impl PositionConstraint { let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS]; for l in 0..manifold_points.len() { - local_p1[l] = manifold.delta1 * (manifold_points[l].local_p1 + shift1); - local_p2[l] = manifold.delta2 * (manifold_points[l].local_p2 + shift2); + local_p1[l] = manifold.data.delta1 * (manifold_points[l].local_p1 + shift1); + local_p2[l] = manifold.data.delta2 * (manifold_points[l].local_p2 + shift2); } let constraint = PositionConstraint { @@ -132,10 +132,10 @@ impl PositionConstraint { } } else { if manifold.kinematics.category == KinematicsCategory::PointPoint { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyPositionConstraint::NongroupedPointPoint(constraint); } else { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyPositionConstraint::NongroupedPlanePoint(constraint); } } diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index 0633926..ecc0fc3 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -35,8 +35,8 @@ impl WPositionConstraint { out_constraints: &mut Vec, push: bool, ) { - let rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH]; - let rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH]; + 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 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); let sqrt_ii1: AngularInertia = @@ -51,8 +51,8 @@ impl WPositionConstraint { 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 delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]); - let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]); + let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]); + let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]); let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -100,10 +100,10 @@ impl WPositionConstraint { } } 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::GroupedPointPoint(constraint); } else { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyPositionConstraint::GroupedPlanePoint(constraint); } } diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs index dcd2d64..7a15f3e 100644 --- a/src/dynamics/solver/position_ground_constraint.rs +++ b/src/dynamics/solver/position_ground_constraint.rs @@ -28,8 +28,8 @@ impl PositionGroundConstraint { out_constraints: &mut Vec, push: bool, ) { - let mut rb1 = &bodies[manifold.body_pair.body1]; - let mut rb2 = &bodies[manifold.body_pair.body2]; + let mut rb1 = &bodies[manifold.data.body_pair.body1]; + let mut rb2 = &bodies[manifold.data.body_pair.body2]; let flip = !rb2.is_dynamic(); let local_n1; @@ -41,13 +41,13 @@ impl PositionGroundConstraint { std::mem::swap(&mut rb1, &mut rb2); local_n1 = manifold.local_n2; local_n2 = manifold.local_n1; - delta1 = &manifold.delta2; - delta2 = &manifold.delta1; + delta1 = &manifold.data.delta2; + delta2 = &manifold.data.delta1; } else { local_n1 = manifold.local_n1; local_n2 = manifold.local_n2; - delta1 = &manifold.delta1; - delta2 = &manifold.delta2; + delta1 = &manifold.data.delta1; + delta2 = &manifold.data.delta2; }; let coll_pos1 = rb1.position * delta1; @@ -105,10 +105,10 @@ impl PositionGroundConstraint { } } else { if manifold.kinematics.category == KinematicsCategory::PointPoint { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyPositionConstraint::NongroupedPointPointGround(constraint); } else { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyPositionConstraint::NongroupedPlanePointGround(constraint); } } 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); } } diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 948081d..1bc4c0e 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -144,14 +144,14 @@ impl VelocityConstraint { out_constraints: &mut Vec, push: bool, ) { - 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]; let mj_lambda1 = rb1.active_set_offset; let mj_lambda2 = rb2.active_set_offset; - let pos_coll1 = rb1.position * manifold.delta1; - let pos_coll2 = rb2.position * manifold.delta2; + let pos_coll1 = rb1.position * manifold.data.delta1; + let pos_coll2 = rb2.position * manifold.data.delta2; let force_dir1 = pos_coll1 * (-manifold.local_n1); - let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff; + let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; for (l, manifold_points) in manifold .active_contacts() @@ -164,7 +164,7 @@ impl VelocityConstraint { elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], im1: rb1.mass_properties.inv_mass, im2: rb2.mass_properties.inv_mass, - limit: manifold.friction, + limit: manifold.data.friction, mj_lambda1, mj_lambda2, manifold_id, @@ -207,7 +207,7 @@ impl VelocityConstraint { constraint.dir1 = force_dir1; constraint.im1 = rb1.mass_properties.inv_mass; constraint.im2 = rb2.mass_properties.inv_mass; - constraint.limit = manifold.friction; + constraint.limit = manifold.data.friction; constraint.mj_lambda1 = mj_lambda1; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; @@ -241,12 +241,12 @@ impl VelocityConstraint { let mut rhs = (vel1 - vel2).dot(&force_dir1); if rhs <= -params.restitution_velocity_threshold { - rhs += manifold.restitution * rhs + rhs += manifold.data.restitution * rhs } rhs += manifold_point.dist.max(0.0) * params.inv_dt(); - let impulse = manifold_points[k].impulse * warmstart_coeff; + let impulse = manifold_points[k].data.impulse * warmstart_coeff; constraint.elements[k].normal_part = VelocityConstraintElementPart { gcross1, @@ -275,9 +275,9 @@ impl VelocityConstraint { + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2).dot(&tangents1[j]); #[cfg(feature = "dim2")] - let impulse = manifold_points[k].tangent_impulse * warmstart_coeff; + let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff; #[cfg(feature = "dim3")] - let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff; + let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff; constraint.elements[k].tangent_part[j] = VelocityConstraintElementPart { gcross1, @@ -294,7 +294,7 @@ impl VelocityConstraint { if push { out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint)); } else { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyVelocityConstraint::Nongrouped(constraint); } } @@ -388,15 +388,15 @@ impl VelocityConstraint { for k in 0..self.num_contacts as usize { let active_contacts = manifold.active_contacts_mut(); - active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse; + active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].tangent_impulse = + active_contacts[k_base + k].data.tangent_impulse = self.elements[k].tangent_part[0].impulse; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].tangent_impulse = [ + active_contacts[k_base + k].data.tangent_impulse = [ self.elements[k].tangent_part[0].impulse, self.elements[k].tangent_part[1].impulse, ]; diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 5d8078a..a4efb9a 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -69,11 +69,11 @@ impl WVelocityConstraint { push: bool, ) { let inv_dt = SimdFloat::splat(params.inv_dt()); - let rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH]; - let rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH]; + 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 delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]); - let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]); + let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]); + let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]); let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); let ii1: AngularInertia = @@ -103,13 +103,13 @@ impl WVelocityConstraint { let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]); - let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]); + let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]); + let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]); let restitution_velocity_threshold = SimdFloat::splat(params.restitution_velocity_threshold); let warmstart_multiplier = - SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]); + SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff); for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { @@ -140,7 +140,7 @@ impl WVelocityConstraint { let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); let impulse = - SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]); + SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); let dp1 = p1 - world_com1; let dp2 = p2 - world_com2; @@ -176,11 +176,11 @@ impl WVelocityConstraint { for j in 0..DIM - 1 { #[cfg(feature = "dim2")] let impulse = SimdFloat::from( - array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH], + array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], ); #[cfg(feature = "dim3")] let impulse = SimdFloat::from( - array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH], + array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH], ); let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j])); @@ -202,7 +202,7 @@ impl WVelocityConstraint { if push { out_constraints.push(AnyVelocityConstraint::Grouped(constraint)); } else { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyVelocityConstraint::Grouped(constraint); } } @@ -342,15 +342,15 @@ impl WVelocityConstraint { let manifold = &mut manifolds_all[self.manifold_id[ii]]; let k_base = self.manifold_contact_id; let active_contacts = manifold.active_contacts_mut(); - active_contacts[k_base + k].impulse = impulses[ii]; + active_contacts[k_base + k].data.impulse = impulses[ii]; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii]; + active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii]; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].tangent_impulse = + active_contacts[k_base + k].data.tangent_impulse = [tangent_impulses[ii], bitangent_impulses[ii]]; } } diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index d8ef8be..c3a3356 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -63,26 +63,26 @@ impl VelocityGroundConstraint { out_constraints: &mut Vec, push: bool, ) { - let mut rb1 = &bodies[manifold.body_pair.body1]; - let mut rb2 = &bodies[manifold.body_pair.body2]; + let mut rb1 = &bodies[manifold.data.body_pair.body1]; + let mut rb2 = &bodies[manifold.data.body_pair.body2]; let flipped = !rb2.is_dynamic(); let force_dir1; let coll_pos1; let coll_pos2; if flipped { - coll_pos1 = rb2.position * manifold.delta2; - coll_pos2 = rb1.position * manifold.delta1; + coll_pos1 = rb2.position * manifold.data.delta2; + coll_pos2 = rb1.position * manifold.data.delta1; force_dir1 = coll_pos1 * (-manifold.local_n2); std::mem::swap(&mut rb1, &mut rb2); } else { - coll_pos1 = rb1.position * manifold.delta1; - coll_pos2 = rb2.position * manifold.delta2; + coll_pos1 = rb1.position * manifold.data.delta1; + coll_pos2 = rb2.position * manifold.data.delta2; force_dir1 = coll_pos1 * (-manifold.local_n1); } let mj_lambda2 = rb2.active_set_offset; - let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff; + let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; for (l, manifold_points) in manifold .active_contacts() @@ -94,7 +94,7 @@ impl VelocityGroundConstraint { dir1: force_dir1, elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], im2: rb2.mass_properties.inv_mass, - limit: manifold.friction, + limit: manifold.data.friction, mj_lambda2, manifold_id, manifold_contact_id: l * MAX_MANIFOLD_POINTS, @@ -135,7 +135,7 @@ impl VelocityGroundConstraint { { constraint.dir1 = force_dir1; constraint.im2 = rb2.mass_properties.inv_mass; - constraint.limit = manifold.friction; + constraint.limit = manifold.data.friction; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS; @@ -173,12 +173,12 @@ impl VelocityGroundConstraint { let mut rhs = (vel1 - vel2).dot(&force_dir1); if rhs <= -params.restitution_velocity_threshold { - rhs += manifold.restitution * rhs + rhs += manifold.data.restitution * rhs } rhs += manifold_point.dist.max(0.0) * params.inv_dt(); - let impulse = manifold_points[k].impulse * warmstart_coeff; + let impulse = manifold_points[k].data.impulse * warmstart_coeff; constraint.elements[k].normal_part = VelocityGroundConstraintElementPart { gcross2, @@ -199,9 +199,9 @@ impl VelocityGroundConstraint { let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2)); let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]); #[cfg(feature = "dim2")] - let impulse = manifold_points[k].tangent_impulse * warmstart_coeff; + let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff; #[cfg(feature = "dim3")] - let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff; + let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff; constraint.elements[k].tangent_part[j] = VelocityGroundConstraintElementPart { @@ -218,7 +218,7 @@ impl VelocityGroundConstraint { if push { out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint)); } else { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyVelocityConstraint::NongroupedGround(constraint); } } @@ -290,15 +290,15 @@ impl VelocityGroundConstraint { for k in 0..self.num_contacts as usize { let active_contacts = manifold.active_contacts_mut(); - active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse; + active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].tangent_impulse = + active_contacts[k_base + k].data.tangent_impulse = self.elements[k].tangent_part[0].impulse; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].tangent_impulse = [ + active_contacts[k_base + k].data.tangent_impulse = [ self.elements[k].tangent_part[0].impulse, self.elements[k].tangent_part[1].impulse, ]; diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index be01944..84c4182 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -65,8 +65,8 @@ impl WVelocityGroundConstraint { push: bool, ) { let inv_dt = SimdFloat::splat(params.inv_dt()); - let mut rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH]; - let mut rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH]; + let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; + let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; let mut flipped = [false; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { @@ -90,10 +90,10 @@ impl WVelocityGroundConstraint { let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); 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 coll_pos1 = pos1 * delta1; @@ -109,13 +109,13 @@ impl WVelocityGroundConstraint { let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]); - let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]); + let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]); + let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]); let restitution_velocity_threshold = SimdFloat::splat(params.restitution_velocity_threshold); let warmstart_multiplier = - SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]); + SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff); for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { @@ -146,7 +146,7 @@ impl WVelocityGroundConstraint { let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); let impulse = - SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]); + SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); let dp1 = p1 - world_com1; let dp2 = p2 - world_com2; @@ -178,11 +178,11 @@ impl WVelocityGroundConstraint { for j in 0..DIM - 1 { #[cfg(feature = "dim2")] let impulse = SimdFloat::from( - array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH], + array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], ); #[cfg(feature = "dim3")] let impulse = SimdFloat::from( - array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH], + array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH], ); let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); @@ -202,7 +202,7 @@ impl WVelocityGroundConstraint { if push { out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint)); } else { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyVelocityConstraint::GroupedGround(constraint); } } @@ -302,15 +302,15 @@ impl WVelocityGroundConstraint { let manifold = &mut manifolds_all[self.manifold_id[ii]]; let k_base = self.manifold_contact_id; let active_contacts = manifold.active_contacts_mut(); - active_contacts[k_base + k].impulse = impulses[ii]; + active_contacts[k_base + k].data.impulse = impulses[ii]; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii]; + active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii]; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].tangent_impulse = + active_contacts[k_base + k].data.tangent_impulse = [tangent_impulses[ii], bitangent_impulses[ii]]; } } -- 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. --- src/dynamics/mass_properties.rs | 442 --------------------- src/dynamics/mass_properties_ball.rs | 30 -- src/dynamics/mass_properties_capsule.rs | 39 -- src/dynamics/mass_properties_cone.rs | 29 -- src/dynamics/mass_properties_cuboid.rs | 33 -- src/dynamics/mass_properties_cylinder.rs | 40 -- src/dynamics/mass_properties_polygon.rs | 146 ------- src/dynamics/mod.rs | 11 +- .../ball_position_constraint_wide.rs | 44 +- .../ball_velocity_constraint_wide.rs | 64 +-- .../fixed_velocity_constraint_wide.rs | 76 ++-- .../prismatic_velocity_constraint_wide.rs | 114 +++--- .../revolute_velocity_constraint_wide.rs | 60 +-- src/dynamics/solver/position_constraint_wide.rs | 48 +-- .../solver/position_ground_constraint_wide.rs | 40 +- src/dynamics/solver/velocity_constraint_wide.rs | 67 ++-- .../solver/velocity_ground_constraint_wide.rs | 59 ++- 17 files changed, 286 insertions(+), 1056 deletions(-) delete mode 100644 src/dynamics/mass_properties.rs delete mode 100644 src/dynamics/mass_properties_ball.rs delete mode 100644 src/dynamics/mass_properties_capsule.rs delete mode 100644 src/dynamics/mass_properties_cone.rs delete mode 100644 src/dynamics/mass_properties_cuboid.rs delete mode 100644 src/dynamics/mass_properties_cylinder.rs delete mode 100644 src/dynamics/mass_properties_polygon.rs (limited to 'src/dynamics') diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs deleted file mode 100644 index a36ceb5..0000000 --- a/src/dynamics/mass_properties.rs +++ /dev/null @@ -1,442 +0,0 @@ -use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Vector}; -use crate::utils; -use num::Zero; -use std::ops::{Add, AddAssign, Sub, SubAssign}; -#[cfg(feature = "dim3")] -use {na::Matrix3, std::ops::MulAssign}; - -#[derive(Copy, Clone, Debug, PartialEq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// The local mass properties of a rigid-body. -pub struct MassProperties { - /// The center of mass of a rigid-body expressed in its local-space. - pub local_com: Point, - /// The inverse of the mass of a rigid-body. - /// - /// If this is zero, the rigid-body is assumed to have infinite mass. - pub inv_mass: f32, - /// The inverse of the principal angular inertia of the rigid-body. - /// - /// Components set to zero are assumed to be infinite along the corresponding principal axis. - pub inv_principal_inertia_sqrt: AngVector, - #[cfg(feature = "dim3")] - /// The principal vectors of the local angular inertia tensor of the rigid-body. - pub principal_inertia_local_frame: Rotation, -} - -impl MassProperties { - /// Initializes the mass properties with the given center-of-mass, mass, and angular inertia. - /// - /// The center-of-mass is specified in the local-space of the rigid-body. - #[cfg(feature = "dim2")] - pub fn new(local_com: Point, mass: f32, principal_inertia: f32) -> Self { - let inv_mass = utils::inv(mass); - let inv_principal_inertia_sqrt = utils::inv(principal_inertia.sqrt()); - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - } - } - - /// Initializes the mass properties from the given center-of-mass, mass, and principal angular inertia. - /// - /// The center-of-mass is specified in the local-space of the rigid-body. - /// The principal angular inertia are the angular inertia along the coordinate axes in the local-space - /// of the rigid-body. - #[cfg(feature = "dim3")] - pub fn new(local_com: Point, mass: f32, principal_inertia: AngVector) -> Self { - Self::with_principal_inertia_frame(local_com, mass, principal_inertia, Rotation::identity()) - } - - /// Initializes the mass properties from the given center-of-mass, mass, and principal angular inertia. - /// - /// The center-of-mass is specified in the local-space of the rigid-body. - /// The principal angular inertia are the angular inertia along the coordinate axes defined by - /// the `principal_inertia_local_frame` expressed in the local-space of the rigid-body. - #[cfg(feature = "dim3")] - pub fn with_principal_inertia_frame( - local_com: Point, - mass: f32, - principal_inertia: AngVector, - principal_inertia_local_frame: Rotation, - ) -> Self { - let inv_mass = utils::inv(mass); - let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt())); - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - principal_inertia_local_frame, - } - } - - /// The world-space center of mass of the rigid-body. - pub fn world_com(&self, pos: &Isometry) -> Point { - pos * self.local_com - } - - #[cfg(feature = "dim2")] - /// The world-space inverse angular inertia tensor of the rigid-body. - pub fn world_inv_inertia_sqrt(&self, _rot: &Rotation) -> AngularInertia { - self.inv_principal_inertia_sqrt - } - - #[cfg(feature = "dim3")] - /// The world-space inverse angular inertia tensor of the rigid-body. - pub fn world_inv_inertia_sqrt(&self, rot: &Rotation) -> AngularInertia { - if !self.inv_principal_inertia_sqrt.is_zero() { - let mut lhs = (rot * self.principal_inertia_local_frame) - .to_rotation_matrix() - .into_inner(); - let rhs = lhs.transpose(); - lhs.column_mut(0) - .mul_assign(self.inv_principal_inertia_sqrt.x); - lhs.column_mut(1) - .mul_assign(self.inv_principal_inertia_sqrt.y); - lhs.column_mut(2) - .mul_assign(self.inv_principal_inertia_sqrt.z); - let inertia = lhs * rhs; - AngularInertia::from_sdp_matrix(inertia) - } else { - AngularInertia::zero() - } - } - - #[cfg(feature = "dim3")] - /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axes. - pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3 { - let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e); - self.principal_inertia_local_frame.to_rotation_matrix() - * Matrix3::from_diagonal(&inv_principal_inertia) - * self - .principal_inertia_local_frame - .inverse() - .to_rotation_matrix() - } - - #[cfg(feature = "dim3")] - /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axes. - pub fn reconstruct_inertia_matrix(&self) -> Matrix3 { - let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e)); - self.principal_inertia_local_frame.to_rotation_matrix() - * Matrix3::from_diagonal(&principal_inertia) - * self - .principal_inertia_local_frame - .inverse() - .to_rotation_matrix() - } - - #[cfg(feature = "dim2")] - pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector) -> f32 { - let i = utils::inv(self.inv_principal_inertia_sqrt * self.inv_principal_inertia_sqrt); - - if self.inv_mass != 0.0 { - let mass = 1.0 / self.inv_mass; - i + shift.norm_squared() * mass - } else { - i - } - } - - #[cfg(feature = "dim3")] - pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector) -> Matrix3 { - let matrix = self.reconstruct_inertia_matrix(); - - if self.inv_mass != 0.0 { - let mass = 1.0 / self.inv_mass; - let diag = shift.norm_squared(); - let diagm = Matrix3::from_diagonal_element(diag); - matrix + (diagm + shift * shift.transpose()) * mass - } else { - matrix - } - } - - /// Transform each element of the mass properties. - pub fn transform_by(&self, m: &Isometry) -> Self { - // NOTE: we don't apply the parallel axis theorem here - // because the center of mass is also transformed. - Self { - local_com: m * self.local_com, - inv_mass: self.inv_mass, - inv_principal_inertia_sqrt: self.inv_principal_inertia_sqrt, - #[cfg(feature = "dim3")] - principal_inertia_local_frame: m.rotation * self.principal_inertia_local_frame, - } - } -} - -impl Zero for MassProperties { - fn zero() -> Self { - Self { - inv_mass: 0.0, - inv_principal_inertia_sqrt: na::zero(), - #[cfg(feature = "dim3")] - principal_inertia_local_frame: Rotation::identity(), - local_com: Point::origin(), - } - } - - fn is_zero(&self) -> bool { - *self == Self::zero() - } -} - -impl Sub for MassProperties { - type Output = Self; - - #[cfg(feature = "dim2")] - fn sub(self, other: MassProperties) -> Self { - if self.is_zero() || other.is_zero() { - return self; - } - - let m1 = utils::inv(self.inv_mass); - let m2 = utils::inv(other.inv_mass); - let inv_mass = utils::inv(m1 - m2); - - let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass; - let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); - let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); - let inertia = i1 - i2; - // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors. - let inv_principal_inertia_sqrt = utils::inv(inertia.max(0.0).sqrt()); - - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - } - } - - #[cfg(feature = "dim3")] - fn sub(self, other: MassProperties) -> Self { - if self.is_zero() || other.is_zero() { - return self; - } - - let m1 = utils::inv(self.inv_mass); - let m2 = utils::inv(other.inv_mass); - let inv_mass = utils::inv(m1 - m2); - let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass; - let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); - let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); - let inertia = i1 - i2; - let eigen = inertia.symmetric_eigen(); - let principal_inertia_local_frame = - Rotation::from_matrix_eps(&eigen.eigenvectors, 1.0e-6, 10, na::one()); - let principal_inertia = eigen.eigenvalues; - // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors. - let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.max(0.0).sqrt())); - - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - principal_inertia_local_frame, - } - } -} - -impl SubAssign for MassProperties { - fn sub_assign(&mut self, rhs: MassProperties) { - *self = *self - rhs - } -} - -impl Add for MassProperties { - type Output = Self; - - #[cfg(feature = "dim2")] - fn add(self, other: MassProperties) -> Self { - if self.is_zero() { - return other; - } else if other.is_zero() { - return self; - } - - let m1 = utils::inv(self.inv_mass); - let m2 = utils::inv(other.inv_mass); - let inv_mass = utils::inv(m1 + m2); - let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass; - let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); - let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); - let inertia = i1 + i2; - let inv_principal_inertia_sqrt = utils::inv(inertia.sqrt()); - - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - } - } - - #[cfg(feature = "dim3")] - fn add(self, other: MassProperties) -> Self { - if self.is_zero() { - return other; - } else if other.is_zero() { - return self; - } - - let m1 = utils::inv(self.inv_mass); - let m2 = utils::inv(other.inv_mass); - let inv_mass = utils::inv(m1 + m2); - let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass; - let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); - let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); - let inertia = i1 + i2; - let eigen = inertia.symmetric_eigen(); - let principal_inertia_local_frame = - Rotation::from_matrix_eps(&eigen.eigenvectors, 1.0e-6, 10, na::one()); - let principal_inertia = eigen.eigenvalues; - let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt())); - - Self { - local_com, - inv_mass, - inv_principal_inertia_sqrt, - principal_inertia_local_frame, - } - } -} - -impl AddAssign for MassProperties { - fn add_assign(&mut self, rhs: MassProperties) { - *self = *self + rhs - } -} - -impl approx::AbsDiffEq for MassProperties { - type Epsilon = f32; - fn default_epsilon() -> Self::Epsilon { - f32::default_epsilon() - } - - fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { - #[cfg(feature = "dim2")] - let inertia_is_ok = self - .inv_principal_inertia_sqrt - .abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon); - - #[cfg(feature = "dim3")] - let inertia_is_ok = self - .reconstruct_inverse_inertia_matrix() - .abs_diff_eq(&other.reconstruct_inverse_inertia_matrix(), epsilon); - - inertia_is_ok - && self.local_com.abs_diff_eq(&other.local_com, epsilon) - && self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon) - && self - .inv_principal_inertia_sqrt - .abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon) - } -} - -impl approx::RelativeEq for MassProperties { - fn default_max_relative() -> Self::Epsilon { - f32::default_max_relative() - } - - fn relative_eq( - &self, - other: &Self, - epsilon: Self::Epsilon, - max_relative: Self::Epsilon, - ) -> bool { - #[cfg(feature = "dim2")] - let inertia_is_ok = self.inv_principal_inertia_sqrt.relative_eq( - &other.inv_principal_inertia_sqrt, - epsilon, - max_relative, - ); - - #[cfg(feature = "dim3")] - let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq( - &other.reconstruct_inverse_inertia_matrix(), - epsilon, - max_relative, - ); - - inertia_is_ok - && self - .local_com - .relative_eq(&other.local_com, epsilon, max_relative) - && self - .inv_mass - .relative_eq(&other.inv_mass, epsilon, max_relative) - } -} - -#[cfg(test)] -mod test { - use super::MassProperties; - use crate::geometry::ColliderBuilder; - use crate::math::{Point, Rotation, Vector}; - use approx::assert_relative_eq; - use num::Zero; - - #[test] - fn mass_properties_add_partial_zero() { - let m1 = MassProperties { - local_com: Point::origin(), - inv_mass: 2.0, - inv_principal_inertia_sqrt: na::zero(), - #[cfg(feature = "dim3")] - principal_inertia_local_frame: Rotation::identity(), - }; - let m2 = MassProperties { - local_com: Point::origin(), - inv_mass: 0.0, - #[cfg(feature = "dim2")] - inv_principal_inertia_sqrt: 1.0, - #[cfg(feature = "dim3")] - inv_principal_inertia_sqrt: Vector::new(1.0, 2.0, 3.0), - #[cfg(feature = "dim3")] - principal_inertia_local_frame: Rotation::identity(), - }; - let result = MassProperties { - local_com: Point::origin(), - inv_mass: 2.0, - #[cfg(feature = "dim2")] - inv_principal_inertia_sqrt: 1.0, - #[cfg(feature = "dim3")] - inv_principal_inertia_sqrt: Vector::new(1.0, 2.0, 3.0), - #[cfg(feature = "dim3")] - principal_inertia_local_frame: Rotation::identity(), - }; - - assert_eq!(m1 + m2, result); - assert_eq!(m2 + m1, result); - } - - #[test] - fn mass_properties_add_sub() { - // Check that addition and subtraction of mass properties behave as expected. - let c1 = ColliderBuilder::capsule_x(1.0, 2.0).build(); - let c2 = ColliderBuilder::capsule_y(3.0, 4.0).build(); - let c3 = ColliderBuilder::ball(5.0).build(); - - let m1 = c1.mass_properties(); - let m2 = c2.mass_properties(); - let m3 = c3.mass_properties(); - let m1m2m3 = m1 + m2 + m3; - - assert_relative_eq!(m1 + m2, m2 + m1, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - m1, m2 + m3, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - m2, m1 + m3, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - m3, m1 + m2, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - (m1 + m2), m3, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - (m1 + m3), m2, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - (m2 + m3), m1, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - m1 - m2, m3, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - m1 - m3, m2, epsilon = 1.0e-6); - assert_relative_eq!(m1m2m3 - m2 - m3, m1, epsilon = 1.0e-6); - assert_relative_eq!( - m1m2m3 - m1 - m2 - m3, - MassProperties::zero(), - epsilon = 1.0e-6 - ); - } -} diff --git a/src/dynamics/mass_properties_ball.rs b/src/dynamics/mass_properties_ball.rs deleted file mode 100644 index ac5790a..0000000 --- a/src/dynamics/mass_properties_ball.rs +++ /dev/null @@ -1,30 +0,0 @@ -use crate::dynamics::MassProperties; -#[cfg(feature = "dim3")] -use crate::math::Vector; -use crate::math::{Point, PrincipalAngularInertia}; - -impl MassProperties { - pub(crate) fn ball_volume_unit_angular_inertia( - radius: f32, - ) -> (f32, PrincipalAngularInertia) { - #[cfg(feature = "dim2")] - { - let volume = std::f32::consts::PI * radius * radius; - let i = radius * radius / 2.0; - (volume, i) - } - #[cfg(feature = "dim3")] - { - let volume = std::f32::consts::PI * radius * radius * radius * 4.0 / 3.0; - let i = radius * radius * 2.0 / 5.0; - - (volume, Vector::repeat(i)) - } - } - - pub(crate) fn from_ball(density: f32, radius: f32) -> Self { - let (vol, unit_i) = Self::ball_volume_unit_angular_inertia(radius); - let mass = vol * density; - Self::new(Point::origin(), mass, unit_i * mass) - } -} diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs deleted file mode 100644 index 3b1b214..0000000 --- a/src/dynamics/mass_properties_capsule.rs +++ /dev/null @@ -1,39 +0,0 @@ -use crate::dynamics::MassProperties; -#[cfg(feature = "dim3")] -use crate::geometry::Capsule; -use crate::math::Point; - -impl MassProperties { - pub(crate) fn from_capsule(density: f32, a: Point, b: Point, radius: f32) -> Self { - let half_height = (b - a).norm() / 2.0; - let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); - let (ball_vol, ball_unit_i) = Self::ball_volume_unit_angular_inertia(radius); - let cap_vol = cyl_vol + ball_vol; - let cap_mass = cap_vol * density; - let mut cap_unit_i = cyl_unit_i + ball_unit_i; - let local_com = na::center(&a, &b); - - #[cfg(feature = "dim2")] - { - let h = half_height * 2.0; - let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; - cap_unit_i += extra; - Self::new(local_com, cap_mass, cap_unit_i * cap_mass) - } - - #[cfg(feature = "dim3")] - { - let h = half_height * 2.0; - let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; - cap_unit_i.x += extra; - cap_unit_i.z += extra; - let local_frame = Capsule::new(a, b, radius).rotation_wrt_y(); - Self::with_principal_inertia_frame( - local_com, - cap_mass, - cap_unit_i * cap_mass, - local_frame, - ) - } - } -} diff --git a/src/dynamics/mass_properties_cone.rs b/src/dynamics/mass_properties_cone.rs deleted file mode 100644 index 12f831f..0000000 --- a/src/dynamics/mass_properties_cone.rs +++ /dev/null @@ -1,29 +0,0 @@ -use crate::dynamics::MassProperties; -use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector}; - -impl MassProperties { - pub(crate) fn cone_y_volume_unit_inertia( - half_height: f32, - radius: f32, - ) -> (f32, PrincipalAngularInertia) { - let volume = radius * radius * std::f32::consts::PI * half_height * 2.0 / 3.0; - let sq_radius = radius * radius; - let sq_height = half_height * half_height * 4.0; - let off_principal = sq_radius * 3.0 / 20.0 + sq_height * 3.0 / 5.0; - let principal = sq_radius * 3.0 / 10.0; - - (volume, Vector::new(off_principal, principal, off_principal)) - } - - pub(crate) fn from_cone(density: f32, half_height: f32, radius: f32) -> Self { - let (cyl_vol, cyl_unit_i) = Self::cone_y_volume_unit_inertia(half_height, radius); - let cyl_mass = cyl_vol * density; - - Self::with_principal_inertia_frame( - Point::new(0.0, -half_height / 2.0, 0.0), - cyl_mass, - cyl_unit_i * cyl_mass, - Rotation::identity(), - ) - } -} diff --git a/src/dynamics/mass_properties_cuboid.rs b/src/dynamics/mass_properties_cuboid.rs deleted file mode 100644 index 2d870cf..0000000 --- a/src/dynamics/mass_properties_cuboid.rs +++ /dev/null @@ -1,33 +0,0 @@ -use crate::dynamics::MassProperties; -use crate::math::{Point, PrincipalAngularInertia, Vector}; - -impl MassProperties { - pub(crate) fn cuboid_volume_unit_inertia( - half_extents: Vector, - ) -> (f32, PrincipalAngularInertia) { - #[cfg(feature = "dim2")] - { - let volume = half_extents.x * half_extents.y * 4.0; - let ix = (half_extents.x * half_extents.x) / 3.0; - let iy = (half_extents.y * half_extents.y) / 3.0; - - (volume, ix + iy) - } - - #[cfg(feature = "dim3")] - { - let volume = half_extents.x * half_extents.y * half_extents.z * 8.0; - let ix = (half_extents.x * half_extents.x) / 3.0; - let iy = (half_extents.y * half_extents.y) / 3.0; - let iz = (half_extents.z * half_extents.z) / 3.0; - - (volume, Vector::new(iy + iz, ix + iz, ix + iy)) - } - } - - pub(crate) fn from_cuboid(density: f32, half_extents: Vector) -> Self { - let (vol, unit_i) = Self::cuboid_volume_unit_inertia(half_extents); - let mass = vol * density; - Self::new(Point::origin(), mass, unit_i * mass) - } -} diff --git a/src/dynamics/mass_properties_cylinder.rs b/src/dynamics/mass_properties_cylinder.rs deleted file mode 100644 index 7c8054a..0000000 --- a/src/dynamics/mass_properties_cylinder.rs +++ /dev/null @@ -1,40 +0,0 @@ -use crate::dynamics::MassProperties; -#[cfg(feature = "dim3")] -use crate::math::{Point, Rotation}; -use crate::math::{PrincipalAngularInertia, Vector}; - -impl MassProperties { - pub(crate) fn cylinder_y_volume_unit_inertia( - half_height: f32, - radius: f32, - ) -> (f32, PrincipalAngularInertia) { - #[cfg(feature = "dim2")] - { - Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height)) - } - - #[cfg(feature = "dim3")] - { - let volume = half_height * radius * radius * std::f32::consts::PI * 2.0; - let sq_radius = radius * radius; - let sq_height = half_height * half_height * 4.0; - let off_principal = (sq_radius * 3.0 + sq_height) / 12.0; - - let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal); - (volume, inertia) - } - } - - #[cfg(feature = "dim3")] - pub(crate) fn from_cylinder(density: f32, half_height: f32, radius: f32) -> Self { - let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); - let cyl_mass = cyl_vol * density; - - Self::with_principal_inertia_frame( - Point::origin(), - cyl_mass, - cyl_unit_i * cyl_mass, - Rotation::identity(), - ) - } -} diff --git a/src/dynamics/mass_properties_polygon.rs b/src/dynamics/mass_properties_polygon.rs deleted file mode 100644 index 8b0b811..0000000 --- a/src/dynamics/mass_properties_polygon.rs +++ /dev/null @@ -1,146 +0,0 @@ -#![allow(dead_code)] // TODO: remove this - -use crate::dynamics::MassProperties; -use crate::math::Point; - -impl MassProperties { - pub(crate) fn from_polygon(density: f32, vertices: &[Point]) -> MassProperties { - let (area, com) = convex_polygon_area_and_center_of_mass(vertices); - - if area == 0.0 { - return MassProperties::new(com, 0.0, 0.0); - } - - let mut itot = 0.0; - let factor = 1.0 / 6.0; - - let mut iterpeek = vertices.iter().peekable(); - let firstelement = *iterpeek.peek().unwrap(); // store first element to close the cycle in the end with unwrap_or - while let Some(elem) = iterpeek.next() { - let area = triangle_area(&com, elem, iterpeek.peek().unwrap_or(&firstelement)); - - // algorithm adapted from Box2D - let e1 = *elem - com; - let e2 = **(iterpeek.peek().unwrap_or(&firstelement)) - com; - - let ex1 = e1[0]; - let ey1 = e1[1]; - let ex2 = e2[0]; - let ey2 = e2[1]; - - let intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2; - let inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2; - - let ipart = factor * (intx2 + inty2); - - itot += ipart * area; - } - - Self::new(com, area * density, itot * density) - } -} - -fn convex_polygon_area_and_center_of_mass(convex_polygon: &[Point]) -> (f32, Point) { - let geometric_center = convex_polygon - .iter() - .fold(Point::origin(), |e1, e2| e1 + e2.coords) - / convex_polygon.len() as f32; - let mut res = Point::origin(); - let mut areasum = 0.0; - - let mut iterpeek = convex_polygon.iter().peekable(); - let firstelement = *iterpeek.peek().unwrap(); // Stores first element to close the cycle in the end with unwrap_or. - while let Some(elem) = iterpeek.next() { - let (a, b, c) = ( - elem, - iterpeek.peek().unwrap_or(&firstelement), - &geometric_center, - ); - let area = triangle_area(a, b, c); - let center = (a.coords + b.coords + c.coords) / 3.0; - - res += center * area; - areasum += area; - } - - if areasum == 0.0 { - (areasum, geometric_center) - } else { - (areasum, res / areasum) - } -} - -pub fn triangle_area(pa: &Point, pb: &Point, pc: &Point) -> f32 { - // Kahan's formula. - let a = na::distance(pa, pb); - let b = na::distance(pb, pc); - let c = na::distance(pc, pa); - - let (c, b, a) = sort3(&a, &b, &c); - let a = *a; - let b = *b;