From f108520b5a110cf59864abac7ac6a37e2b5a1dd9 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Wed, 20 Apr 2022 12:29:57 +0200 Subject: Finalize refactoring --- src/dynamics/ccd/ccd_solver.rs | 311 +++++++++------------ src/dynamics/ccd/toi_entry.rs | 104 +++---- src/dynamics/island_manager.rs | 104 +++---- .../joint/impulse_joint/impulse_joint_set.rs | 30 +- src/dynamics/joint/multibody_joint/multibody.rs | 70 +++-- src/dynamics/rigid_body_components.rs | 25 +- src/dynamics/solver/categorization.rs | 8 +- src/dynamics/solver/generic_velocity_constraint.rs | 92 +++--- .../solver/generic_velocity_ground_constraint.rs | 30 +- src/dynamics/solver/interaction_groups.rs | 55 ++-- .../solver/joint_constraint/joint_constraint.rs | 129 ++++----- src/dynamics/solver/parallel_island_solver.rs | 13 +- src/dynamics/solver/parallel_solver_constraints.rs | 3 +- src/dynamics/solver/parallel_velocity_solver.rs | 51 +--- src/dynamics/solver/velocity_constraint.rs | 17 +- src/dynamics/solver/velocity_constraint_wide.rs | 16 +- src/dynamics/solver/velocity_ground_constraint.rs | 16 +- .../solver/velocity_ground_constraint_wide.rs | 14 +- src/dynamics/solver/velocity_solver.rs | 64 ++--- 19 files changed, 475 insertions(+), 677 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index 8fc5a7f..77a1ff7 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -1,16 +1,10 @@ use super::TOIEntry; -use crate::dynamics::{ - IslandManager, RigidBodyCcd, RigidBodyColliders, RigidBodyForces, RigidBodyHandle, - RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyVelocity, -}; -use crate::geometry::{ - ColliderParent, ColliderPosition, ColliderSet, ColliderShape, ColliderType, CollisionEvent, - NarrowPhase, -}; +use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet}; +use crate::geometry::{ColliderParent, ColliderSet, CollisionEvent, NarrowPhase}; use crate::math::Real; use crate::parry::utils::SortedPair; use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode}; -use crate::prelude::{ActiveEvents, ColliderFlags}; +use crate::prelude::ActiveEvents; use parry::query::{DefaultQueryDispatcher, QueryDispatcher}; use parry::utils::hashmap::HashMap; use std::collections::BinaryHeap; @@ -61,23 +55,18 @@ impl CCDSolver { match impacts { PredictedImpacts::Impacts(tois) => { for (handle, toi) in tois { - let (rb_poss, vels, ccd, mprops): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyCcd, - &RigidBodyMassProps, - ) = bodies.index_bundle(handle.0); - let local_com = &mprops.local_mprops.local_com; - - let min_toi = (ccd.ccd_thickness + let rb = bodies.index_mut_internal(*handle); + let local_com = &rb.mprops.local_mprops.local_com; + + let min_toi = (rb.ccd.ccd_thickness * 0.15 - * crate::utils::inv(ccd.max_point_velocity(vels))) + * crate::utils::inv(rb.ccd.max_point_velocity(&rb.vels))) .min(dt); // println!("Min toi: {}, Toi: {}", min_toi, toi); - let new_pos = vels.integrate(toi.max(min_toi), &rb_poss.position, &local_com); - bodies.map_mut_internal(handle.0, |rb_poss| { - rb_poss.next_position = new_pos; - }); + let new_pos = rb + .vels + .integrate(toi.max(min_toi), &rb.pos.position, &local_com); + rb.pos.next_position = new_pos; } } _ => {} @@ -98,17 +87,16 @@ impl CCDSolver { // println!("Checking CCD activation"); for handle in islands.active_dynamic_bodies() { - let (ccd, vels, forces): (&RigidBodyCcd, &RigidBodyVelocity, &RigidBodyForces) = - bodies.index_bundle(handle.0); - - if ccd.ccd_enabled { - let forces = if include_forces { Some(forces) } else { None }; - let moving_fast = ccd.is_moving_fast(dt, vels, forces); - - bodies.map_mut_internal(handle.0, |ccd| { - ccd.ccd_active = moving_fast; - }); - + let rb = bodies.index_mut_internal(*handle); + + if rb.ccd.ccd_enabled { + let forces = if include_forces { + Some(&rb.forces) + } else { + None + }; + let moving_fast = rb.ccd.is_moving_fast(dt, &rb.vels, forces); + rb.ccd.ccd_active = moving_fast; ccd_active = ccd_active || moving_fast; } } @@ -137,36 +125,31 @@ impl CCDSolver { let mut min_toi = dt; for handle in islands.active_dynamic_bodies() { - let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0); - - if rb_ccd1.ccd_active { - let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyForces, - &RigidBodyMassProps, - &RigidBodyColliders, - ) = bodies.index_bundle(handle.0); - - let predicted_body_pos1 = - rb_pos1.integrate_forces_and_velocities(dt, forces1, rb_vels1, rb_mprops1); - - for ch1 in &rb_colliders1.0 { - let co_parent1: &ColliderParent = colliders - .get(ch1.0) + let rb1 = &bodies[*handle]; + + if rb1.ccd.ccd_active { + let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( + dt, + &rb1.forces, + &rb1.vels, + &rb1.mprops, + ); + + for ch1 in &rb1.colliders.0 { + let co1 = &colliders[*ch1]; + let co1_parent = co1 + .parent + .as_ref() .expect("Could not find the ColliderParent component."); - let (co_shape1, co_pos1, co_type1): ( - &ColliderShape, - &ColliderPosition, - &ColliderType, - ) = colliders.index_bundle(ch1.0); - if co_type1.is_sensor() { + if co1.is_sensor() { continue; // Ignore sensors. } - let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent; - let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1); + let predicted_collider_pos1 = predicted_body_pos1 * co1_parent.pos_wrt_parent; + let aabb1 = co1 + .shape + .compute_swept_aabb(&co1.pos, &predicted_collider_pos1); self.query_pipeline .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { @@ -182,21 +165,17 @@ impl CCDSolver { ) .is_none() { - let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); - let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); - let c1: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch1.0); - let c2: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch2.0); - let co_type1: &ColliderType = colliders.index(ch1.0); - let co_type2: &ColliderType = colliders.index(ch1.0); + let co1 = &colliders[*ch1]; + let co2 = &colliders[*ch2]; - let bh1 = co_parent1.map(|p| p.handle); - let bh2 = co_parent2.map(|p| p.handle); + let bh1 = co1.parent.map(|p| p.handle); + let bh2 = co2.parent.map(|p| p.handle); // Ignore self-intersection and sensors and apply collision groups filter. - if bh1 == bh2 // Ignore self-intersection. - || (co_type1.is_sensor() || co_type2.is_sensor()) // Ignore sensors. - || !c1.3.collision_groups.test(c2.3.collision_groups) // Apply collision groups. - || !c1.3.solver_groups.test(c2.3.solver_groups) + if bh1 == bh2 // Ignore self-intersection. + || (co1.is_sensor() || co2.is_sensor()) // Ignore sensors. + || !co1.flags.collision_groups.test(co2.flags.collision_groups) // Apply collision groups. + || !co1.flags.solver_groups.test(co2.flags.solver_groups) // Apply solver groups. { return true; @@ -208,16 +187,16 @@ impl CCDSolver { .map(|c| c.1.dist) .unwrap_or(0.0); - let b2 = bh2.map(|h| bodies.index_bundle(h.0)); + let rb2 = bh2.and_then(|h| bodies.get(h)); if let Some(toi) = TOIEntry::try_from_colliders( self.query_pipeline.query_dispatcher(), *ch1, *ch2, - (c1.0, c1.1, c1.2, c1.3, co_parent1), - (c2.0, c2.1, c2.2, c2.3, co_parent2), - Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)), - b2, + co1, + co2, + Some(rb1), + rb2, None, None, 0.0, @@ -271,29 +250,27 @@ impl CCDSolver { */ // TODO: don't iterate through all the colliders. for handle in islands.active_dynamic_bodies() { - let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0); - - if rb_ccd1.ccd_active { - let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyForces, - &RigidBodyMassProps, - &RigidBodyColliders, - ) = bodies.index_bundle(handle.0); - - let predicted_body_pos1 = - rb_pos1.integrate_forces_and_velocities(dt, forces1, rb_vels1, rb_mprops1); - - for ch1 in &rb_colliders1.0 { - let co_parent1: &ColliderParent = colliders - .get(ch1.0) + let rb1 = &bodies[*handle]; + + if rb1.ccd.ccd_active { + let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( + dt, + &rb1.forces, + &rb1.vels, + &rb1.mprops, + ); + + for ch1 in &rb1.colliders.0 { + let co1 = &colliders[*ch1]; + let co_parent1 = co1 + .parent + .as_ref() .expect("Could not find the ColliderParent component."); - let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) = - colliders.index_bundle(ch1.0); let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent; - let aabb1 = co_shape1.compute_swept_aabb(&co_pos1, &predicted_collider_pos1); + let aabb1 = co1 + .shape + .compute_swept_aabb(&co1.pos, &predicted_collider_pos1); self.query_pipeline .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { @@ -309,16 +286,15 @@ impl CCDSolver { ) .is_none() { - let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); - let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); - let c1: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch1.0); - let c2: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch2.0); + let co1 = &colliders[*ch1]; + let co2 = &colliders[*ch2]; - let bh1 = co_parent1.map(|p| p.handle); - let bh2 = co_parent2.map(|p| p.handle); + let bh1 = co1.parent.map(|p| p.handle); + let bh2 = co2.parent.map(|p| p.handle); // Ignore self-intersections and apply groups filter. - if bh1 == bh2 || !c1.3.collision_groups.test(c2.3.collision_groups) + if bh1 == bh2 + || !co1.flags.collision_groups.test(co2.flags.collision_groups) { return true; } @@ -329,17 +305,17 @@ impl CCDSolver { .map(|c| c.1.dist) .unwrap_or(0.0); - let b1 = bh1.map(|h| bodies.index_bundle(h.0)); - let b2 = bh2.map(|h| bodies.index_bundle(h.0)); + let rb1 = bh1.map(|h| &bodies[h]); + let rb2 = bh2.map(|h| &bodies[h]); if let Some(toi) = TOIEntry::try_from_colliders( self.query_pipeline.query_dispatcher(), *ch1, *ch2, - (c1.0, c1.1, c1.2, c1.3, co_parent1), - (c2.0, c2.1, c2.2, c2.3, co_parent2), - b1, - b2, + co1, + co2, + rb1, + rb2, None, None, 0.0, @@ -381,17 +357,15 @@ impl CCDSolver { while let Some(toi) = all_toi.pop() { assert!(toi.toi <= dt); - let rb1: Option<(&RigidBodyCcd, &RigidBodyColliders)> = - toi.b1.map(|b| bodies.index_bundle(b.0)); - let rb2: Option<(&RigidBodyCcd, &RigidBodyColliders)> = - toi.b2.map(|b| bodies.index_bundle(b.0)); + let rb1 = toi.b1.and_then(|b| bodies.get(b)); + let rb2 = toi.b2.and_then(|b| bodies.get(b)); let mut colliders_to_check = Vec::new(); let should_freeze1 = rb1.is_some() - && rb1.unwrap().0.ccd_active + && rb1.unwrap().ccd.ccd_active && !frozen.contains_key(&toi.b1.unwrap()); let should_freeze2 = rb2.is_some() - && rb2.unwrap().0.ccd_active + && rb2.unwrap().ccd.ccd_active && !frozen.contains_key(&toi.b2.unwrap()); if !should_freeze1 && !should_freeze2 { @@ -413,12 +387,12 @@ impl CCDSolver { if should_freeze1 { let _ = frozen.insert(toi.b1.unwrap(), toi.toi); - colliders_to_check.extend_from_slice(&rb1.unwrap().1 .0); + colliders_to_check.extend_from_slice(&rb1.unwrap().colliders.0); } if should_freeze2 { let _ = frozen.insert(toi.b2.unwrap(), toi.toi); - colliders_to_check.extend_from_slice(&rb2.unwrap().1 .0); + colliders_to_check.extend_from_slice(&rb2.unwrap().colliders.0); } let start_time = toi.toi; @@ -426,39 +400,36 @@ impl CCDSolver { // NOTE: the 1 and 2 indices (e.g., `ch1`, `ch2`) bellow are unrelated to the // ones we used above. for ch1 in &colliders_to_check { - let co_parent1: &ColliderParent = colliders.get(ch1.0).unwrap(); - let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) = - colliders.index_bundle(ch1.0); + let co1 = &colliders[*ch1]; + let co1_parent = co1.parent.as_ref().unwrap(); + let rb1 = &bodies[co1_parent.handle]; - let rb_pos1: &RigidBodyPosition = bodies.index(co_parent1.handle.0); - let co_next_pos1 = rb_pos1.next_position * co_parent1.pos_wrt_parent; - let aabb = co_shape1.compute_swept_aabb(&co_pos1, &co_next_pos1); + let co_next_pos1 = rb1.pos.next_position * co1_parent.pos_wrt_parent; + let aabb = co1.shape.compute_swept_aabb(&co1.pos, &co_next_pos1); self.query_pipeline .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| { - let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); - let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); - let c1: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch1.0); - let c2: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch2.0); + let co2 = &colliders[*ch2]; - let bh1 = co_parent1.map(|p| p.handle); - let bh2 = co_parent2.map(|p| p.handle); + let bh1 = co1.parent.map(|p| p.handle); + let bh2 = co2.parent.map(|p| p.handle); // Ignore self-intersection and apply groups filter. - if bh1 == bh2 || !c1.3.collision_groups.test(c2.3.collision_groups) { + if bh1 == bh2 + || !co1.flags.collision_groups.test(co2.flags.collision_groups) + { return true; } let frozen1 = bh1.and_then(|h| frozen.get(&h)); let frozen2 = bh2.and_then(|h| frozen.get(&h)); - let b1: Option<(_, _, _, &RigidBodyCcd)> = - bh1.map(|h| bodies.index_bundle(h.0)); - let b2: Option<(_, _, _, &RigidBodyCcd)> = - bh2.map(|h| bodies.index_bundle(h.0)); + let rb1 = bh1.and_then(|h| bodies.get(h)); + let rb2 = bh2.and_then(|h| bodies.get(h)); - if (frozen1.is_some() || !b1.map(|b| b.3.ccd_active).unwrap_or(false)) - && (frozen2.is_some() || !b2.map(|b| b.3.ccd_active).unwrap_or(false)) + if (frozen1.is_some() || !rb1.map(|b| b.ccd.ccd_active).unwrap_or(false)) + && (frozen2.is_some() + || !rb2.map(|b| b.ccd.ccd_active).unwrap_or(false)) { // We already did a resweep. return true; @@ -474,10 +445,10 @@ impl CCDSolver { self.query_pipeline.query_dispatcher(), *ch1, *ch2, - (c1.0, c1.1, c1.2, c1.3, co_parent1), - (c2.0, c2.1, c2.2, c2.3, co_parent2), - b1, - b2, + co1, + co2, + rb1, + rb2, frozen1.copied(), frozen2.copied(), start_time, @@ -500,20 +471,10 @@ impl CCDSolver { // - If the intersection isn't active anymore, and it wasn't intersecting // before, then we need to generate one interaction-start and one interaction-stop // events because it will never be detected by the narrow-phase because of tunneling. - let (co_type1, co_pos1, co_shape1, co_flags1): ( - &ColliderType, - &ColliderPosition, - &ColliderShape, - &ColliderFlags, - ) = colliders.index_bundle(toi.c1.0); - let (co_type2, co_pos2, co_shape2, co_flags2): ( - &ColliderType, - &ColliderPosition, - &ColliderShape, - &ColliderFlags, - ) = colliders.index_bundle(toi.c2.0); - - if !co_type1.is_sensor() && !co_type2.is_sensor() { + let co1 = &colliders[toi.c1]; + let co2 = &colliders[toi.c2]; + + if !co1.is_sensor() && !co2.is_sensor() { // TODO: this happens if we found a TOI between two non-sensor // colliders with mismatching solver_flags. It is not clear // what we should do in this case: we could report a @@ -525,56 +486,46 @@ impl CCDSolver { } let co_next_pos1 = if let Some(b1) = toi.b1 { - let co_parent1: &ColliderParent = colliders.get(toi.c1.0).unwrap(); - let (rb_pos1, rb_vels1, rb_mprops1): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - ) = bodies.index_bundle(b1.0); - - let local_com1 = &rb_mprops1.local_mprops.local_com; + let co_parent1: &ColliderParent = co1.parent.as_ref().unwrap(); + let rb1 = &bodies[b1]; + let local_com1 = &rb1.mprops.local_mprops.local_com; let frozen1 = frozen.get(&b1); let pos1 = frozen1 - .map(|t| rb_vels1.integrate(*t, &rb_pos1.position, local_com1)) - .unwrap_or(rb_pos1.next_position); + .map(|t| rb1.vels.integrate(*t, &rb1.pos.position, local_com1)) + .unwrap_or(rb1.pos.next_position); pos1 * co_parent1.pos_wrt_parent } else { - co_pos1.0 + co1.pos.0 }; let co_next_pos2 = if let Some(b2) = toi.b2 { - let co_parent2: &ColliderParent = colliders.get(toi.c2.0).unwrap(); - let (rb_pos2, rb_vels2, rb_mprops2): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - ) = bodies.index_bundle(b2.0); - - let local_com2 = &rb_mprops2.local_mprops.local_com; + let co_parent2: &ColliderParent = co2.parent.as_ref().unwrap(); + let rb2 = &bodies[b2]; + let local_com2 = &rb2.mprops.local_mprops.local_com; let frozen2 = frozen.get(&b2); let pos2 = frozen2 - .map(|t| rb_vels2.integrate(*t, &rb_pos2.position, local_com2)) - .unwrap_or(rb_pos2.next_position); + .map(|t| rb2.vels.integrate(*t, &rb2.pos.position, local_com2)) + .unwrap_or(rb2.pos.next_position); pos2 * co_parent2.pos_wrt_parent } else { - co_pos2.0 + co2.pos.0 }; - let prev_coll_pos12 = co_pos1.inv_mul(&co_pos2); + let prev_coll_pos12 = co1.pos.inv_mul(&co2.pos); let next_coll_pos12 = co_next_pos1.inv_mul(&co_next_pos2); let query_dispatcher = self.query_pipeline.query_dispatcher(); let intersect_before = query_dispatcher - .intersection_test(&prev_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref()) + .intersection_test(&prev_coll_pos12, co1.shape.as_ref(), co2.shape.as_ref()) .unwrap_or(false); let intersect_after = query_dispatcher - .intersection_test(&next_coll_pos12, co_shape1.as_ref(), co_shape2.as_ref()) + .intersection_test(&next_coll_pos12, co1.shape.as_ref(), co2.shape.as_ref()) .unwrap_or(false); if !intersect_before && !intersect_after - && (co_flags1.active_events | co_flags2.active_events) + && (co1.flags.active_events | co2.flags.active_events) .contains(ActiveEvents::COLLISION_EVENTS) { // Emit one intersection-started and one intersection-stopped event. diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index f937979..4591825 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -1,9 +1,5 @@ -use crate::dynamics::{ - RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, -}; -use crate::geometry::{ - ColliderFlags, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType, -}; +use crate::dynamics::{RigidBody, RigidBodyHandle}; +use crate::geometry::{Collider, ColliderHandle}; use crate::math::Real; use parry::query::{NonlinearRigidMotion, QueryDispatcher}; @@ -45,32 +41,10 @@ impl TOIEntry { query_dispatcher: &QD, ch1: ColliderHandle, ch2: ColliderHandle, - c1: ( - &ColliderType, - &ColliderShape, - &ColliderPosition, - &ColliderFlags, - Option<&ColliderParent>, - ), - c2: ( - &ColliderType, - &ColliderShape, - &ColliderPosition, - &ColliderFlags, - Option<&ColliderParent>, - ), - b1: Option<( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyCcd, - )>, - b2: Option<( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyCcd, - )>, + co1: &Collider, + co2: &Collider, + rb1: Option<&RigidBody>, + rb2: Option<&RigidBody>, frozen1: Option, frozen2: Option, start_time: Real, @@ -78,39 +52,36 @@ impl TOIEntry { smallest_contact_dist: Real, ) -> Option { assert!(start_time <= end_time); - if b1.is_none() && b2.is_none() { + if rb1.is_none() && rb2.is_none() { return None; } - let (co_type1, co_shape1, co_pos1, co_flags1, co_parent1) = c1; - let (co_type2, co_shape2, co_pos2, co_flags2, co_parent2) = c2; - let linvel1 = - frozen1.is_none() as u32 as Real * b1.map(|b| b.1.linvel).unwrap_or(na::zero()); + frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.linvel).unwrap_or(na::zero()); let linvel2 = - frozen2.is_none() as u32 as Real * b2.map(|b| b.1.linvel).unwrap_or(na::zero()); + frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.linvel).unwrap_or(na::zero()); let angvel1 = - frozen1.is_none() as u32 as Real * b1.map(|b| b.1.angvel).unwrap_or(na::zero()); + frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.angvel).unwrap_or(na::zero()); let angvel2 = - frozen2.is_none() as u32 as Real * b2.map(|b| b.1.angvel).unwrap_or(na::zero()); + frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.angvel).unwrap_or(na::zero()); #[cfg(feature = "dim2")] let vel12 = (linvel2 - linvel1).norm() - + angvel1.abs() * b1.map(|b| b.3.ccd_max_dist).unwrap_or(0.0) - + angvel2.abs() * b2.map(|b| b.3.ccd_max_dist).unwrap_or(0.0); + + angvel1.abs() * rb1.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0) + + angvel2.abs() * rb2.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0); #[cfg(feature = "dim3")] let vel12 = (linvel2 - linvel1).norm() - + angvel1.norm() * b1.map(|b| b.3.ccd_max_dist).unwrap_or(0.0) - + angvel2.norm() * b2.map(|b| b.3.ccd_max_dist).unwrap_or(0.0); + + angvel1.norm() * rb1.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0) + + angvel2.norm() * rb2.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0); // We may be slightly over-conservative by taking the `max(0.0)` here. // But removing the `max` doesn't really affect performances so let's // keep it since more conservatism is good at this stage. - let thickness = (co_shape1.0.ccd_thickness() + co_shape2.0.ccd_thickness()) + let thickness = (co1.shape.0.ccd_thickness() + co2.shape.0.ccd_thickness()) + smallest_contact_dist.max(0.0); - let is_pseudo_intersection_test = co_type1.is_sensor() - || co_type2.is_sensor() - || !co_flags1.solver_groups.test(co_flags2.solver_groups); + let is_pseudo_intersection_test = co1.is_sensor() + || co2.is_sensor() + || !co1.flags.solver_groups.test(co2.flags.solver_groups); if (end_time - start_time) * vel12 < thickness { return None; @@ -118,8 +89,8 @@ impl TOIEntry { // Compute the TOI. let identity = NonlinearRigidMotion::identity(); - let mut motion1 = b1.map(Self::body_motion).unwrap_or(identity); - let mut motion2 = b2.map(Self::body_motion).unwrap_or(identity); + let mut motion1 = rb1.map(Self::body_motion).unwrap_or(identity); + let mut motion2 = rb2.map(Self::body_motion).unwrap_or(identity); if let Some(t) = frozen1 { motion1.freeze(t); @@ -129,8 +100,8 @@ impl TOIEntry { motion2.freeze(t); } - let motion_c1 = motion1.prepend(co_parent1.map(|p| p.pos_wrt_parent).unwrap_or(co_pos1.0)); - let motion_c2 = motion2.prepend(co_parent2.map(|p| p.pos_wrt_parent).unwrap_or(co_pos2.0)); + let motion_c1 = motion1.prepend(co1.parent.map(|p| p.pos_wrt_parent).unwrap_or(co1.pos.0)); + let motion_c2 = motion2.prepend(co2.parent.map(|p| p.pos_wrt_parent).unwrap_or(co2.pos.0)); // println!("start_time: {}", start_time); @@ -146,9 +117,9 @@ impl TOIEntry { let res_toi = query_dispatcher .nonlinear_time_of_impact( &motion_c1, - co_shape1.as_ref(), + co1.shape.as_ref(), &motion_c2, - co_shape2.as_ref(), + co2.shape.as_ref(), start_time, end_time, stop_at_penetration, @@ -160,31 +131,24 @@ impl TOIEntry { Some(Self::new( toi.toi, ch1, - co_parent1.map(|p| p.handle), + co1.parent.map(|p| p.handle), ch2, - co_parent2.map(|p| p.handle), + co2.parent.map(|p| p.handle), is_pseudo_intersection_test, 0, )) } - fn body_motion( - (poss, vels, mprops, ccd): ( - &RigidBodyPosition, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyCcd, - ), - ) -> NonlinearRigidMotion { - if ccd.ccd_active { + fn body_motion(rb: &RigidBody) -> NonlinearRigidMotion { + if rb.ccd.ccd_active { NonlinearRigidMotion::new( - poss.position, - mprops.local_mprops.local_com, - vels.linvel, - vels.angvel, + rb.pos.position, + rb.mprops.local_mprops.local_com, + rb.vels.linvel, + rb.vels.angvel, ) } else { - NonlinearRigidMotion::constant_position(poss.next_position) + NonlinearRigidMotion::constant_position(rb.pos.next_position) } } } diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index 0cb92e9..246b50e 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -47,12 +47,15 @@ impl IslandManager { while i < active_set.len() { let handle = active_set[i]; - if bodies.get(handle.0).is_none() { + if bodies.get(handle).is_none() { // This rigid-body no longer exists, so we need to remove it from the active set. active_set.swap_remove(i); if i < active_set.len() { - bodies.map_mut_internal(active_set[i].0, |rb_ids| rb_ids.active_set_id = i); + // Update the active_set_id for the body that has been swapped. + if let Some(swapped_rb) = bodies.get_mut_internal(active_set[i]) { + swapped_rb.ids.active_set_id = i; + } } } else { i += 1; @@ -92,15 +95,13 @@ impl IslandManager { // deleting a joint attached to an already-removed body) where we could be // attempting to wake-up a rigid-body that has already been deleted. if bodies.get(handle).map(|rb| rb.body_type()) == Some(RigidBodyType::Dynamic) { - bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| { - activation.wake_up(strong) - }); - bodies.map_mut_internal(handle.0, |ids: &mut RigidBodyIds| { - if self.active_dynamic_set.get(ids.active_set_id) != Some(&handle) { - ids.active_set_id = self.active_dynamic_set.len(); - self.active_dynamic_set.push(handle); - } - }); + let rb = bodies.index_mut_internal(handle); + rb.activation.wake_up(strong); + + if self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle) { + rb.ids.active_set_id = self.active_dynamic_set.len(); + self.active_dynamic_set.push(handle); + } } } @@ -162,25 +163,22 @@ impl IslandManager { let can_sleep = &mut self.can_sleep; let stack = &mut self.stack; - let vels: &RigidBodyVelocity = bodies.index(h.0); - let sq_linvel = vels.linvel.norm_squared(); - let sq_angvel = vels.angvel.gdot(vels.angvel); - - bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| { - update_energy(activation, sq_linvel, sq_angvel, dt); - - if activation.time_since_can_sleep - >= RigidBodyActivation::default_time_until_sleep() - { - // Mark them as sleeping for now. This will - // be set to false during the graph traversal - // if it should not be put to sleep. - activation.sleeping = true; - can_sleep.push(h); - } else { - stack.push(h); - } - }); + let rb = bodies.index_mut_internal(h); + let sq_linvel = rb.vels.linvel.norm_squared(); + let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel); + + update_energy(&mut rb.activation, sq_linvel, sq_angvel, dt); + + if rb.activation.time_since_can_sleep >= RigidBodyActivation::default_time_until_sleep() + { + // Mark them as sleeping for now. This will + // be set to false during the graph traversal + // if it should not be put to sleep. + rb.activation.sleeping = true; + can_sleep.push(h); + } else { + stack.push(h); + } } // Read all the contacts and push objects touching touching this rigid-body. @@ -199,7 +197,7 @@ impl IslandManager { (inter.collider1, inter.collider2), *collider_handle, ); - if let Some(other_body) = colliders.get(other.0) { + if let Some(other_body) = colliders[other].parent { stack.push(other_body.handle); } break; @@ -212,15 +210,15 @@ impl IslandManager { // Now iterate on all active kinematic bodies and push all the bodies // touching them to the stack so they can be woken up. for h in self.active_kinematic_set.iter() { - let (vels, rb_colliders): (&RigidBodyVelocity, _) = bodies.index_bundle(h.0); + let rb = &bodies[*h]; - if vels.is_zero() { + if rb.vels.is_zero() { // If the kinematic body does not move, it does not have // to wake up any dynamic body. continue; } - push_contacting_bodies(rb_colliders, colliders, narrow_phase, &mut self.stack); + push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack); } // println!("Selection: {}", instant::now() - t); @@ -235,13 +233,9 @@ impl IslandManager { let mut island_marker = self.stack.len().max(1) - 1; while let Some(handle) = self.stack.pop() { - let (rb_status, rb_ids, rb_colliders): ( - &RigidBodyType, - &RigidBodyIds, - &RigidBodyColliders, - ) = bodies.index_bundle(handle.0); + let rb = bodies.index_mut_internal(handle); - if rb_ids.active_set_timestamp == self.active_set_timestamp || !rb_status.is_dynamic() { + if rb.ids.active_set_timestamp == self.active_set_timestamp || !rb.is_dynamic() { // We already visited this body and its neighbors. // Also, we don't propagate awake state through fixed bodies. continue; @@ -260,7 +254,7 @@ impl IslandManager { // Transmit the active state to all the rigid-bodies with colliders // in contact or joined with this collider. - push_contacting_bodies(rb_colliders, colliders, narrow_phase, &mut self.stack); + push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack); for inter in impulse_joints.joints_with(handle) { let other = crate::utils::select_other((inter.0, inter.1), handle); @@ -271,16 +265,12 @@ impl IslandManager { self.stack.push(other); } - bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| { - activation.wake_up(false); - }); - bodies.map_mut_internal(handle.0, |ids: &mut RigidBodyIds| { - ids.active_island_id = self.active_islands.len() - 1; - ids.active_set_id = self.active_dynamic_set.len(); - ids.active_set_offset = - ids.active_set_id - self.active_islands[ids.active_island_id]; - ids.active_set_timestamp = self.active_set_timestamp; - }); + rb.activation.wake_up(false); + rb.ids.active_island_id = self.active_islands.len() - 1; + rb.ids.active_set_id = self.active_dynamic_set.len(); + rb.ids.active_set_offset = + rb.ids.active_set_id - self.active_islands[rb.ids.active_island_id]; + rb.ids.active_set_timestamp = self.active_set_timestamp; self.active_dynamic_set.push(handle); } @@ -293,13 +283,11 @@ impl IslandManager { // ); // Actually put to sleep bodies which have not been detected as awake. - for h in &self.can_sleep { - let activation: &RigidBodyActivation = bodies.index(h.0); - if activation.sleeping { - bodies.set_internal(h.0, RigidBodyVelocity::zero()); - bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| { - activation.sleep() - }); + for handle in &self.can_sleep { + let rb = bodies.index_mut_internal(*handle); + if rb.activation.sleeping { + rb.vels = RigidBodyVelocity::zero(); + rb.activation.sleep(); } } } diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 6b6d980..1cd177d 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -3,10 +3,7 @@ use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractio use crate::data::arena::Arena; use crate::data::Coarena; -use crate::dynamics::{ - GenericJoint, IslandManager, RigidBodyActivation, RigidBodyHandle, RigidBodyIds, RigidBodySet, - RigidBodyType, -}; +use crate::dynamics::{GenericJoint, IslandManager, RigidBodyHandle, RigidBodySet}; /// The unique identifier of a joint added to the joint set. /// The unique identifier of a collider added to a collider set. @@ -230,26 +227,17 @@ impl ImpulseJointSet { // FIXME: don't iterate through all the interactions. for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() { let joint = &edge.weight; + let rb1 = &bodies[joint.body1]; + let rb2 = &bodies[joint.body2]; - let (status1, activation1, ids1): ( - &RigidBodyType, - &RigidBodyActivation, - &RigidBodyIds, - ) = bodies.index_bundle(joint.body1.0); - let (status2, activation2, ids2): ( - &RigidBodyType, - &RigidBodyActivation, - &RigidBodyIds, - ) = bodies.index_bundle(joint.body2.0); - - if (status1.is_dynamic() || status2.is_dynamic()) - && (!status1.is_dynamic() || !activation1.sleeping) - && (!status2.is_dynamic() || !activation2.sleeping) + if (rb1.is_dynamic() || rb2.is_dynamic()) + && (!rb1.is_dynamic() || !rb1.is_sleeping()) + && (!rb2.is_dynamic() || !rb2.is_sleeping()) { - let island_index = if !status1.is_dynamic() { - ids2.active_island_id + let island_index = if !rb1.is_dynamic() { + rb2.ids.active_island_id } else { - ids1.active_island_id + rb1.ids.active_island_id }; out[island_index].push(i); diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 5bd790a..2eb90eb 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -1,8 +1,8 @@ use super::multibody_link::{MultibodyLink, MultibodyLinkVec}; use super::multibody_workspace::MultibodyWorkspace; use crate::dynamics::{ - solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyForces, RigidBodyHandle, - RigidBodyMassProps, RigidBodySet, RigidBodyType, RigidBodyVelocity, + solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyHandle, RigidBodySet, + RigidBodyType, RigidBodyVelocity, }; #[cfg(feature = "dim3")] use crate::math::Matrix; @@ -376,36 +376,32 @@ impl Multibody { for i in 0..self.links.len() { let link = &self.links[i]; - - let (rb_vels, rb_mprops, rb_forces): ( - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyForces, - ) = bodies.index_bundle(link.rigid_body.0); + let rb = &bodies[link.rigid_body]; let mut acc = RigidBodyVelocity::zero(); if i != 0 { let parent_id = link.parent_internal_id; let parent_link = &self.links[parent_id]; - let parent_rb_vels: &RigidBodyVelocity = bodies.index(parent_link.rigid_body.0); + let parent_rb = &bodies[parent_link.rigid_body]; acc += self.workspace.accs[parent_id]; // The 2.0 originates from the two identical terms of Jdot (the terms become // identical once they are multiplied by the generalized velocities). - acc.linvel += 2.0 * parent_rb_vels.angvel.gcross(link.joint_velocity.linvel); + acc.linvel += 2.0 * parent_rb.vels.angvel.gcross(link.joint_velocity.linvel); #[cfg(feature = "dim3")] { - acc.angvel += parent_rb_vels.angvel.cross(&link.joint_velocity.angvel); + acc.angvel += parent_rb.vels.angvel.cross(&link.joint_velocity.angvel); } - acc.linvel += parent_rb_vels + acc.linvel += parent_rb + .vels .angvel - .gcross(parent_rb_vels.angvel.gcross(link.shift02)); + .gcross(parent_rb.vels.angvel.gcross(link.shift02)); acc.linvel += self.workspace.accs[parent_id].angvel.gcross(link.shift02); } - acc.linvel += rb_vels.angvel.gcross(rb_vels.angvel.gcross(link.shift23)); + acc.linvel += rb.vels.angvel.gcross(rb.vels.angvel.gcross(link.shift23)); acc.linvel += self.workspace.accs[i].angvel.gcross(link.shift23); self.workspace.accs[i] = acc; @@ -413,12 +409,12 @@ impl Multibody { // TODO: should gyroscopic forces already be computed by the rigid-body itself // (at the same time that we add the gravity force)? let gyroscopic; - let rb_inertia = rb_mprops.effective_angular_inertia(); - let rb_mass = rb_mprops.effective_mass(); + let rb_inertia = rb.mprops.effective_angular_inertia(); + let rb_mass = rb.mprops.effective_mass(); #[cfg(feature = "dim3")] { - gyroscopic = rb_vels.angvel.cross(&(rb_inertia * rb_vels.angvel)); + gyroscopic = rb.vels.angvel.cross(&(rb_inertia * rb.vels.angvel)); } #[cfg(feature = "dim2")] { @@ -426,8 +422,8 @@ impl Multibody { } let external_forces = Force::new( - rb_forces.force - rb_mass.component_mul(&acc.linvel), - rb_forces.torque - gyroscopic - rb_inertia * acc.angvel, + rb.forces.force - rb_mass.component_mul(&acc.linvel), + rb.forces.torque - gyroscopic - rb_inertia * acc.angvel, ); self.accelerations.gemv_tr( 1.0, @@ -456,13 +452,12 @@ impl Multibody { .jacobian_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]); link.joint_velocity = joint_velocity; - bodies.set_internal(link.rigid_body.0, link.joint_velocity); + bodies.index_mut_internal(link.rigid_body).vels = link.joint_velocity; for i in 1..self.links.len() { let (link, parent_link) = self.links.get_mut_with_parent(i); - let rb_mprops: &RigidBodyMassProps = bodies.index(link.rigid_body.0); - let (parent_rb_vels, parent_rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(parent_link.rigid_body.0); + let rb = &bodies[link.rigid_body]; + let parent_rb = &bodies[parent_link.rigid_body]; let joint_velocity = link .joint @@ -470,12 +465,12 @@ impl Multibody { link.joint_velocity = joint_velocity.transformed( &(parent_link.local_to_world.rotation * link.joint.data.local_frame1.rotation), ); - let mut new_rb_vels = *parent_rb_vels + link.joint_velocity; - let shift = rb_mprops.world_com - parent_rb_mprops.world_com; - new_rb_vels.linvel += parent_rb_vels.angvel.gcross(shift); + let mut new_rb_vels = parent_rb.vels + link.joint_velocity; + let shift = rb.mprops.world_com - parent_rb.mprops.world_com; + new_rb_vels.linvel += parent_rb.vels.angvel.gcross(shift); new_rb_vels.linvel += link.joint_velocity.angvel.gcross(link.shift23); - bodies.set_internal(link.rigid_body.0, new_rb_vels); + bodies.index_mut_internal(link.rigid_body).vels = new_rb_vels; } /* @@ -563,10 +558,9 @@ impl Multibody { for i in 0..self.links.len() { let link = &self.links[i]; - let (rb_vels, rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(link.rigid_body.0); - let rb_mass = rb_mprops.effective_mass(); - let rb_inertia = rb_mprops.effective_angular_inertia().into_matrix(); + let rb = &bodies[link.rigid_body]; + let rb_mass = rb.mprops.effective_mass(); + let rb_inertia = rb.mprops.effective_angular_inertia().into_matrix(); let body_jacobian = &self.body_jacobians[i]; @@ -576,8 +570,8 @@ impl Multibody { #[cfg(feature = "dim3")] { // Derivative of gyroscopic forces. - let gyroscopic_matrix = rb_vels.angvel.gcross_matrix() * rb_inertia - - (rb_inertia * rb_vels.angvel).gcross_matrix(); + let gyroscopic_matrix = rb.vels.angvel.gcross_matrix() * rb_inertia + - (rb_inertia * rb.vels.angvel).gcross_matrix(); augmented_inertia += gyroscopic_matrix * dt; } @@ -604,10 +598,10 @@ impl Multibody { if i != 0 { let parent_id = link.parent_internal_id; let parent_link = &self.links[parent_id]; - let parent_rb_vels: &RigidBodyVelocity = bodies.index(parent_link.rigid_body.0); + let parent_rb = &bodies[parent_link.rigid_body]; let parent_j = &self.body_jacobians[parent_id]; let parent_j_w = parent_j.fixed_rows::(DIM); - let parent_w = parent_rb_vels.angvel.gcross_matrix(); + let parent_w = parent_rb.vels.angvel.gcross_matrix(); let (coriolis_v, parent_coriolis_v) = self.coriolis_v.index_mut2(i, parent_id); let (coriolis_w, parent_coriolis_w) = self.coriolis_w.index_mut2(i, parent_id); @@ -620,7 +614,7 @@ impl Multibody { coriolis_v.gemm(1.0, &shift_cross_tr, &parent_coriolis_w, 1.0); // JDot (but the 2.0 originates from the sum of two identical terms in JDot and JDot/u * gdot) - let dvel_cross = (rb_vels.angvel.gcross(link.shift02) + let dvel_cross = (rb.vels.angvel.gcross(link.shift02) + 2.0 * link.joint_velocity.linvel) .gcross_matrix_tr(); coriolis_v.gemm(1.0, &dvel_cross, &parent_j_w, 1.0); @@ -676,13 +670,13 @@ impl Multibody { coriolis_v.gemm(1.0, &shift_cross_tr, &coriolis_w, 1.0); // JDot - let dvel_cross = rb_vels.angvel.gcross(link.shift23).gcross_matrix_tr(); + let dvel_cross = rb.vels.angvel.gcross(link.shift23).gcross_matrix_tr(); coriolis_v.gemm(1.0, &dvel_cross, &rb_j_w, 1.0); // JDot/u * qdot coriolis_v.gemm( 1.0, - &(rb_vels.angvel.gcross_matrix() * shift_cross_tr), + &(rb.vels.angvel.gcross_matrix() * shift_cross_tr), &rb_j_w, 1.0, ); diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index c7655e0..b818ce2 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -231,6 +231,7 @@ pub struct RigidBodyMassProps { pub flags: LockedAxes, /// The local mass properties of the rigid-body. pub local_mprops: MassProperties, + /// Mass-properties of this rigid-bodies, added to the contributions of its attached colliders. pub additional_local_mprops: Option>, /// The world-space center of mass of the rigid-body. pub world_com: Point, @@ -307,11 +308,11 @@ impl RigidBodyMassProps { .unwrap_or_else(MassProperties::default); for handle in &attached_colliders.0 { - if let Some(co) = colliders.get(handle) { + if let Some(co) = colliders.get(*handle) { if let Some(co_parent) = co.parent { let to_add = co .mprops - .mass_properties(&**co.shape) + .mass_properties(&*co.shape) .transform_by(&co_parent.pos_wrt_parent); self.local_mprops += to_add; } @@ -895,21 +896,17 @@ impl RigidBodyColliders { ) { for handle in &self.0 { // NOTE: the ColliderParent component must exist if we enter this method. - let co_parent: &ColliderParent = colliders - .get(handle.0) - .expect("Could not find the ColliderParent component."); - let new_pos = parent_pos * co_parent.pos_wrt_parent; + let co = colliders.index_mut_internal(*handle); + let new_pos = parent_pos * co.parent.as_ref().unwrap().pos_wrt_parent; + + if !co.changes.contains(ColliderChanges::MODIFIED) { + modified_colliders.push(*handle); + } // Set the modification flag so we can benefit from the modification-tracking // when updating the narrow-phase/broad-phase afterwards. - colliders.map_mut_internal(handle.0, |co_changes: &mut ColliderChanges| { - if !co_changes.contains(ColliderChanges::MODIFIED) { - modified_colliders.push(*handle); - } - - *co_changes |= ColliderChanges::POSITION; - }); - colliders.set_internal(handle.0, ColliderPosition(new_pos)); + co.changes |= ColliderChanges::POSITION; + co.pos = ColliderPosition(new_pos); } } } diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index 06ba340..d110971 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -50,18 +50,18 @@ pub(crate) fn categorize_joints( ) { for joint_i in joint_indices { let joint = &impulse_joints[*joint_i].weight; - let status1 = bodies.index(joint.body1.0); - let status2 = bodies.index(joint.body2.0); + let rb1 = &bodies[joint.body1.0]; + let rb2 = &bodies[joint.body2.0]; if multibody_joints.rigid_body_link(joint.body1).is_some() || multibody_joints.rigid_body_link(joint.body2).is_some() { - if !status1.is_dynamic() || !status2.is_dynamic() { + if !rb1.is_dynamic() || !rb2.is_dynamic() { generic_ground_joints.push(*joint_i); } else { generic_nonground_joints.push(*joint_i); } - } else if !status1.is_dynamic() || !status2.is_dynamic() { + } else if !rb1.is_dynamic() || !rb2.is_dynamic() { ground_joints.push(*joint_i); } else { nonground_joints.push(*joint_i); diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs index 1be34bc..318727e 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -1,8 +1,5 @@ use crate::dynamics::solver::{GenericRhs, VelocityConstraint}; -use crate::dynamics::{ - IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet, - RigidBodyType, RigidBodyVelocity, -}; +use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS}; use crate::utils::{WAngularInertia, WCross, WDot}; @@ -42,18 +39,12 @@ impl GenericVelocityConstraint { let handle1 = manifold.data.rigid_body1.unwrap(); let handle2 = manifold.data.rigid_body2.unwrap(); - let (rb_ids1, rb_vels1, rb_mprops1, rb_type1): ( - &RigidBodyIds, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyType, - ) = bodies.index_bundle(handle1.0); - let (rb_ids2, rb_vels2, rb_mprops2, rb_type2): ( - &RigidBodyIds, - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyType, - ) = bodies.index_bundle(handle2.0); + + let rb1 = &bodies[handle1]; + let rb2 = &bodies[handle2]; + + let (vels1, mprops1, type1) = (&rb1.vels, &rb1.mprops, &rb1.body_type); + let (vels2, mprops2, type2) = (&rb2.vels, &rb2.mprops, &rb2.body_type); let multibody1 = multibodies .rigid_body_link(handle1) @@ -63,15 +54,15 @@ impl GenericVelocityConstraint { .map(|m| (&multibodies[m.multibody], m.id)); let mj_lambda1 = multibody1 .map(|mb| mb.0.solver_id) - .unwrap_or(if rb_type1.is_dynamic() { - rb_ids1.active_set_offset + .unwrap_or(if type1.is_dynamic() { + rb1.ids.active_set_offset } else { 0 }); let mj_lambda2 = multibody2 .map(|mb| mb.0.solver_id) - .unwrap_or(if rb_type2.is_dynamic() { - rb_ids2.active_set_offset + .unwrap_or(if type2.is_dynamic() { + rb2.ids.active_set_offset } else { 0 }); @@ -80,11 +71,8 @@ impl GenericVelocityConstraint { #[cfg(feature = "dim2")] let tangents1 = force_dir1.orthonormal_basis(); #[cfg(feature = "dim3")] - let tangents1 = super::compute_tangent_contact_directions( - &force_dir1, - &rb_vels1.linvel, - &rb_vels2.linvel, - ); + let tangents1 = + super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); let multibodies_ndof = multibody1.map(|m| m.0.ndofs()).unwrap_or(0) + multibody2.map(|m| m.0.ndofs()).unwrap_or(0); @@ -109,13 +97,13 @@ impl GenericVelocityConstraint { #[cfg(feature = "dim3")] tangent1: tangents1[0], elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], - im1: if rb_type1.is_dynamic() { - rb_mprops1.effective_inv_mass + im1: if type1.is_dynamic() { + mprops1.effective_inv_mass } else { na::zero() }, - im2: if rb_type2.is_dynamic() { - rb_mprops2.effective_inv_mass + im2: if type2.is_dynamic() { + mprops2.effective_inv_mass } else { na::zero() }, @@ -129,11 +117,11 @@ impl GenericVelocityConstraint { for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; - let dp1 = manifold_point.point - rb_mprops1.world_com; - let dp2 = manifold_point.point - rb_mprops2.world_com; + let dp1 = manifold_point.point - mprops1.world_com; + let dp2 = manifold_point.point - mprops2.world_com; - let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1); - let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2); + let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); + let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); constraint.limit = manifold_point.friction; constraint.manifold_contact_id[k] = manifold_point.contact_id; @@ -143,15 +131,15 @@ impl GenericVelocityConstraint { let torque_dir1 = dp1.gcross(force_dir1); let torque_dir2 = dp2.gcross(-force_dir1); - let gcross1 = if rb_type1.is_dynamic() { - rb_mprops1 + let gcross1 = if type1.is_dynamic() { + mprops1 .effective_world_inv_inertia_sqrt .transform_vector(torque_dir1) } else { na::zero() }; - let gcross2 = if rb_type2.is_dynamic() { - rb_mprops2 + let gcross2 = if type2.is_dynamic() { + mprops2 .effective_world_inv_inertia_sqrt .transform_vector(torque_dir2) } else { @@ -170,8 +158,8 @@ impl GenericVelocityConstraint { jacobians, ) .0 - } else if rb_type1.is_dynamic() { - force_dir1.dot(&rb_mprops1.effective_inv_mass.component_mul(&force_dir1)) + } else if type1.is_dynamic() { + force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1)) + gcross1.gdot(gcross1) } else { 0.0 @@ -189,8 +177,8 @@ impl GenericVelocityConstraint { jacobians, ) .0 - } else if rb_type2.is_dynamic() { - force_dir1.dot(&rb_mprops2.effective_inv_mass.component_mul(&force_dir1)) + } else if type2.is_dynamic() { + force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + gcross2.gdot(gcross2) } else { 0.0 @@ -224,8 +212,8 @@ impl GenericVelocityConstraint { for j in 0..DIM - 1 { let torque_dir1 = dp1.gcross(tangents1[j]); - let gcross1 = if rb_type1.is_dynamic() { - rb_mprops1 + let gcross1 = if type1.is_dynamic() { + mprops1 .effective_world_inv_inertia_sqrt .transform_vector(torque_dir1) } else { @@ -234,8 +222,8 @@ impl GenericVelocityConstraint { constraint.elements[k].tangent_part.gcross1[j] = gcross1; let torque_dir2 = dp2.gcross(-tangents1[j]); - let gcross2 = if rb_type2.is_dynamic() { - rb_mprops2 + let gcross2 = if type2.is_dynamic() { + mprops2 .effective