nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Finalize refactoring

Browse Source
This commit is contained in:
Sébastien Crozet
2022-04-20 12:29:57 +02:00
committed by Sébastien Crozet
parent 2b1374c596
commit f108520b5a
32 changed files with 707 additions and 1030 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
Cargo.toml
View File

@@ -8,18 +8,18 @@ resolver = "2"
#simba = { path = "../simba" } #simba = { path = "../simba" }
#kiss3d = { path = "../kiss3d" } #kiss3d = { path = "../kiss3d" }
parry2d = { path = "../parry/crates/parry2d" } #parry2d = { path = "../parry/crates/parry2d" }
parry3d = { path = "../parry/crates/parry3d" } #parry3d = { path = "../parry/crates/parry3d" }
parry2d-f64 = { path = "../parry/crates/parry2d-f64" } #parry2d-f64 = { path = "../parry/crates/parry2d-f64" }
parry3d-f64 = { path = "../parry/crates/parry3d-f64" } #parry3d-f64 = { path = "../parry/crates/parry3d-f64" }
# nalgebra = { path = "../nalgebra" } # nalgebra = { path = "../nalgebra" }
#kiss3d = { git = "https://github.com/sebcrozet/kiss3d" } #kiss3d = { git = "https://github.com/sebcrozet/kiss3d" }
#nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" } #nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" }
#parry2d = { git = "https://github.com/dimforge/parry", branch = "special_cases" } parry2d = { git = "https://github.com/dimforge/parry", branch = "split-and-qbvh" }
#parry3d = { git = "https://github.com/dimforge/parry", branch = "special_cases" } parry3d = { git = "https://github.com/dimforge/parry", branch = "split-and-qbvh" }
#parry2d-f64 = { git = "https://github.com/dimforge/parry", branch = "special_cases" } parry2d-f64 = { git = "https://github.com/dimforge/parry", branch = "split-and-qbvh" }
#parry3d-f64 = { git = "https://github.com/dimforge/parry", branch = "special_cases" } parry3d-f64 = { git = "https://github.com/dimforge/parry", branch = "split-and-qbvh" }
[profile.release] [profile.release]
#debug = true #debug = true

7
examples2d/one_way_platforms2.rs
View File

@@ -6,11 +6,8 @@ struct OneWayPlatformHook {
platform2: ColliderHandle, platform2: ColliderHandle,
} }
impl PhysicsHooks<RigidBodySet, ColliderSet> for OneWayPlatformHook { impl PhysicsHooks for OneWayPlatformHook {
fn modify_solver_contacts( fn modify_solver_contacts(&self, context: &mut ContactModificationContext) {
&self,
context: &mut ContactModificationContext<RigidBodySet, ColliderSet>,
) {
// The allowed normal for the first platform is its local +y axis, and the // The allowed normal for the first platform is its local +y axis, and the
// allowed normal for the second platform is its local -y axis. // allowed normal for the second platform is its local -y axis.
// //

7
examples3d/one_way_platforms3.rs
View File

@@ -6,11 +6,8 @@ struct OneWayPlatformHook {
platform2: ColliderHandle, platform2: ColliderHandle,
} }
impl PhysicsHooks<RigidBodySet, ColliderSet> for OneWayPlatformHook { impl PhysicsHooks for OneWayPlatformHook {
fn modify_solver_contacts( fn modify_solver_contacts(&self, context: &mut ContactModificationContext) {
&self,
context: &mut ContactModificationContext<RigidBodySet, ColliderSet>,
) {
// The allowed normal for the first platform is its local +y axis, and the // The allowed normal for the first platform is its local +y axis, and the
// allowed normal for the second platform is its local -y axis. // allowed normal for the second platform is its local -y axis.
// //

297
src/dynamics/ccd/ccd_solver.rs
View File

@@ -1,16 +1,10 @@
use super::TOIEntry; use super::TOIEntry;
use crate::dynamics::{ use crate::dynamics::{IslandManager, RigidBodyHandle, RigidBodySet};
IslandManager, RigidBodyCcd, RigidBodyColliders, RigidBodyForces, RigidBodyHandle, use crate::geometry::{ColliderParent, ColliderSet, CollisionEvent, NarrowPhase};
RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{
ColliderParent, ColliderPosition, ColliderSet, ColliderShape, ColliderType, CollisionEvent,
NarrowPhase,
};
use crate::math::Real; use crate::math::Real;
use crate::parry::utils::SortedPair; use crate::parry::utils::SortedPair;
use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode}; use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode};
use crate::prelude::{ActiveEvents, ColliderFlags}; use crate::prelude::ActiveEvents;
use parry::query::{DefaultQueryDispatcher, QueryDispatcher}; use parry::query::{DefaultQueryDispatcher, QueryDispatcher};
use parry::utils::hashmap::HashMap; use parry::utils::hashmap::HashMap;
use std::collections::BinaryHeap; use std::collections::BinaryHeap;
@@ -61,23 +55,18 @@ impl CCDSolver {
match impacts { match impacts {
PredictedImpacts::Impacts(tois) => { PredictedImpacts::Impacts(tois) => {
for (handle, toi) in tois { for (handle, toi) in tois {
let (rb_poss, vels, ccd, mprops): ( let rb = bodies.index_mut_internal(*handle);
&RigidBodyPosition, let local_com = &rb.mprops.local_mprops.local_com;
&RigidBodyVelocity,
&RigidBodyCcd,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let local_com = &mprops.local_mprops.local_com;
let min_toi = (ccd.ccd_thickness let min_toi = (rb.ccd.ccd_thickness
* 0.15 * 0.15
* crate::utils::inv(ccd.max_point_velocity(vels))) * crate::utils::inv(rb.ccd.max_point_velocity(&rb.vels)))
.min(dt); .min(dt);
// println!("Min toi: {}, Toi: {}", min_toi, toi); // println!("Min toi: {}, Toi: {}", min_toi, toi);
let new_pos = vels.integrate(toi.max(min_toi), &rb_poss.position, &local_com); let new_pos = rb
bodies.map_mut_internal(handle.0, |rb_poss| { .vels
rb_poss.next_position = new_pos; .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"); // println!("Checking CCD activation");
for handle in islands.active_dynamic_bodies() { for handle in islands.active_dynamic_bodies() {
let (ccd, vels, forces): (&RigidBodyCcd, &RigidBodyVelocity, &RigidBodyForces) = let rb = bodies.index_mut_internal(*handle);
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;
});
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; ccd_active = ccd_active || moving_fast;
} }
} }
@@ -137,36 +125,31 @@ impl CCDSolver {
let mut min_toi = dt; let mut min_toi = dt;
for handle in islands.active_dynamic_bodies() { for handle in islands.active_dynamic_bodies() {
let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0); let rb1 = &bodies[*handle];
if rb_ccd1.ccd_active { if rb1.ccd.ccd_active {
let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): ( let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
&RigidBodyPosition, dt,
&RigidBodyVelocity, &rb1.forces,
&RigidBodyForces, &rb1.vels,
&RigidBodyMassProps, &rb1.mprops,
&RigidBodyColliders, );
) = bodies.index_bundle(handle.0);
let predicted_body_pos1 = for ch1 in &rb1.colliders.0 {
rb_pos1.integrate_forces_and_velocities(dt, forces1, rb_vels1, rb_mprops1); let co1 = &colliders[*ch1];
let co1_parent = co1
for ch1 in &rb_colliders1.0 { .parent
let co_parent1: &ColliderParent = colliders .as_ref()
.get(ch1.0)
.expect("Could not find the ColliderParent component."); .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. continue; // Ignore sensors.
} }
let predicted_collider_pos1 = predicted_body_pos1 * co_parent1.pos_wrt_parent; let predicted_collider_pos1 = predicted_body_pos1 * co1_parent.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 self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| {
@@ -182,21 +165,17 @@ impl CCDSolver {
) )
.is_none() .is_none()
{ {
let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); let co1 = &colliders[*ch1];
let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); let co2 = &colliders[*ch2];
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 bh1 = co_parent1.map(|p| p.handle); let bh1 = co1.parent.map(|p| p.handle);
let bh2 = co_parent2.map(|p| p.handle); let bh2 = co2.parent.map(|p| p.handle);
// Ignore self-intersection and sensors and apply collision groups filter. // Ignore self-intersection and sensors and apply collision groups filter.
if bh1 == bh2 // Ignore self-intersection. if bh1 == bh2 // Ignore self-intersection.
|| (co_type1.is_sensor() || co_type2.is_sensor()) // Ignore sensors. || (co1.is_sensor() || co2.is_sensor()) // Ignore sensors.
|| !c1.3.collision_groups.test(c2.3.collision_groups) // Apply collision groups. || !co1.flags.collision_groups.test(co2.flags.collision_groups) // Apply collision groups.
|| !c1.3.solver_groups.test(c2.3.solver_groups) || !co1.flags.solver_groups.test(co2.flags.solver_groups)
// Apply solver groups. // Apply solver groups.
{ {
return true; return true;
@@ -208,16 +187,16 @@ impl CCDSolver {
.map(|c| c.1.dist) .map(|c| c.1.dist)
.unwrap_or(0.0); .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( if let Some(toi) = TOIEntry::try_from_colliders(
self.query_pipeline.query_dispatcher(), self.query_pipeline.query_dispatcher(),
*ch1, *ch1,
*ch2, *ch2,
(c1.0, c1.1, c1.2, c1.3, co_parent1), co1,
(c2.0, c2.1, c2.2, c2.3, co_parent2), co2,
Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)), Some(rb1),
b2, rb2,
None, None,
None, None,
0.0, 0.0,
@@ -271,29 +250,27 @@ impl CCDSolver {
*/ */
// TODO: don't iterate through all the colliders. // TODO: don't iterate through all the colliders.
for handle in islands.active_dynamic_bodies() { for handle in islands.active_dynamic_bodies() {
let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0); let rb1 = &bodies[*handle];
if rb_ccd1.ccd_active { if rb1.ccd.ccd_active {
let (rb_pos1, rb_vels1, forces1, rb_mprops1, rb_colliders1): ( let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
&RigidBodyPosition, dt,
&RigidBodyVelocity, &rb1.forces,
&RigidBodyForces, &rb1.vels,
&RigidBodyMassProps, &rb1.mprops,
&RigidBodyColliders, );
) = bodies.index_bundle(handle.0);
let predicted_body_pos1 = for ch1 in &rb1.colliders.0 {
rb_pos1.integrate_forces_and_velocities(dt, forces1, rb_vels1, rb_mprops1); let co1 = &colliders[*ch1];
let co_parent1 = co1
for ch1 in &rb_colliders1.0 { .parent
let co_parent1: &ColliderParent = colliders .as_ref()
.get(ch1.0)
.expect("Could not find the ColliderParent component."); .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 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 self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| { .colliders_with_aabb_intersecting_aabb(&aabb1, |ch2| {
@@ -309,16 +286,15 @@ impl CCDSolver {
) )
.is_none() .is_none()
{ {
let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); let co1 = &colliders[*ch1];
let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); let co2 = &colliders[*ch2];
let c1: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch1.0);
let c2: (_, _, _, &ColliderFlags) = colliders.index_bundle(ch2.0);
let bh1 = co_parent1.map(|p| p.handle); let bh1 = co1.parent.map(|p| p.handle);
let bh2 = co_parent2.map(|p| p.handle); let bh2 = co2.parent.map(|p| p.handle);
// Ignore self-intersections and apply groups filter. // 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; return true;
} }
@@ -329,17 +305,17 @@ impl CCDSolver {
.map(|c| c.1.dist) .map(|c| c.1.dist)
.unwrap_or(0.0); .unwrap_or(0.0);
let b1 = bh1.map(|h| bodies.index_bundle(h.0)); let rb1 = bh1.map(|h| &bodies[h]);
let b2 = bh2.map(|h| bodies.index_bundle(h.0)); let rb2 = bh2.map(|h| &bodies[h]);
if let Some(toi) = TOIEntry::try_from_colliders( if let Some(toi) = TOIEntry::try_from_colliders(
self.query_pipeline.query_dispatcher(), self.query_pipeline.query_dispatcher(),
*ch1, *ch1,
*ch2, *ch2,
(c1.0, c1.1, c1.2, c1.3, co_parent1), co1,
(c2.0, c2.1, c2.2, c2.3, co_parent2), co2,
b1, rb1,
b2, rb2,
None, None,
None, None,
0.0, 0.0,
@@ -381,17 +357,15 @@ impl CCDSolver {
while let Some(toi) = all_toi.pop() { while let Some(toi) = all_toi.pop() {
assert!(toi.toi <= dt); assert!(toi.toi <= dt);
let rb1: Option<(&RigidBodyCcd, &RigidBodyColliders)> = let rb1 = toi.b1.and_then(|b| bodies.get(b));
toi.b1.map(|b| bodies.index_bundle(b.0)); let rb2 = toi.b2.and_then(|b| bodies.get(b));
let rb2: Option<(&RigidBodyCcd, &RigidBodyColliders)> =
toi.b2.map(|b| bodies.index_bundle(b.0));
let mut colliders_to_check = Vec::new(); let mut colliders_to_check = Vec::new();
let should_freeze1 = rb1.is_some() let should_freeze1 = rb1.is_some()
&& rb1.unwrap().0.ccd_active && rb1.unwrap().ccd.ccd_active
&& !frozen.contains_key(&toi.b1.unwrap()); && !frozen.contains_key(&toi.b1.unwrap());
let should_freeze2 = rb2.is_some() let should_freeze2 = rb2.is_some()
&& rb2.unwrap().0.ccd_active && rb2.unwrap().ccd.ccd_active
&& !frozen.contains_key(&toi.b2.unwrap()); && !frozen.contains_key(&toi.b2.unwrap());
if !should_freeze1 && !should_freeze2 { if !should_freeze1 && !should_freeze2 {
@@ -413,12 +387,12 @@ impl CCDSolver {
if should_freeze1 { if should_freeze1 {
let _ = frozen.insert(toi.b1.unwrap(), toi.toi); 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 { if should_freeze2 {
let _ = frozen.insert(toi.b2.unwrap(), toi.toi); 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; 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 // NOTE: the 1 and 2 indices (e.g., `ch1`, `ch2`) bellow are unrelated to the
// ones we used above. // ones we used above.
for ch1 in &colliders_to_check { for ch1 in &colliders_to_check {
let co_parent1: &ColliderParent = colliders.get(ch1.0).unwrap(); let co1 = &colliders[*ch1];
let (co_shape1, co_pos1): (&ColliderShape, &ColliderPosition) = let co1_parent = co1.parent.as_ref().unwrap();
colliders.index_bundle(ch1.0); let rb1 = &bodies[co1_parent.handle];
let rb_pos1: &RigidBodyPosition = bodies.index(co_parent1.handle.0); let co_next_pos1 = rb1.pos.next_position * co1_parent.pos_wrt_parent;
let co_next_pos1 = rb_pos1.next_position * co_parent1.pos_wrt_parent; let aabb = co1.shape.compute_swept_aabb(&co1.pos, &co_next_pos1);
let aabb = co_shape1.compute_swept_aabb(&co_pos1, &co_next_pos1);
self.query_pipeline self.query_pipeline
.colliders_with_aabb_intersecting_aabb(&aabb, |ch2| { .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| {
let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); let co2 = &colliders[*ch2];
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 bh1 = co_parent1.map(|p| p.handle); let bh1 = co1.parent.map(|p| p.handle);
let bh2 = co_parent2.map(|p| p.handle); let bh2 = co2.parent.map(|p| p.handle);
// Ignore self-intersection and apply groups filter. // 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; return true;
} }
let frozen1 = bh1.and_then(|h| frozen.get(&h)); let frozen1 = bh1.and_then(|h| frozen.get(&h));
let frozen2 = bh2.and_then(|h| frozen.get(&h)); let frozen2 = bh2.and_then(|h| frozen.get(&h));
let b1: Option<(_, _, _, &RigidBodyCcd)> = let rb1 = bh1.and_then(|h| bodies.get(h));
bh1.map(|h| bodies.index_bundle(h.0)); let rb2 = bh2.and_then(|h| bodies.get(h));
let b2: Option<(_, _, _, &RigidBodyCcd)> =
bh2.map(|h| bodies.index_bundle(h.0));
if (frozen1.is_some() || !b1.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() || !b2.map(|b| b.3.ccd_active).unwrap_or(false)) && (frozen2.is_some()
|| !rb2.map(|b| b.ccd.ccd_active).unwrap_or(false))
{ {
// We already did a resweep. // We already did a resweep.
return true; return true;
@@ -474,10 +445,10 @@ impl CCDSolver {
self.query_pipeline.query_dispatcher(), self.query_pipeline.query_dispatcher(),
*ch1, *ch1,
*ch2, *ch2,
(c1.0, c1.1, c1.2, c1.3, co_parent1), co1,
(c2.0, c2.1, c2.2, c2.3, co_parent2), co2,
b1, rb1,
b2, rb2,
frozen1.copied(), frozen1.copied(),
frozen2.copied(), frozen2.copied(),
start_time, start_time,
@@ -500,20 +471,10 @@ impl CCDSolver {
// - If the intersection isn't active anymore, and it wasn't intersecting // - 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 // 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. // events because it will never be detected by the narrow-phase because of tunneling.
let (co_type1, co_pos1, co_shape1, co_flags1): ( let co1 = &colliders[toi.c1];
&ColliderType, let co2 = &colliders[toi.c2];
&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() { if !co1.is_sensor() && !co2.is_sensor() {
// TODO: this happens if we found a TOI between two non-sensor // TODO: this happens if we found a TOI between two non-sensor
// colliders with mismatching solver_flags. It is not clear // colliders with mismatching solver_flags. It is not clear
// what we should do in this case: we could report a // 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_next_pos1 = if let Some(b1) = toi.b1 {
let co_parent1: &ColliderParent = colliders.get(toi.c1.0).unwrap(); let co_parent1: &ColliderParent = co1.parent.as_ref().unwrap();
let (rb_pos1, rb_vels1, rb_mprops1): ( let rb1 = &bodies[b1];
&RigidBodyPosition, let local_com1 = &rb1.mprops.local_mprops.local_com;
&RigidBodyVelocity,
&RigidBodyMassProps,
) = bodies.index_bundle(b1.0);
let local_com1 = &rb_mprops1.local_mprops.local_com;
let frozen1 = frozen.get(&b1); let frozen1 = frozen.get(&b1);
let pos1 = frozen1 let pos1 = frozen1
.map(|t| rb_vels1.integrate(*t, &rb_pos1.position, local_com1)) .map(|t| rb1.vels.integrate(*t, &rb1.pos.position, local_com1))
.unwrap_or(rb_pos1.next_position); .unwrap_or(rb1.pos.next_position);
pos1 * co_parent1.pos_wrt_parent pos1 * co_parent1.pos_wrt_parent
} else { } else {
co_pos1.0 co1.pos.0
}; };
let co_next_pos2 = if let Some(b2) = toi.b2 { let co_next_pos2 = if let Some(b2) = toi.b2 {
let co_parent2: &ColliderParent = colliders.get(toi.c2.0).unwrap(); let co_parent2: &ColliderParent = co2.parent.as_ref().unwrap();
let (rb_pos2, rb_vels2, rb_mprops2): ( let rb2 = &bodies[b2];
&RigidBodyPosition, let local_com2 = &rb2.mprops.local_mprops.local_com;
&RigidBodyVelocity,
&RigidBodyMassProps,
) = bodies.index_bundle(b2.0);
let local_com2 = &rb_mprops2.local_mprops.local_com;
let frozen2 = frozen.get(&b2); let frozen2 = frozen.get(&b2);
let pos2 = frozen2 let pos2 = frozen2
.map(|t| rb_vels2.integrate(*t, &rb_pos2.position, local_com2)) .map(|t| rb2.vels.integrate(*t, &rb2.pos.position, local_com2))
.unwrap_or(rb_pos2.next_position); .unwrap_or(rb2.pos.next_position);
pos2 * co_parent2.pos_wrt_parent pos2 * co_parent2.pos_wrt_parent
} else { } 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 next_coll_pos12 = co_next_pos1.inv_mul(&co_next_pos2);
let query_dispatcher = self.query_pipeline.query_dispatcher(); let query_dispatcher = self.query_pipeline.query_dispatcher();
let intersect_before = 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); .unwrap_or(false);
let intersect_after = query_dispatcher 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); .unwrap_or(false);
if !intersect_before if !intersect_before
&& !intersect_after && !intersect_after
&& (co_flags1.active_events | co_flags2.active_events) && (co1.flags.active_events | co2.flags.active_events)
.contains(ActiveEvents::COLLISION_EVENTS) .contains(ActiveEvents::COLLISION_EVENTS)
{ {
// Emit one intersection-started and one intersection-stopped event. // Emit one intersection-started and one intersection-stopped event.

104
src/dynamics/ccd/toi_entry.rs
View File

@@ -1,9 +1,5 @@
use crate::dynamics::{ use crate::dynamics::{RigidBody, RigidBodyHandle};
RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, use crate::geometry::{Collider, ColliderHandle};
};
use crate::geometry::{
ColliderFlags, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType,
};
use crate::math::Real; use crate::math::Real;
use parry::query::{NonlinearRigidMotion, QueryDispatcher}; use parry::query::{NonlinearRigidMotion, QueryDispatcher};
@@ -45,32 +41,10 @@ impl TOIEntry {
query_dispatcher: &QD, query_dispatcher: &QD,
ch1: ColliderHandle, ch1: ColliderHandle,
ch2: ColliderHandle, ch2: ColliderHandle,
c1: ( co1: &Collider,
&ColliderType, co2: &Collider,
&ColliderShape, rb1: Option<&RigidBody>,
&ColliderPosition, rb2: Option<&RigidBody>,
&ColliderFlags,
Option<&ColliderParent>,
),
c2: (
&ColliderType,
&ColliderShape,
&ColliderPosition,
&ColliderFlags,
Option<&ColliderParent>,
),
b1: Option<(
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyCcd,
)>,
b2: Option<(
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyCcd,
)>,
frozen1: Option<Real>, frozen1: Option<Real>,
frozen2: Option<Real>, frozen2: Option<Real>,
start_time: Real, start_time: Real,
@@ -78,39 +52,36 @@ impl TOIEntry {
smallest_contact_dist: Real, smallest_contact_dist: Real,
) -> Option<Self> { ) -> Option<Self> {
assert!(start_time <= end_time); assert!(start_time <= end_time);
if b1.is_none() && b2.is_none() { if rb1.is_none() && rb2.is_none() {
return 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 = 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 = 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 = 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 = 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")] #[cfg(feature = "dim2")]
let vel12 = (linvel2 - linvel1).norm() let vel12 = (linvel2 - linvel1).norm()
+ angvel1.abs() * b1.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() * b2.map(|b| b.3.ccd_max_dist).unwrap_or(0.0); + angvel2.abs() * rb2.map(|b| b.ccd.ccd_max_dist).unwrap_or(0.0);
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let vel12 = (linvel2 - linvel1).norm() let vel12 = (linvel2 - linvel1).norm()
+ angvel1.norm() * b1.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() * b2.map(|b| b.3.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. // 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 // But removing the `max` doesn't really affect performances so let's
// keep it since more conservatism is good at this stage. // 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); + smallest_contact_dist.max(0.0);
let is_pseudo_intersection_test = co_type1.is_sensor() let is_pseudo_intersection_test = co1.is_sensor()
|| co_type2.is_sensor() || co2.is_sensor()
|| !co_flags1.solver_groups.test(co_flags2.solver_groups); || !co1.flags.solver_groups.test(co2.flags.solver_groups);
if (end_time - start_time) * vel12 < thickness { if (end_time - start_time) * vel12 < thickness {
return None; return None;
@@ -118,8 +89,8 @@ impl TOIEntry {
// Compute the TOI. // Compute the TOI.
let identity = NonlinearRigidMotion::identity(); let identity = NonlinearRigidMotion::identity();
let mut motion1 = b1.map(Self::body_motion).unwrap_or(identity); let mut motion1 = rb1.map(Self::body_motion).unwrap_or(identity);
let mut motion2 = b2.map(Self::body_motion).unwrap_or(identity); let mut motion2 = rb2.map(Self::body_motion).unwrap_or(identity);
if let Some(t) = frozen1 { if let Some(t) = frozen1 {
motion1.freeze(t); motion1.freeze(t);
@@ -129,8 +100,8 @@ impl TOIEntry {
motion2.freeze(t); motion2.freeze(t);
} }
let motion_c1 = motion1.prepend(co_parent1.map(|p| p.pos_wrt_parent).unwrap_or(co_pos1.0)); let motion_c1 = motion1.prepend(co1.parent.map(|p| p.pos_wrt_parent).unwrap_or(co1.pos.0));
let motion_c2 = motion2.prepend(co_parent2.map(|p| p.pos_wrt_parent).unwrap_or(co_pos2.0)); let motion_c2 = motion2.prepend(co2.parent.map(|p| p.pos_wrt_parent).unwrap_or(co2.pos.0));
// println!("start_time: {}", start_time); // println!("start_time: {}", start_time);
@@ -146,9 +117,9 @@ impl TOIEntry {
let res_toi = query_dispatcher let res_toi = query_dispatcher
.nonlinear_time_of_impact( .nonlinear_time_of_impact(
&motion_c1, &motion_c1,
co_shape1.as_ref(), co1.shape.as_ref(),
&motion_c2, &motion_c2,
co_shape2.as_ref(), co2.shape.as_ref(),
start_time, start_time,
end_time, end_time,
stop_at_penetration, stop_at_penetration,
@@ -160,31 +131,24 @@ impl TOIEntry {
Some(Self::new( Some(Self::new(
toi.toi, toi.toi,
ch1, ch1,
co_parent1.map(|p| p.handle), co1.parent.map(|p| p.handle),
ch2, ch2,
co_parent2.map(|p| p.handle), co2.parent.map(|p| p.handle),
is_pseudo_intersection_test, is_pseudo_intersection_test,
0, 0,
)) ))
} }
fn body_motion( fn body_motion(rb: &RigidBody) -> NonlinearRigidMotion {
(poss, vels, mprops, ccd): ( if rb.ccd.ccd_active {
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyCcd,
),
) -> NonlinearRigidMotion {
if ccd.ccd_active {
NonlinearRigidMotion::new( NonlinearRigidMotion::new(
poss.position, rb.pos.position,
mprops.local_mprops.local_com, rb.mprops.local_mprops.local_com,
vels.linvel, rb.vels.linvel,
vels.angvel, rb.vels.angvel,
) )
} else { } else {
NonlinearRigidMotion::constant_position(poss.next_position) NonlinearRigidMotion::constant_position(rb.pos.next_position)
} }
} }
} }

100
src/dynamics/island_manager.rs
View File

@@ -47,12 +47,15 @@ impl IslandManager {
while i < active_set.len() { while i < active_set.len() {
let handle = active_set[i]; 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. // This rigid-body no longer exists, so we need to remove it from the active set.
active_set.swap_remove(i); active_set.swap_remove(i);
if i < active_set.len() { 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 { } else {
i += 1; i += 1;
@@ -92,15 +95,13 @@ impl IslandManager {
// deleting a joint attached to an already-removed body) where we could be // 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. // attempting to wake-up a rigid-body that has already been deleted.
if bodies.get(handle).map(|rb| rb.body_type()) == Some(RigidBodyType::Dynamic) { if bodies.get(handle).map(|rb| rb.body_type()) == Some(RigidBodyType::Dynamic) {
bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| { let rb = bodies.index_mut_internal(handle);
activation.wake_up(strong) rb.activation.wake_up(strong);
});
bodies.map_mut_internal(handle.0, |ids: &mut RigidBodyIds| { if self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle) {
if self.active_dynamic_set.get(ids.active_set_id) != Some(&handle) { rb.ids.active_set_id = self.active_dynamic_set.len();
ids.active_set_id = self.active_dynamic_set.len(); self.active_dynamic_set.push(handle);
self.active_dynamic_set.push(handle); }
}
});
} }
} }
@@ -162,25 +163,22 @@ impl IslandManager {
let can_sleep = &mut self.can_sleep; let can_sleep = &mut self.can_sleep;
let stack = &mut self.stack; let stack = &mut self.stack;
let vels: &RigidBodyVelocity = bodies.index(h.0); let rb = bodies.index_mut_internal(h);
let sq_linvel = vels.linvel.norm_squared(); let sq_linvel = rb.vels.linvel.norm_squared();
let sq_angvel = vels.angvel.gdot(vels.angvel); let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel);
bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| { update_energy(&mut rb.activation, sq_linvel, sq_angvel, dt);
update_energy(activation, sq_linvel, sq_angvel, dt);
if activation.time_since_can_sleep if rb.activation.time_since_can_sleep >= RigidBodyActivation::default_time_until_sleep()
>= RigidBodyActivation::default_time_until_sleep() {
{ // Mark them as sleeping for now. This will
// Mark them as sleeping for now. This will // be set to false during the graph traversal
// be set to false during the graph traversal // if it should not be put to sleep.
// if it should not be put to sleep. rb.activation.sleeping = true;
activation.sleeping = true; can_sleep.push(h);
can_sleep.push(h); } else {
} else { stack.push(h);
stack.push(h); }
}
});
} }
// Read all the contacts and push objects touching touching this rigid-body. // Read all the contacts and push objects touching touching this rigid-body.
@@ -199,7 +197,7 @@ impl IslandManager {
(inter.collider1, inter.collider2), (inter.collider1, inter.collider2),
*collider_handle, *collider_handle,
); );
if let Some(other_body) = colliders.get(other.0) { if let Some(other_body) = colliders[other].parent {
stack.push(other_body.handle); stack.push(other_body.handle);
} }
break; break;
@@ -212,15 +210,15 @@ impl IslandManager {
// Now iterate on all active kinematic bodies and push all the bodies // Now iterate on all active kinematic bodies and push all the bodies
// touching them to the stack so they can be woken up. // touching them to the stack so they can be woken up.
for h in self.active_kinematic_set.iter() { 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 // If the kinematic body does not move, it does not have
// to wake up any dynamic body. // to wake up any dynamic body.
continue; 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); // println!("Selection: {}", instant::now() - t);
@@ -235,13 +233,9 @@ impl IslandManager {
let mut island_marker = self.stack.len().max(1) - 1; let mut island_marker = self.stack.len().max(1) - 1;
while let Some(handle) = self.stack.pop() { while let Some(handle) = self.stack.pop() {
let (rb_status, rb_ids, rb_colliders): ( let rb = bodies.index_mut_internal(handle);
&RigidBodyType,
&RigidBodyIds,
&RigidBodyColliders,
) = bodies.index_bundle(handle.0);
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. // We already visited this body and its neighbors.
// Also, we don't propagate awake state through fixed bodies. // Also, we don't propagate awake state through fixed bodies.
continue; continue;
@@ -260,7 +254,7 @@ impl IslandManager {
// Transmit the active state to all the rigid-bodies with colliders // Transmit the active state to all the rigid-bodies with colliders
// in contact or joined with this collider. // 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) { for inter in impulse_joints.joints_with(handle) {
let other = crate::utils::select_other((inter.0, inter.1), handle); let other = crate::utils::select_other((inter.0, inter.1), handle);
@@ -271,16 +265,12 @@ impl IslandManager {
self.stack.push(other); self.stack.push(other);
} }
bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| { rb.activation.wake_up(false);
activation.wake_up(false); rb.ids.active_island_id = self.active_islands.len() - 1;
}); rb.ids.active_set_id = self.active_dynamic_set.len();
bodies.map_mut_internal(handle.0, |ids: &mut RigidBodyIds| { rb.ids.active_set_offset =
ids.active_island_id = self.active_islands.len() - 1; rb.ids.active_set_id - self.active_islands[rb.ids.active_island_id];
ids.active_set_id = self.active_dynamic_set.len(); rb.ids.active_set_timestamp = self.active_set_timestamp;
ids.active_set_offset =
ids.active_set_id - self.active_islands[ids.active_island_id];
ids.active_set_timestamp = self.active_set_timestamp;
});
self.active_dynamic_set.push(handle); self.active_dynamic_set.push(handle);
} }
@@ -293,13 +283,11 @@ impl IslandManager {
// ); // );
// Actually put to sleep bodies which have not been detected as awake. // Actually put to sleep bodies which have not been detected as awake.
for h in &self.can_sleep { for handle in &self.can_sleep {
let activation: &RigidBodyActivation = bodies.index(h.0); let rb = bodies.index_mut_internal(*handle);
if activation.sleeping { if rb.activation.sleeping {
bodies.set_internal(h.0, RigidBodyVelocity::zero()); rb.vels = RigidBodyVelocity::zero();
bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| { rb.activation.sleep();
activation.sleep()
});
} }
} }
} }

30
src/dynamics/joint/impulse_joint/impulse_joint_set.rs
View File

@@ -3,10 +3,7 @@ use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractio
use crate::data::arena::Arena; use crate::data::arena::Arena;
use crate::data::Coarena; use crate::data::Coarena;
use crate::dynamics::{ use crate::dynamics::{GenericJoint, IslandManager, RigidBodyHandle, RigidBodySet};
GenericJoint, IslandManager, RigidBodyActivation, RigidBodyHandle, RigidBodyIds, RigidBodySet,
RigidBodyType,
};
/// The unique identifier of a joint added to the joint set. /// The unique identifier of a joint added to the joint set.
/// The unique identifier of a collider added to a collider 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. // FIXME: don't iterate through all the interactions.
for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() { for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() {
let joint = &edge.weight; let joint = &edge.weight;
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
let (status1, activation1, ids1): ( if (rb1.is_dynamic() || rb2.is_dynamic())
&RigidBodyType, && (!rb1.is_dynamic() || !rb1.is_sleeping())
&RigidBodyActivation, && (!rb2.is_dynamic() || !rb2.is_sleeping())
&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)
{ {
let island_index = if !status1.is_dynamic() { let island_index = if !rb1.is_dynamic() {
ids2.active_island_id rb2.ids.active_island_id
} else { } else {
ids1.active_island_id rb1.ids.active_island_id
}; };
out[island_index].push(i); out[island_index].push(i);

70
src/dynamics/joint/multibody_joint/multibody.rs
View File

@@ -1,8 +1,8 @@
use super::multibody_link::{MultibodyLink, MultibodyLinkVec}; use super::multibody_link::{MultibodyLink, MultibodyLinkVec};
use super::multibody_workspace::MultibodyWorkspace; use super::multibody_workspace::MultibodyWorkspace;
use crate::dynamics::{ use crate::dynamics::{
solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyForces, RigidBodyHandle, solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyHandle, RigidBodySet,
RigidBodyMassProps, RigidBodySet, RigidBodyType, RigidBodyVelocity, RigidBodyType, RigidBodyVelocity,
}; };
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
use crate::math::Matrix; use crate::math::Matrix;
@@ -376,36 +376,32 @@ impl Multibody {
for i in 0..self.links.len() { for i in 0..self.links.len() {
let link = &self.links[i]; let link = &self.links[i];
let rb = &bodies[link.rigid_body];
let (rb_vels, rb_mprops, rb_forces): (
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyForces,
) = bodies.index_bundle(link.rigid_body.0);
let mut acc = RigidBodyVelocity::zero(); let mut acc = RigidBodyVelocity::zero();
if i != 0 { if i != 0 {
let parent_id = link.parent_internal_id; let parent_id = link.parent_internal_id;
let parent_link = &self.links[parent_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]; acc += self.workspace.accs[parent_id];
// The 2.0 originates from the two identical terms of Jdot (the terms become // The 2.0 originates from the two identical terms of Jdot (the terms become
// identical once they are multiplied by the generalized velocities). // 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")] #[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 .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 += 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); acc.linvel += self.workspace.accs[i].angvel.gcross(link.shift23);
self.workspace.accs[i] = acc; self.workspace.accs[i] = acc;
@@ -413,12 +409,12 @@ impl Multibody {
// TODO: should gyroscopic forces already be computed by the rigid-body itself // TODO: should gyroscopic forces already be computed by the rigid-body itself
// (at the same time that we add the gravity force)? // (at the same time that we add the gravity force)?
let gyroscopic; let gyroscopic;
let rb_inertia = rb_mprops.effective_angular_inertia(); let rb_inertia = rb.mprops.effective_angular_inertia();
let rb_mass = rb_mprops.effective_mass(); let rb_mass = rb.mprops.effective_mass();
#[cfg(feature = "dim3")] #[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")] #[cfg(feature = "dim2")]
{ {
@@ -426,8 +422,8 @@ impl Multibody {
} }
let external_forces = Force::new( let external_forces = Force::new(
rb_forces.force - rb_mass.component_mul(&acc.linvel), rb.forces.force - rb_mass.component_mul(&acc.linvel),
rb_forces.torque - gyroscopic - rb_inertia * acc.angvel, rb.forces.torque - gyroscopic - rb_inertia * acc.angvel,
); );
self.accelerations.gemv_tr( self.accelerations.gemv_tr(
1.0, 1.0,
@@ -456,13 +452,12 @@ impl Multibody {
.jacobian_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]); .jacobian_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]);
link.joint_velocity = joint_velocity; 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() { for i in 1..self.links.len() {
let (link, parent_link) = self.links.get_mut_with_parent(i); let (link, parent_link) = self.links.get_mut_with_parent(i);
let rb_mprops: &RigidBodyMassProps = bodies.index(link.rigid_body.0); let rb = &bodies[link.rigid_body];
let (parent_rb_vels, parent_rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) = let parent_rb = &bodies[parent_link.rigid_body];
bodies.index_bundle(parent_link.rigid_body.0);
let joint_velocity = link let joint_velocity = link
.joint .joint
@@ -470,12 +465,12 @@ impl Multibody {
link.joint_velocity = joint_velocity.transformed( link.joint_velocity = joint_velocity.transformed(
&(parent_link.local_to_world.rotation * link.joint.data.local_frame1.rotation), &(parent_link.local_to_world.rotation * link.joint.data.local_frame1.rotation),
); );
let mut new_rb_vels = *parent_rb_vels + link.joint_velocity; let mut new_rb_vels = parent_rb.vels + link.joint_velocity;
let shift = rb_mprops.world_com - parent_rb_mprops.world_com; 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 += parent_rb.vels.angvel.gcross(shift);
new_rb_vels.linvel += link.joint_velocity.angvel.gcross(link.shift23); 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() { for i in 0..self.links.len() {
let link = &self.links[i]; let link = &self.links[i];
let (rb_vels, rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) = let rb = &bodies[link.rigid_body];
bodies.index_bundle(link.rigid_body.0); let rb_mass = rb.mprops.effective_mass();
let rb_mass = rb_mprops.effective_mass(); let rb_inertia = rb.mprops.effective_angular_inertia().into_matrix();
let rb_inertia = rb_mprops.effective_angular_inertia().into_matrix();
let body_jacobian = &self.body_jacobians[i]; let body_jacobian = &self.body_jacobians[i];
@@ -576,8 +570,8 @@ impl Multibody {
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
{ {
// Derivative of gyroscopic forces. // Derivative of gyroscopic forces.
let gyroscopic_matrix = rb_vels.angvel.gcross_matrix() * rb_inertia let gyroscopic_matrix = rb.vels.angvel.gcross_matrix() * rb_inertia
- (rb_inertia * rb_vels.angvel).gcross_matrix(); - (rb_inertia * rb.vels.angvel).gcross_matrix();
augmented_inertia += gyroscopic_matrix * dt; augmented_inertia += gyroscopic_matrix * dt;
} }
@@ -604,10 +598,10 @@ impl Multibody {
if i != 0 { if i != 0 {
let parent_id = link.parent_internal_id; let parent_id = link.parent_internal_id;
let parent_link = &self.links[parent_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 = &self.body_jacobians[parent_id];
let parent_j_w = parent_j.fixed_rows::<ANG_DIM>(DIM); let parent_j_w = parent_j.fixed_rows::<ANG_DIM>(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_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); 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); 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) // 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) + 2.0 * link.joint_velocity.linvel)
.gcross_matrix_tr(); .gcross_matrix_tr();
coriolis_v.gemm(1.0, &dvel_cross, &parent_j_w, 1.0); 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); coriolis_v.gemm(1.0, &shift_cross_tr, &coriolis_w, 1.0);
// JDot // 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); coriolis_v.gemm(1.0, &dvel_cross, &rb_j_w, 1.0);
// JDot/u * qdot // JDot/u * qdot
coriolis_v.gemm( coriolis_v.gemm(
1.0, 1.0,
&(rb_vels.angvel.gcross_matrix() * shift_cross_tr), &(rb.vels.angvel.gcross_matrix() * shift_cross_tr),
&rb_j_w, &rb_j_w,
1.0, 1.0,
); );

25
src/dynamics/rigid_body_components.rs
View File

@@ -231,6 +231,7 @@ pub struct RigidBodyMassProps {
pub flags: LockedAxes, pub flags: LockedAxes,
/// The local mass properties of the rigid-body. /// The local mass properties of the rigid-body.
pub local_mprops: MassProperties, pub local_mprops: MassProperties,
/// Mass-properties of this rigid-bodies, added to the contributions of its attached colliders.
pub additional_local_mprops: Option<Box<MassProperties>>, pub additional_local_mprops: Option<Box<MassProperties>>,
/// The world-space center of mass of the rigid-body. /// The world-space center of mass of the rigid-body.
pub world_com: Point<Real>, pub world_com: Point<Real>,
@@ -307,11 +308,11 @@ impl RigidBodyMassProps {
.unwrap_or_else(MassProperties::default); .unwrap_or_else(MassProperties::default);
for handle in &attached_colliders.0 { 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 { if let Some(co_parent) = co.parent {
let to_add = co let to_add = co
.mprops .mprops
.mass_properties(&**co.shape) .mass_properties(&*co.shape)
.transform_by(&co_parent.pos_wrt_parent); .transform_by(&co_parent.pos_wrt_parent);
self.local_mprops += to_add; self.local_mprops += to_add;
} }
@@ -895,21 +896,17 @@ impl RigidBodyColliders {
) { ) {
for handle in &self.0 { for handle in &self.0 {
// NOTE: the ColliderParent component must exist if we enter this method. // NOTE: the ColliderParent component must exist if we enter this method.
let co_parent: &ColliderParent = colliders let co = colliders.index_mut_internal(*handle);
.get(handle.0) let new_pos = parent_pos * co.parent.as_ref().unwrap().pos_wrt_parent;
.expect("Could not find the ColliderParent component.");
let new_pos = parent_pos * co_parent.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 // Set the modification flag so we can benefit from the modification-tracking
// when updating the narrow-phase/broad-phase afterwards. // when updating the narrow-phase/broad-phase afterwards.
colliders.map_mut_internal(handle.0, |co_changes: &mut ColliderChanges| { co.changes |= ColliderChanges::POSITION;
if !co_changes.contains(ColliderChanges::MODIFIED) { co.pos = ColliderPosition(new_pos);
modified_colliders.push(*handle);
}
*co_changes |= ColliderChanges::POSITION;
});
colliders.set_internal(handle.0, ColliderPosition(new_pos));
} }
} }
} }

8
src/dynamics/solver/categorization.rs
View File

@@ -50,18 +50,18 @@ pub(crate) fn categorize_joints(
) { ) {
for joint_i in joint_indices { for joint_i in joint_indices {
let joint = &impulse_joints[*joint_i].weight; let joint = &impulse_joints[*joint_i].weight;
let status1 = bodies.index(joint.body1.0); let rb1 = &bodies[joint.body1.0];
let status2 = bodies.index(joint.body2.0); let rb2 = &bodies[joint.body2.0];
if multibody_joints.rigid_body_link(joint.body1).is_some() if multibody_joints.rigid_body_link(joint.body1).is_some()
|| multibody_joints.rigid_body_link(joint.body2).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); generic_ground_joints.push(*joint_i);
} else { } else {
generic_nonground_joints.push(*joint_i); 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); ground_joints.push(*joint_i);
} else { } else {
nonground_joints.push(*joint_i); nonground_joints.push(*joint_i);

92
src/dynamics/solver/generic_velocity_constraint.rs
View File

@@ -1,8 +1,5 @@
use crate::dynamics::solver::{GenericRhs, VelocityConstraint}; use crate::dynamics::solver::{GenericRhs, VelocityConstraint};
use crate::dynamics::{ use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet,
RigidBodyType, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS}; use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{WAngularInertia, WCross, WDot}; use crate::utils::{WAngularInertia, WCross, WDot};
@@ -42,18 +39,12 @@ impl GenericVelocityConstraint {
let handle1 = manifold.data.rigid_body1.unwrap(); let handle1 = manifold.data.rigid_body1.unwrap();
let handle2 = manifold.data.rigid_body2.unwrap(); let handle2 = manifold.data.rigid_body2.unwrap();
let (rb_ids1, rb_vels1, rb_mprops1, rb_type1): (
&RigidBodyIds, let rb1 = &bodies[handle1];
&RigidBodyVelocity, let rb2 = &bodies[handle2];
&RigidBodyMassProps,
&RigidBodyType, let (vels1, mprops1, type1) = (&rb1.vels, &rb1.mprops, &rb1.body_type);
) = bodies.index_bundle(handle1.0); let (vels2, mprops2, type2) = (&rb2.vels, &rb2.mprops, &rb2.body_type);
let (rb_ids2, rb_vels2, rb_mprops2, rb_type2): (
&RigidBodyIds,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyType,
) = bodies.index_bundle(handle2.0);
let multibody1 = multibodies let multibody1 = multibodies
.rigid_body_link(handle1) .rigid_body_link(handle1)
@@ -63,15 +54,15 @@ impl GenericVelocityConstraint {
.map(|m| (&multibodies[m.multibody], m.id)); .map(|m| (&multibodies[m.multibody], m.id));
let mj_lambda1 = multibody1 let mj_lambda1 = multibody1
.map(|mb| mb.0.solver_id) .map(|mb| mb.0.solver_id)
.unwrap_or(if rb_type1.is_dynamic() { .unwrap_or(if type1.is_dynamic() {
rb_ids1.active_set_offset rb1.ids.active_set_offset
} else { } else {
0 0
}); });
let mj_lambda2 = multibody2 let mj_lambda2 = multibody2
.map(|mb| mb.0.solver_id) .map(|mb| mb.0.solver_id)
.unwrap_or(if rb_type2.is_dynamic() { .unwrap_or(if type2.is_dynamic() {
rb_ids2.active_set_offset rb2.ids.active_set_offset
} else { } else {
0 0
}); });
@@ -80,11 +71,8 @@ impl GenericVelocityConstraint {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis(); let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let tangents1 = super::compute_tangent_contact_directions( let tangents1 =
&force_dir1, super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
&rb_vels1.linvel,
&rb_vels2.linvel,
);
let multibodies_ndof = multibody1.map(|m| m.0.ndofs()).unwrap_or(0) let multibodies_ndof = multibody1.map(|m| m.0.ndofs()).unwrap_or(0)
+ multibody2.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")] #[cfg(feature = "dim3")]
tangent1: tangents1[0], tangent1: tangents1[0],
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1: if rb_type1.is_dynamic() { im1: if type1.is_dynamic() {
rb_mprops1.effective_inv_mass mprops1.effective_inv_mass
} else { } else {
na::zero() na::zero()
}, },
im2: if rb_type2.is_dynamic() { im2: if type2.is_dynamic() {
rb_mprops2.effective_inv_mass mprops2.effective_inv_mass
} else { } else {
na::zero() na::zero()
}, },
@@ -129,11 +117,11 @@ impl GenericVelocityConstraint {
for k in 0..manifold_points.len() { for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k]; let manifold_point = &manifold_points[k];
let dp1 = manifold_point.point - rb_mprops1.world_com; let dp1 = manifold_point.point - mprops1.world_com;
let dp2 = manifold_point.point - rb_mprops2.world_com; let dp2 = manifold_point.point - mprops2.world_com;
let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1); let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2); let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction; constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id; constraint.manifold_contact_id[k] = manifold_point.contact_id;
@@ -143,15 +131,15 @@ impl GenericVelocityConstraint {
let torque_dir1 = dp1.gcross(force_dir1); let torque_dir1 = dp1.gcross(force_dir1);
let torque_dir2 = dp2.gcross(-force_dir1); let torque_dir2 = dp2.gcross(-force_dir1);
let gcross1 = if rb_type1.is_dynamic() { let gcross1 = if type1.is_dynamic() {
rb_mprops1 mprops1
.effective_world_inv_inertia_sqrt .effective_world_inv_inertia_sqrt
.transform_vector(torque_dir1) .transform_vector(torque_dir1)
} else { } else {
na::zero() na::zero()
}; };
let gcross2 = if rb_type2.is_dynamic() { let gcross2 = if type2.is_dynamic() {
rb_mprops2 mprops2
.effective_world_inv_inertia_sqrt .effective_world_inv_inertia_sqrt
.transform_vector(torque_dir2) .transform_vector(torque_dir2)
} else { } else {
@@ -170,8 +158,8 @@ impl GenericVelocityConstraint {
jacobians, jacobians,
) )
.0 .0
} else if rb_type1.is_dynamic() { } else if type1.is_dynamic() {
force_dir1.dot(&rb_mprops1.effective_inv_mass.component_mul(&force_dir1)) force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1))
+ gcross1.gdot(gcross1) + gcross1.gdot(gcross1)
} else { } else {
0.0 0.0
@@ -189,8 +177,8 @@ impl GenericVelocityConstraint {
jacobians, jacobians,
) )
.0 .0
} else if rb_type2.is_dynamic() { } else if type2.is_dynamic() {
force_dir1.dot(&rb_mprops2.effective_inv_mass.component_mul(&force_dir1)) force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2) + gcross2.gdot(gcross2)
} else { } else {
0.0 0.0
@@ -224,8 +212,8 @@ impl GenericVelocityConstraint {
for j in 0..DIM - 1 { for j in 0..DIM - 1 {
let torque_dir1 = dp1.gcross(tangents1[j]); let torque_dir1 = dp1.gcross(tangents1[j]);
let gcross1 = if rb_type1.is_dynamic() { let gcross1 = if type1.is_dynamic() {
rb_mprops1 mprops1
.effective_world_inv_inertia_sqrt .effective_world_inv_inertia_sqrt
.transform_vector(torque_dir1) .transform_vector(torque_dir1)
} else { } else {
@@ -234,8 +222,8 @@ impl GenericVelocityConstraint {
constraint.elements[k].tangent_part.gcross1[j] = gcross1; constraint.elements[k].tangent_part.gcross1[j] = gcross1;
let torque_dir2 = dp2.gcross(-tangents1[j]); let torque_dir2 = dp2.gcross(-tangents1[j]);
let gcross2 = if rb_type2.is_dynamic() { let gcross2 = if type2.is_dynamic() {
rb_mprops2 mprops2
.effective_world_inv_inertia_sqrt .effective_world_inv_inertia_sqrt
.transform_vector(torque_dir2) .transform_vector(torque_dir2)
} else { } else {
@@ -255,9 +243,8 @@ impl GenericVelocityConstraint {
jacobians, jacobians,
) )
.0 .0
} else if rb_type1.is_dynamic() { } else if type1.is_dynamic() {
force_dir1 force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1))
.dot(&rb_mprops1.effective_inv_mass.component_mul(&force_dir1))
+ gcross1.gdot(gcross1) + gcross1.gdot(gcross1)
} else { } else {
0.0 0.0
@@ -275,9 +262,8 @@ impl GenericVelocityConstraint {
jacobians, jacobians,
) )
.0 .0
} else if rb_type2.is_dynamic() { } else if type2.is_dynamic() {
force_dir1 force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
.dot(&rb_mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2) + gcross2.gdot(gcross2)
} else { } else {
0.0 0.0
@@ -303,8 +289,8 @@ impl GenericVelocityConstraint {
// reduce all ops to nothing because its ndofs will be zero. // reduce all ops to nothing because its ndofs will be zero.
let generic_constraint_mask = (multibody1.is_some() as u8) let generic_constraint_mask = (multibody1.is_some() as u8)
| ((multibody2.is_some() as u8) << 1) | ((multibody2.is_some() as u8) << 1)
| (!rb_type1.is_dynamic() as u8) | (!type1.is_dynamic() as u8)
| ((!rb_type2.is_dynamic() as u8) << 1); | ((!type2.is_dynamic() as u8) << 1);
let constraint = GenericVelocityConstraint { let constraint = GenericVelocityConstraint {
velocity_constraint: constraint, velocity_constraint: constraint,

30
src/dynamics/solver/generic_velocity_ground_constraint.rs
View File

@@ -1,7 +1,5 @@
use crate::dynamics::solver::VelocityGroundConstraint; use crate::dynamics::solver::VelocityGroundConstraint;
use crate::dynamics::{ use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity};
IntegrationParameters, MultibodyJointSet, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS}; use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::WCross; use crate::utils::WCross;
@@ -48,16 +46,15 @@ impl GenericVelocityGroundConstraint {
(-manifold.data.normal, 1.0) (-manifold.data.normal, 1.0)
}; };
let (rb_vels1, world_com1) = if let Some(handle1) = handle1 { let (vels1, world_com1) = if let Some(handle1) = handle1 {
let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) = let rb1 = &bodies[handle1];
bodies.index_bundle(handle1.0); (rb1.vels, rb1.mprops.world_com)
(*vels1, mprops1.world_com)
} else { } else {
(RigidBodyVelocity::zero(), Point::origin()) (RigidBodyVelocity::zero(), Point::origin())
}; };
let (rb_vels2, rb_mprops2): (&RigidBodyVelocity, &RigidBodyMassProps) = let rb2 = &bodies[handle2.unwrap()];
bodies.index_bundle(handle2.unwrap().0); let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
let (mb2, link_id2) = handle2 let (mb2, link_id2) = handle2
.and_then(|h| multibodies.rigid_body_link(h)) .and_then(|h| multibodies.rigid_body_link(h))
@@ -68,11 +65,8 @@ impl GenericVelocityGroundConstraint {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis(); let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let tangents1 = super::compute_tangent_contact_directions( let tangents1 =
&force_dir1, super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
&rb_vels1.linvel,
&rb_vels2.linvel,
);
let multibodies_ndof = mb2.ndofs(); let multibodies_ndof = mb2.ndofs();
// For each solver contact we generate DIM constraints, and each constraints appends // For each solver contact we generate DIM constraints, and each constraints appends
@@ -96,7 +90,7 @@ impl GenericVelocityGroundConstraint {
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
tangent1: tangents1[0], tangent1: tangents1[0],
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: rb_mprops2.effective_inv_mass, im2: mprops2.effective_inv_mass,
limit: 0.0, limit: 0.0,
mj_lambda2, mj_lambda2,
manifold_id, manifold_id,
@@ -107,10 +101,10 @@ impl GenericVelocityGroundConstraint {
for k in 0..manifold_points.len() { for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k]; let manifold_point = &manifold_points[k];
let dp1 = manifold_point.point - world_com1; let dp1 = manifold_point.point - world_com1;
let dp2 = manifold_point.point - rb_mprops2.world_com; let dp2 = manifold_point.point - mprops2.world_com;
let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1); let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2); let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction; constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id; constraint.manifold_contact_id[k] = manifold_point.contact_id;

55
src/dynamics/solver/interaction_groups.rs
View File

@@ -3,7 +3,6 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[cfg(feature = "simd-is-enabled")] #[cfg(feature = "simd-is-enabled")]
use { use {
crate::data::BundleSet,
crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH}, crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
vec_map::VecMap, vec_map::VecMap,
}; };
@@ -90,14 +89,8 @@ impl ParallelInteractionGroups {
.zip(self.interaction_colors.iter_mut()) .zip(self.interaction_colors.iter_mut())
{ {
let mut body_pair = interactions[*interaction_id].body_pair(); let mut body_pair = interactions[*interaction_id].body_pair();
let is_fixed1 = body_pair let is_fixed1 = body_pair.0.map(|b| bodies[b].is_fixed()).unwrap_or(true);
.0 let is_fixed2 = body_pair.1.map(|b| bodies[b].is_fixed()).unwrap_or(true);
.map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_fixed())
.unwrap_or(true);
let is_fixed2 = body_pair
.1
.map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_fixed())
.unwrap_or(true);
let representative = |handle: RigidBodyHandle| { let representative = |handle: RigidBodyHandle| {
if let Some(link) = multibodies.rigid_body_link(handle).copied() { if let Some(link) = multibodies.rigid_body_link(handle).copied() {
@@ -119,28 +112,28 @@ impl ParallelInteractionGroups {
match (is_fixed1, is_fixed2) { match (is_fixed1, is_fixed2) {
(false, false) => { (false, false) => {
let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0); let rb1 = &bodies[body_pair.0.unwrap()];
let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0); let rb2 = &bodies[body_pair.1.unwrap()];
let color_mask = let color_mask =
bcolors[rb_ids1.active_set_offset] | bcolors[rb_ids2.active_set_offset]; bcolors[rb1.ids.active_set_offset] | bcolors[rb2.ids.active_set_offset];
*color = (!color_mask).trailing_zeros() as usize; *color = (!color_mask).trailing_zeros() as usize;
color_len[*color] += 1; color_len[*color] += 1;
bcolors[rb_ids1.active_set_offset] |= 1 << *color; bcolors[rb1.ids.active_set_offset] |= 1 << *color;
bcolors[rb_ids2.active_set_offset] |= 1 << *color; bcolors[rb2.ids.active_set_offset] |= 1 << *color;
} }
(true, false) => { (true, false) => {
let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0); let rb2 = &bodies[body_pair.1.unwrap()];
let color_mask = bcolors[rb_ids2.active_set_offset]; let color_mask = bcolors[rb2.ids.active_set_offset];
*color = 127 - (!color_mask).leading_zeros() as usize; *color = 127 - (!color_mask).leading_zeros() as usize;
color_len[*color] += 1; color_len[*color] += 1;
bcolors[rb_ids2.active_set_offset] |= 1 << *color; bcolors[rb2.ids.active_set_offset] |= 1 << *color;
} }
(false, true) => { (false, true) => {
let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0); let rb1 = &bodies[body_pair.0.unwrap()];
let color_mask = bcolors[rb_ids1.active_set_offset]; let color_mask = bcolors[rb1.ids.active_set_offset];
*color = 127 - (!color_mask).leading_zeros() as usize; *color = 127 - (!color_mask).leading_zeros() as usize;
color_len[*color] += 1; color_len[*color] += 1;
bcolors[rb_ids1.active_set_offset] |= 1 << *color; bcolors[rb1.ids.active_set_offset] |= 1 << *color;
} }
(true, true) => unreachable!(), (true, true) => unreachable!(),
} }
@@ -258,13 +251,11 @@ impl InteractionGroups {
for interaction_i in interaction_indices { for interaction_i in interaction_indices {
let interaction = &interactions[*interaction_i].weight; let interaction = &interactions[*interaction_i].weight;
let (status1, ids1): (&RigidBodyType, &RigidBodyIds) = let rb1 = &bodies[interaction.body1];
bodies.index_bundle(interaction.body1.0); let rb2 = &bodies[interaction.body2];
let (status2, ids2): (&RigidBodyType, &RigidBodyIds) =
bodies.index_bundle(interaction.body2.0);
let is_fixed1 = !status1.is_dynamic(); let is_fixed1 = !rb1.is_dynamic();
let is_fixed2 = !status2.is_dynamic(); let is_fixed2 = !rb2.is_dynamic();
if is_fixed1 && is_fixed2 { if is_fixed1 && is_fixed2 {
continue; continue;
@@ -277,8 +268,8 @@ impl InteractionGroups {
} }
let ijoint = interaction.data.locked_axes.bits() as usize; let ijoint = interaction.data.locked_axes.bits() as usize;
let i1 = ids1.active_set_offset; let i1 = rb1.ids.active_set_offset;
let i2 = ids2.active_set_offset; let i2 = rb2.ids.active_set_offset;
let conflicts = let conflicts =
self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint]; self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint];
let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
@@ -421,15 +412,15 @@ impl InteractionGroups {
let (status1, active_set_offset1) = if let Some(rb1) = interaction.data.rigid_body1 let (status1, active_set_offset1) = if let Some(rb1) = interaction.data.rigid_body1
{ {
let data: (_, &RigidBodyIds) = bodies.index_bundle(rb1.0); let rb1 = &bodies[rb1];
(*data.0, data.1.active_set_offset) (rb1.body_type, rb1.ids.active_set_offset)
} else { } else {
(RigidBodyType::Fixed, usize::MAX) (RigidBodyType::Fixed, usize::MAX)
}; };
let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2 let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2
{ {
let data: (_, &RigidBodyIds) = bodies.index_bundle(rb2.0); let rb2 = &bodies[rb2];
(*data.0, data.1.active_set_offset) (rb2.body_type, rb2.ids.active_set_offset)
} else { } else {
(RigidBodyType::Fixed, usize::MAX) (RigidBodyType::Fixed, usize::MAX)
}; };

129
src/dynamics/solver/joint_constraint/joint_constraint.rs
View File

@@ -6,8 +6,7 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
}; };
use crate::dynamics::solver::DeltaVel; use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{ use crate::dynamics::{
ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds, ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyType, RigidBodyVelocity,
}; };
use crate::math::{Real, SPATIAL_DIM}; use crate::math::{Real, SPATIAL_DIM};
use crate::prelude::MultibodyJointSet; use crate::prelude::MultibodyJointSet;
@@ -63,39 +62,26 @@ impl AnyJointVelocityConstraint {
) { ) {
let local_frame1 = joint.data.local_frame1; let local_frame1 = joint.data.local_frame1;
let local_frame2 = joint.data.local_frame2; let local_frame2 = joint.data.local_frame2;
let rb1: ( let rb1 = &bodies[joint.body1];
&RigidBodyPosition, let rb2 = &bodies[joint.body2];
&RigidBodyVelocity, let frame1 = rb1.pos.position * local_frame1;
&RigidBodyMassProps, let frame2 = rb2.pos.position * local_frame2;
&RigidBodyIds,
) = bodies.index_bundle(joint.body1.0);
let rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
) = bodies.index_bundle(joint.body2.0);
let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rb1;
let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rb2;
let frame1 = rb_pos1.position * local_frame1;
let frame2 = rb_pos2.position * local_frame2;
let body1 = SolverBody { let body1 = SolverBody {
linvel: rb_vel1.linvel, linvel: rb1.vels.linvel,
angvel: rb_vel1.angvel, angvel: rb1.vels.angvel,
im: rb_mprops1.effective_inv_mass, im: rb1.mprops.effective_inv_mass,
sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt, sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
world_com: rb_mprops1.world_com, world_com: rb1.mprops.world_com,
mj_lambda: [rb_ids1.active_set_offset], mj_lambda: [rb1.ids.active_set_offset],
}; };
let body2 = SolverBody { let body2 = SolverBody {
linvel: rb_vel2.linvel, linvel: rb2.vels.linvel,
angvel: rb_vel2.angvel, angvel: rb2.vels.angvel,
im: rb_mprops2.effective_inv_mass, im: rb2.mprops.effective_inv_mass,
sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt, sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
world_com: rb_mprops2.world_com, world_com: rb2.mprops.world_com,
mj_lambda: [rb_ids2.active_set_offset], mj_lambda: [rb2.ids.active_set_offset],
}; };
let mb1 = multibodies let mb1 = multibodies
@@ -186,16 +172,20 @@ impl AnyJointVelocityConstraint {
out: &mut Vec<Self>, out: &mut Vec<Self>,
insert_at: Option<usize>, insert_at: Option<usize>,
) { ) {
use crate::dynamics::{
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
let rbs1: ( let rbs1: (
[&RigidBodyPosition; SIMD_WIDTH], [&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH], [&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH], [&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH], [&RigidBodyIds; SIMD_WIDTH],
) = ( ) = (
gather![|ii| bodies.index(impulse_joints[ii].body1.0)], gather![|ii| &bodies[impulse_joints[ii].body1].pos],
gather![|ii| bodies.index(impulse_joints[ii].body1.0)], gather![|ii| &bodies[impulse_joints[ii].body1].vels],
gather![|ii| bodies.index(impulse_joints[ii].body1.0)], gather![|ii| &bodies[impulse_joints[ii].body1].mprops],
gather![|ii| bodies.index(impulse_joints[ii].body1.0)], gather![|ii| &bodies[impulse_joints[ii].body1].ids],
); );
let rbs2: ( let rbs2: (
[&RigidBodyPosition; SIMD_WIDTH], [&RigidBodyPosition; SIMD_WIDTH],
@@ -203,10 +193,10 @@ impl AnyJointVelocityConstraint {
[&RigidBodyMassProps; SIMD_WIDTH], [&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH], [&RigidBodyIds; SIMD_WIDTH],
) = ( ) = (
gather![|ii| bodies.index(impulse_joints[ii].body2.0)], gather![|ii| &bodies[impulse_joints[ii].body2].pos],
gather![|ii| bodies.index(impulse_joints[ii].body2.0)], gather![|ii| &bodies[impulse_joints[ii].body2].vels],
gather![|ii| bodies.index(impulse_joints[ii].body2.0)], gather![|ii| &bodies[impulse_joints[ii].body2].mprops],
gather![|ii| bodies.index(impulse_joints[ii].body2.0)], gather![|ii| &bodies[impulse_joints[ii].body2].ids],
); );
let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rbs1; let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rbs1;
@@ -276,8 +266,7 @@ impl AnyJointVelocityConstraint {
) { ) {
let mut handle1 = joint.body1; let mut handle1 = joint.body1;
let mut handle2 = joint.body2; let mut handle2 = joint.body2;
let status2: &RigidBodyType = bodies.index(handle2.0); let flipped = !bodies[handle2].is_dynamic();
let flipped = !status2.is_dynamic();
let (local_frame1, local_frame2) = if flipped { let (local_frame1, local_frame2) = if flipped {
std::mem::swap(&mut handle1, &mut handle2); std::mem::swap(&mut handle1, &mut handle2);
@@ -286,35 +275,27 @@ impl AnyJointVelocityConstraint {
(joint.data.local_frame1, joint.data.local_frame2) (joint.data.local_frame1, joint.data.local_frame2)
}; };
let rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps) = let rb1 = &bodies[handle1];
bodies.index_bundle(handle1.0); let rb2 = &bodies[handle2];
let rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
) = bodies.index_bundle(handle2.0);
let (rb_pos1, rb_vel1, rb_mprops1) = rb1; let frame1 = rb1.pos.position * local_frame1;
let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rb2; let frame2 = rb2.pos.position * local_frame2;
let frame1 = rb_pos1.position * local_frame1;
let frame2 = rb_pos2.position * local_frame2;
let body1 = SolverBody { let body1 = SolverBody {
linvel: rb_vel1.linvel, linvel: rb1.vels.linvel,
angvel: rb_vel1.angvel, angvel: rb1.vels.angvel,
im: rb_mprops1.effective_inv_mass, im: rb1.mprops.effective_inv_mass,
sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt, sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
world_com: rb_mprops1.world_com, world_com: rb1.mprops.world_com,
mj_lambda: [crate::INVALID_USIZE], mj_lambda: [crate::INVALID_USIZE],
}; };
let body2 = SolverBody { let body2 = SolverBody {
linvel: rb_vel2.linvel, linvel: rb2.vels.linvel,
angvel: rb_vel2.angvel, angvel: rb2.vels.angvel,
im: rb_mprops2.effective_inv_mass, im: rb2.mprops.effective_inv_mass,
sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt, sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
world_com: rb_mprops2.world_com, world_com: rb2.mprops.world_com,
mj_lambda: [rb_ids2.active_set_offset], mj_lambda: [rb2.ids.active_set_offset],
}; };
if let Some(mb2) = multibodies if let Some(mb2) = multibodies
@@ -399,9 +380,13 @@ impl AnyJointVelocityConstraint {
out: &mut Vec<Self>, out: &mut Vec<Self>,
insert_at: Option<usize>, insert_at: Option<usize>,
) { ) {
use crate::dynamics::{
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
};
let mut handles1 = gather![|ii| impulse_joints[ii].body1]; let mut handles1 = gather![|ii| impulse_joints[ii].body1];
let mut handles2 = gather![|ii| impulse_joints[ii].body2]; let mut handles2 = gather![|ii| impulse_joints[ii].body2];
let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)]; let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].body_type];
let mut flipped = [false; SIMD_WIDTH]; let mut flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH { for ii in 0..SIMD_WIDTH {
@@ -429,9 +414,9 @@ impl AnyJointVelocityConstraint {
[&RigidBodyVelocity; SIMD_WIDTH], [&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH], [&RigidBodyMassProps; SIMD_WIDTH],
) = ( ) = (
gather![|ii| bodies.index(handles1[ii].0)], gather![|ii| &bodies[handles1[ii]].pos],
gather![|ii| bodies.index(handles1[ii].0)], gather![|ii| &bodies[handles1[ii]].vels],
gather![|ii| bodies.index(handles1[ii].0)], gather![|ii| &bodies[handles1[ii]].mprops],
); );
let rbs2: ( let rbs2: (
[&RigidBodyPosition; SIMD_WIDTH], [&RigidBodyPosition; SIMD_WIDTH],
@@ -439,10 +424,10 @@ impl AnyJointVelocityConstraint {
[&RigidBodyMassProps; SIMD_WIDTH], [&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH], [&RigidBodyIds; SIMD_WIDTH],
) = ( ) = (
gather![|ii| bodies.index(handles2[ii].0)], gather![|ii| &bodies[handles2[ii]].pos],
gather![|ii| bodies.index(handles2[ii].0)], gather![|ii| &bodies[handles2[ii]].vels],
gather![|ii| bodies.index(handles2[ii].0)], gather![|ii| &bodies[handles2[ii]].mprops],
gather![|ii| bodies.index(handles2[ii].0)], gather![|ii| &bodies[handles2[ii]].ids],
); );
let (rb_pos1, rb_vel1, rb_mprops1) = rbs1; let (rb_pos1, rb_vel1, rb_mprops1) = rbs1;

13
src/dynamics/solver/parallel_island_solver.rs
View File

@@ -7,8 +7,7 @@ use crate::dynamics::solver::{
}; };
use crate::dynamics::{ use crate::dynamics::{
IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet, IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,
RigidBodyDamping, RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodySet,
RigidBodyType, RigidBodyVelocity,
}; };
use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::geometry::{ContactManifold, ContactManifoldIndex};
use na::DVector; use na::DVector;
@@ -316,15 +315,13 @@ impl ParallelIslandSolver {
mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0); mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
} }
} else { } else {
let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) = let rb = &bodies[*handle];
bodies.index_bundle(handle.0); let dvel = &mut velocity_solver.mj_lambdas[rb.ids.active_set_offset];
let dvel = &mut velocity_solver.mj_lambdas[ids.active_set_offset];
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor: // by the square root of the inertia tensor:
dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt; dvel.angular += rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt;
dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt; dvel.linear += rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt;
} }
} }
} }

3
src/dynamics/solver/parallel_solver_constraints.rs
View File

@@ -8,8 +8,7 @@ use crate::dynamics::solver::{
}; };
use crate::dynamics::{ use crate::dynamics::{
ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyIndex, ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyIndex,
MultibodyJointSet, RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, MultibodyJointSet, RigidBodyHandle, RigidBodySet,
RigidBodyType, RigidBodyVelocity,
}; };
use crate::geometry::ContactManifold; use crate::geometry::ContactManifold;
use crate::math::{Real, SPATIAL_DIM}; use crate::math::{Real, SPATIAL_DIM};

51
src/dynamics/solver/parallel_velocity_solver.rs
View File

@@ -2,8 +2,7 @@ use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadC
use crate::concurrent_loop; use crate::concurrent_loop;
use crate::dynamics::{ use crate::dynamics::{
solver::ParallelSolverConstraints, IntegrationParameters, IslandManager, JointGraphEdge, solver::ParallelSolverConstraints, IntegrationParameters, IslandManager, JointGraphEdge,
MultibodyJointSet, RigidBodyDamping, RigidBodyForces, RigidBodyIds, RigidBodyMassProps, MultibodyJointSet, RigidBodySet,
RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
}; };
use crate::geometry::ContactManifold; use crate::geometry::ContactManifold;
use crate::math::Real; use crate::math::Real;
@@ -210,33 +209,22 @@ impl ParallelVelocitySolver {
multibody.velocities = prev_vels; multibody.velocities = prev_vels;
} }
} else { } else {
let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) = let rb = bodies.index_mut_internal(*handle);
bodies.index_bundle(handle.0); let dvel = self.mj_lambdas[rb.ids.active_set_offset];
let dangvel = rb.mprops
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
.effective_world_inv_inertia_sqrt .effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular); .transform_vector(dvel.angular);
// Update positions. // Update positions.
let (rb_pos, rb_vels, rb_damping, rb_mprops): ( let mut new_vels = rb.vels;
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let mut new_pos = *rb_pos;
let mut new_vels = *rb_vels;
new_vels.linvel += dvel.linear; new_vels.linvel += dvel.linear;
new_vels.angvel += dangvel; new_vels.angvel += dangvel;
new_vels = new_vels.apply_damping(params.dt, rb_damping); new_vels = new_vels.apply_damping(params.dt, &rb.damping);
new_pos.next_position = new_vels.integrate( rb.pos.next_position = new_vels.integrate(
params.dt, params.dt,
&rb_pos.position, &rb.pos.position,
&rb_mprops.local_mprops.local_com, &rb.mprops.local_mprops.local_com,
); );
bodies.set_internal(handle.0, new_pos);
} }
} }
} }
@@ -321,23 +309,14 @@ impl ParallelVelocitySolver {
multibody.velocities += mj_lambdas; multibody.velocities += mj_lambdas;
} }
} else { } else {
let (rb_ids, rb_damping, rb_mprops): ( let rb = bodies.index_mut_internal(*handle);
&RigidBodyIds, let dvel = self.mj_lambdas[rb.ids.active_set_offset];
&RigidBodyDamping, let dangvel = rb.mprops
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
.effective_world_inv_inertia_sqrt .effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular); .transform_vector(dvel.angular);
let damping = *rb_damping; // To avoid borrow issues. rb.vels.linvel += dvel.linear;
rb.vels.angvel += dangvel;
bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| { rb.vels = rb.vels.apply_damping(params.dt, &rb.damping);
vels.linvel += dvel.linear;
vels.angvel += dangvel;
*vels = vels.apply_damping(params.dt, &damping);
});
} }
} }
} }

17
src/dynamics/solver/velocity_constraint.rs
View File

@@ -3,9 +3,7 @@ use crate::dynamics::solver::{
}; };
#[cfg(feature = "simd-is-enabled")] #[cfg(feature = "simd-is-enabled")]
use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}; use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::dynamics::{ use crate::dynamics::{IntegrationParameters, RigidBodySet};
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS}; use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot}; use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot};
@@ -160,13 +158,14 @@ impl VelocityConstraint {
let handle1 = manifold.data.rigid_body1.unwrap(); let handle1 = manifold.data.rigid_body1.unwrap();
let handle2 = manifold.data.rigid_body2.unwrap(); let handle2 = manifold.data.rigid_body2.unwrap();
let (ids1, vels1, mprops1): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle1.0);
let (ids2, vels2, mprops2): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle2.0);
let mj_lambda1 = ids1.active_set_offset; let rb1 = &bodies[handle1];
let mj_lambda2 = ids2.active_set_offset; let (vels1, mprops1) = (&rb1.vels, &rb1.mprops);
let rb2 = &bodies[handle2];
let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
let mj_lambda1 = rb1.ids.active_set_offset;
let mj_lambda2 = rb2.ids.active_set_offset;
let force_dir1 = -manifold.data.normal; let force_dir1 = -manifold.data.normal;
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]

16
src/dynamics/solver/velocity_constraint_wide.rs
View File

@@ -1,7 +1,9 @@
use super::{ use super::{
AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart, AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart,
}; };
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{ use crate::math::{
AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
@@ -49,12 +51,12 @@ impl WVelocityConstraint {
let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()]; let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()]; let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
let vels1: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)]; let vels1: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].vels];
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)]; let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].vels];
let ids1: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)]; let ids1: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].ids];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)]; let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].ids];
let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)]; let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].mprops];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)]; let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].mprops];
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]); let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]);

16
src/dynamics/solver/velocity_ground_constraint.rs
View File

@@ -7,9 +7,7 @@ use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::WBasis; use crate::utils::WBasis;
use crate::utils::{self, WAngularInertia, WCross, WDot}; use crate::utils::{self, WAngularInertia, WCross, WDot};
use crate::dynamics::{ use crate::dynamics::{IntegrationParameters, RigidBodySet, RigidBodyVelocity};
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[derive(Copy, Clone, Debug)] #[derive(Copy, Clone, Debug)]
@@ -51,15 +49,15 @@ impl VelocityGroundConstraint {
}; };
let (vels1, world_com1) = if let Some(handle1) = handle1 { let (vels1, world_com1) = if let Some(handle1) = handle1 {
let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) = let rb1 = &bodies[handle1];
bodies.index_bundle(handle1.0); (rb1.vels, rb1.mprops.world_com)
(*vels1, mprops1.world_com)
} else { } else {
(RigidBodyVelocity::zero(), Point::origin()) (RigidBodyVelocity::zero(), Point::origin())
}; };
let (ids2, vels2, mprops2): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) = let rb2 = &bodies[handle2.unwrap()];
bodies.index_bundle(handle2.unwrap().0); let vels2 = &rb2.vels;
let mprops2 = &rb2.mprops;
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis(); let tangents1 = force_dir1.orthonormal_basis();
@@ -67,7 +65,7 @@ impl VelocityGroundConstraint {
let tangents1 = let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
let mj_lambda2 = ids2.active_set_offset; let mj_lambda2 = rb2.ids.active_set_offset;
for (_l, manifold_points) in manifold for (_l, manifold_points) in manifold
.data .data

14
src/dynamics/solver/velocity_ground_constraint_wide.rs
View File

@@ -2,7 +2,9 @@ use super::{
AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement, AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement,
VelocityGroundConstraintNormalPart, VelocityGroundConstraintNormalPart,
}; };
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{ use crate::math::{
AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH, AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
@@ -54,20 +56,20 @@ impl WVelocityGroundConstraint {
let vels1: [RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| { let vels1: [RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| {
handles1[ii] handles1[ii]
.map(|h| *bodies.index(h.0)) .map(|h| bodies[h].vels)
.unwrap_or_else(RigidBodyVelocity::zero) .unwrap_or_else(RigidBodyVelocity::zero)
}]; }];
let world_com1 = Point::from(gather![|ii| { let world_com1 = Point::from(gather![|ii| {
handles1[ii] handles1[ii]
.map(|h| ComponentSet::<RigidBodyMassProps>::index(bodies, h.0).world_com) .map(|h| bodies[h].mprops.world_com)
.unwrap_or_else(Point::origin) .unwrap_or_else(Point::origin)
}]); }]);
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = let vels2: [&RigidBodyVelocity; SIMD_WIDTH] =
gather![|ii| bodies.index(handles2[ii].unwrap().0)]; gather![|ii| &bodies[handles2[ii].unwrap()].vels];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].unwrap().0)]; let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii].unwrap()].ids];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] =
gather![|ii| bodies.index(handles2[ii].unwrap().0)]; gather![|ii| &bodies[handles2[ii].unwrap()].mprops];
let flipped_sign = SimdReal::from(flipped); let flipped_sign = SimdReal::from(flipped);

64
src/dynamics/solver/velocity_solver.rs
View File

@@ -1,9 +1,7 @@
use super::AnyJointVelocityConstraint; use super::AnyJointVelocityConstraint;
use crate::dynamics::{ use crate::dynamics::{
solver::{AnyVelocityConstraint, DeltaVel}, solver::{AnyVelocityConstraint, DeltaVel},
IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyDamping, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodySet,
RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodySet,
RigidBodyVelocity,
}; };
use crate::geometry::ContactManifold; use crate::geometry::ContactManifold;
use crate::math::Real; use crate::math::Real;
@@ -59,15 +57,15 @@ impl VelocitySolver {
mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0); mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
} }
} else { } else {
let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) = let rb = &bodies[*handle];
bodies.index_bundle(handle.0); let dvel = &mut self.mj_lambdas[rb.ids.active_set_offset];
let dvel = &mut self.mj_lambdas[ids.active_set_offset];
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor: // by the square root of the inertia tensor:
dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt; dvel.angular +=
dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt; rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt;
dvel.linear +=
rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt;
} }
} }
@@ -151,33 +149,26 @@ impl VelocitySolver {
multibody.velocities = prev_vels; multibody.velocities = prev_vels;
} }
} else { } else {
let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) = let rb = bodies.index_mut_internal(*handle);
bodies.index_bundle(handle.0);
let dvel = self.mj_lambdas[rb_ids.active_set_offset]; let dvel = self.mj_lambdas[rb.ids.active_set_offset];
let dangvel = rb_mprops let dangvel = rb
.mprops
.effective_world_inv_inertia_sqrt .effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular); .transform_vector(dvel.angular);
// Update positions. // Update positions.
let (rb_pos, rb_vels, rb_damping, rb_mprops): ( let mut new_pos = rb.pos;
&RigidBodyPosition, let mut new_vels = rb.vels;
&RigidBodyVelocity,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let mut new_pos = *rb_pos;
let mut new_vels = *rb_vels;
new_vels.linvel += dvel.linear; new_vels.linvel += dvel.linear;
new_vels.angvel += dangvel; new_vels.angvel += dangvel;
new_vels = new_vels.apply_damping(params.dt, rb_damping); new_vels = new_vels.apply_damping(params.dt, &rb.damping);
new_pos.next_position = new_vels.integrate( new_pos.next_position = new_vels.integrate(
params.dt, params.dt,
&rb_pos.position, &rb.pos.position,
&rb_mprops.local_mprops.local_com, &rb.mprops.local_mprops.local_com,
); );
bodies.set_internal(handle.0, new_pos); rb.pos = new_pos;
} }
} }
@@ -234,23 +225,16 @@ impl VelocitySolver {
multibody.velocities += mj_lambdas; multibody.velocities += mj_lambdas;
} }
} else { } else {
let (rb_ids, rb_damping, rb_mprops): ( let rb = bodies.index_mut_internal(*handle);
&RigidBodyIds, let dvel = self.mj_lambdas[rb.ids.active_set_offset];
&RigidBodyDamping, let dangvel = rb
&RigidBodyMassProps, .mprops
) = bodies.index_bundle(handle.0);
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
.effective_world_inv_inertia_sqrt .effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular); .transform_vector(dvel.angular);
let damping = *rb_damping; // To avoid borrow issues.
bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| { rb.vels.linvel += dvel.linear;
vels.linvel += dvel.linear; rb.vels.angvel += dangvel;
vels.angvel += dangvel; rb.vels = rb.vels.apply_damping(params.dt, &rb.damping);
*vels = vels.apply_damping(params.dt, &damping);
});
} }
} }

11
src/geometry/broad_phase_multi_sap/broad_phase.rs
View File

@@ -451,7 +451,7 @@ impl BroadPhase {
for handle in modified_colliders { for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer // NOTE: we use `get` because the collider may no longer
// exist if it has been removed. // exist if it has been removed.
if let Some(co) = colliders.get(*handle) { if let Some(co) = colliders.get_mut_internal(*handle) {
if !co.changes.needs_broad_phase_update() { if !co.changes.needs_broad_phase_update() {
continue; continue;
} }
@@ -471,12 +471,9 @@ impl BroadPhase {
// Make sure we have the new proxy index in case // Make sure we have the new proxy index in case
// the collider was added for the first time. // the collider was added for the first time.
colliders.set_internal( co.bf_data = ColliderBroadPhaseData {
handle.0, proxy_index: new_proxy_id,
ColliderBroadPhaseData { };
proxy_index: new_proxy_id,
},
);
} }
} }
} }

54
src/geometry/collider_set.rs
View File

@@ -73,7 +73,7 @@ impl ColliderSet {
// Make sure the internal links are reset, they may not be // Make sure the internal links are reset, they may not be
// if this rigid-body was obtained by cloning another one. // if this rigid-body was obtained by cloning another one.
coll.reset_internal_references(); coll.reset_internal_references();
coll.co_parent = None; coll.parent = None;
let handle = ColliderHandle(self.colliders.insert(coll)); let handle = ColliderHandle(self.colliders.insert(coll));
self.modified_colliders.push(handle); self.modified_colliders.push(handle);
handle handle
@@ -91,12 +91,12 @@ impl ColliderSet {
// if this collider was obtained by cloning another one. // if this collider was obtained by cloning another one.
coll.reset_internal_references(); coll.reset_internal_references();
if let Some(prev_parent) = &mut coll.co_parent { if let Some(prev_parent) = &mut coll.parent {
prev_parent.handle = parent_handle; prev_parent.handle = parent_handle;
} else { } else {
coll.co_parent = Some(ColliderParent { coll.parent = Some(ColliderParent {
handle: parent_handle, handle: parent_handle,
pos_wrt_parent: coll.co_pos.0, pos_wrt_parent: coll.pos.0,
}); });
} }
@@ -111,10 +111,10 @@ impl ColliderSet {
let coll = self.colliders.get_mut(handle.0).unwrap(); let coll = self.colliders.get_mut(handle.0).unwrap();
parent.add_collider( parent.add_collider(
handle, handle,
coll.co_parent.as_mut().unwrap(), coll.parent.as_mut().unwrap(),
&mut coll.co_pos, &mut coll.pos,
&coll.co_shape, &coll.shape,
&coll.co_mprops, &coll.mprops,
); );
handle handle
} }
@@ -128,12 +128,12 @@ impl ColliderSet {
bodies: &mut RigidBodySet, bodies: &mut RigidBodySet,
) { ) {
if let Some(collider) = self.get_mut(handle) { if let Some(collider) = self.get_mut(handle) {
let curr_parent = collider.co_parent.map(|p| p.handle); let curr_parent = collider.parent.map(|p| p.handle);
if new_parent_handle == curr_parent { if new_parent_handle == curr_parent {
return; // Nothing to do, this is the same parent. return; // Nothing to do, this is the same parent.
} }
collider.co_changes |= ColliderChanges::PARENT; collider.changes |= ColliderChanges::PARENT;
if let Some(parent_handle) = curr_parent { if let Some(parent_handle) = curr_parent {
if let Some(rb) = bodies.get_mut(parent_handle) { if let Some(rb) = bodies.get_mut(parent_handle) {
@@ -143,10 +143,10 @@ impl ColliderSet {
match new_parent_handle { match new_parent_handle {
Some(new_parent_handle) => { Some(new_parent_handle) => {
if let Some(co_parent) = &mut collider.co_parent { if let Some(parent) = &mut collider.parent {
co_parent.handle = new_parent_handle; parent.handle = new_parent_handle;
} else { } else {
collider.co_parent = Some(ColliderParent { collider.parent = Some(ColliderParent {
handle: new_parent_handle, handle: new_parent_handle,
pos_wrt_parent: Isometry::identity(), pos_wrt_parent: Isometry::identity(),
}) })
@@ -155,14 +155,14 @@ impl ColliderSet {
if let Some(rb) = bodies.get_mut(new_parent_handle) { if let Some(rb) = bodies.get_mut(new_parent_handle) {
rb.add_collider( rb.add_collider(
handle, handle,
collider.co_parent.as_ref().unwrap(), collider.parent.as_ref().unwrap(),
&mut collider.co_pos, &mut collider.pos,
&collider.co_shape, &collider.shape,
&collider.co_mprops, &collider.mprops,
); );
} }
} }
None => collider.co_parent = None, None => collider.parent = None,
} }
} }
} }
@@ -185,14 +185,14 @@ impl ColliderSet {
*/ */
// NOTE: we use `get_mut_internal_with_modification_tracking` instead of `get_mut_internal` so that the // NOTE: we use `get_mut_internal_with_modification_tracking` instead of `get_mut_internal` so that the
// modification flag is updated properly. // modification flag is updated properly.
if let Some(co_parent) = &collider.co_parent { if let Some(parent) = &collider.parent {
if let Some(parent) = if let Some(parent_rb) =
bodies.get_mut_internal_with_modification_tracking(co_parent.handle) bodies.get_mut_internal_with_modification_tracking(parent.handle)
{ {
parent.remove_collider_internal(handle, &collider); parent_rb.remove_collider_internal(handle, &collider);
if wake_up { if wake_up {
islands.wake_up(bodies, co_parent.handle, true); islands.wake_up(bodies, parent.handle, true);
} }
} }
} }
@@ -247,8 +247,8 @@ impl ColliderSet {
collider: &mut Collider, collider: &mut Collider,
modified_colliders: &mut Vec<ColliderHandle>, modified_colliders: &mut Vec<ColliderHandle>,
) { ) {
if !collider.co_changes.contains(ColliderChanges::MODIFIED) { if !collider.changes.contains(ColliderChanges::MODIFIED) {
collider.co_changes = ColliderChanges::MODIFIED; collider.changes = ColliderChanges::MODIFIED;
modified_colliders.push(handle); modified_colliders.push(handle);
} }
} }
@@ -261,6 +261,10 @@ impl ColliderSet {
Some(result) Some(result)
} }
pub(crate) fn index_mut_internal(&mut self, handle: ColliderHandle) -> &mut Collider {
&mut self.colliders[handle.0]
}
pub(crate) fn get_mut_internal(&mut self, handle: ColliderHandle) -> Option<&mut Collider> { pub(crate) fn get_mut_internal(&mut self, handle: ColliderHandle) -> Option<&mut Collider> {
self.colliders.get_mut(handle.0) self.colliders.get_mut(handle.0)
} }

252
src/geometry/narrow_phase.rs
View File

@@ -3,13 +3,11 @@ use rayon::prelude::*;
use crate::data::Coarena; use crate::data::Coarena;
use crate::dynamics::{ use crate::dynamics::{
CoefficientCombineRule, IslandManager, RigidBodyActivation, RigidBodyDominance, RigidBodyIds, CoefficientCombineRule, IslandManager, RigidBodyDominance, RigidBodySet, RigidBodyType,
RigidBodySet, RigidBodyType,
}; };
use crate::geometry::{ use crate::geometry::{
BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderMaterial, BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair,
ColliderPair, ColliderParent, ColliderPosition, ColliderSet, ColliderShape, ColliderType, ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair,
CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair,
InteractionGraph, IntersectionPair, SolverContact, SolverFlags, InteractionGraph, IntersectionPair, SolverContact, SolverFlags,
}; };
use crate::math::{Real, Vector}; use crate::math::{Real, Vector};
@@ -17,7 +15,6 @@ use crate::pipeline::{
ActiveEvents, ActiveHooks, ContactModificationContext, EventHandler, PairFilterContext, ActiveEvents, ActiveHooks, ContactModificationContext, EventHandler, PairFilterContext,
PhysicsHooks, PhysicsHooks,
}; };
use crate::prelude::ColliderFlags;
use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher}; use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher};
use parry::utils::IsometryOpt; use parry::utils::IsometryOpt;
use std::collections::HashMap; use std::collections::HashMap;
@@ -311,12 +308,12 @@ impl NarrowPhase {
// Wake up every body in contact with the deleted collider and generate Stopped collision events. // Wake up every body in contact with the deleted collider and generate Stopped collision events.
if let Some(islands) = islands.as_deref_mut() { if let Some(islands) = islands.as_deref_mut() {
for (a, b, pair) in self.contact_graph.interactions_with(contact_graph_id) { for (a, b, pair) in self.contact_graph.interactions_with(contact_graph_id) {
if let Some(parent) = colliders.get(a.0).map(|c| c.handle) { if let Some(parent) = colliders.get(a).and_then(|c| c.parent.as_ref()) {
islands.wake_up(bodies, parent, true) islands.wake_up(bodies, parent.handle, true)
} }
if let Some(parent) = colliders.get(b.0).map(|c| c.handle) { if let Some(parent) = colliders.get(b).and_then(|c| c.parent.as_ref()) {
islands.wake_up(bodies, parent, true) islands.wake_up(bodies, parent.handle, true)
} }
if pair.start_event_emited { if pair.start_event_emited {
@@ -379,10 +376,8 @@ impl NarrowPhase {
for handle in modified_colliders { for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer // NOTE: we use `get` because the collider may no longer
// exist if it has been removed. // exist if it has been removed.
let co_changes: Option<&ColliderChanges> = colliders.get(handle.0); if let Some(co) = colliders.get(*handle) {
if co.changes.needs_narrow_phase_update() {
if let Some(co_changes) = co_changes {
if co_changes.needs_narrow_phase_update() {
// No flag relevant to the narrow-phase is enabled for this collider. // No flag relevant to the narrow-phase is enabled for this collider.
continue; continue;
} }
@@ -392,13 +387,8 @@ impl NarrowPhase {
// so that the narrow-phase properly takes into account the change in, e.g., // so that the narrow-phase properly takes into account the change in, e.g.,
// collision groups. Waking up the modified collider's parent isn't enough because // collision groups. Waking up the modified collider's parent isn't enough because
// it could be a fixed or kinematic body which don't propagate the wake-up state. // it could be a fixed or kinematic body which don't propagate the wake-up state.
let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
let (co_changes, co_type): (&ColliderChanges, &ColliderType) =
colliders.index_bundle(handle.0);
if let Some(islands) = islands.as_deref_mut() { if let Some(islands) = islands.as_deref_mut() {
if let Some(co_parent) = co_parent { if let Some(co_parent) = &co.parent {
islands.wake_up(bodies, co_parent.handle, true); islands.wake_up(bodies, co_parent.handle, true);
} }
@@ -407,8 +397,9 @@ impl NarrowPhase {
.interactions_with(gid.contact_graph_index) .interactions_with(gid.contact_graph_index)
{ {
let other_handle = if *handle == inter.0 { inter.1 } else { inter.0 }; let other_handle = if *handle == inter.0 { inter.1 } else { inter.0 };
let other_parent: Option<&ColliderParent> = let other_parent = colliders
colliders.get(other_handle.0); .get(other_handle)
.and_then(|co| co.parent.as_ref());
if let Some(other_parent) = other_parent { if let Some(other_parent) = other_parent {
islands.wake_up(bodies, other_parent.handle, true); islands.wake_up(bodies, other_parent.handle, true);
@@ -420,8 +411,8 @@ impl NarrowPhase {
// to transfer their contact/intersection graph edges to the intersection/contact graph. // to transfer their contact/intersection graph edges to the intersection/contact graph.
// To achieve this we will remove the relevant contact/intersection pairs form the // To achieve this we will remove the relevant contact/intersection pairs form the
// contact/intersection graphs, and then add them into the other graph. // contact/intersection graphs, and then add them into the other graph.
if co_changes.contains(ColliderChanges::TYPE) { if co.changes.contains(ColliderChanges::TYPE) {
if co_type.is_sensor() { if co.is_sensor() {
// Find the contact pairs for this collider and // Find the contact pairs for this collider and
// push them to `pairs_to_remove`. // push them to `pairs_to_remove`.
for inter in self for inter in self
@@ -441,9 +432,7 @@ impl NarrowPhase {
.intersection_graph .intersection_graph
.interactions_with(gid.intersection_graph_index) .interactions_with(gid.intersection_graph_index)
.filter(|(h1, h2, _)| { .filter(|(h1, h2, _)| {
let co_type1: &ColliderType = colliders.index(h1.0); !colliders[*h1].is_sensor() && !colliders[*h2].is_sensor()
let co_type2: &ColliderType = colliders.index(h2.0);
!co_type1.is_sensor() && !co_type2.is_sensor()
}) })
{ {
pairs_to_remove.push(( pairs_to_remove.push((
@@ -484,10 +473,9 @@ impl NarrowPhase {
events: &dyn EventHandler, events: &dyn EventHandler,
mode: PairRemovalMode, mode: PairRemovalMode,
) { ) {
let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0); if let (Some(co1), Some(co2)) =
let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0); (colliders.get(pair.collider1), colliders.get(pair.collider2))
{
if let (Some(co_type1), Some(co_type2)) = (co_type1, co_type2) {
// TODO: could we just unwrap here? // TODO: could we just unwrap here?
// Don't we have the guarantee that we will get a `AddPair` before a `DeletePair`? // Don't we have the guarantee that we will get a `AddPair` before a `DeletePair`?
if let (Some(gid1), Some(gid2)) = ( if let (Some(gid1), Some(gid2)) = (
@@ -495,8 +483,7 @@ impl NarrowPhase {
self.graph_indices.get(pair.collider2.0), self.graph_indices.get(pair.collider2.0),
) { ) {
if mode == PairRemovalMode::FromIntersectionGraph if mode == PairRemovalMode::FromIntersectionGraph
|| (mode == PairRemovalMode::Auto || (mode == PairRemovalMode::Auto && (co1.is_sensor() || co2.is_sensor()))
&& (co_type1.is_sensor() || co_type2.is_sensor()))
{ {
let intersection = self let intersection = self
.intersection_graph .intersection_graph
@@ -505,10 +492,7 @@ impl NarrowPhase {
// Emit an intersection lost event if we had an intersection before removing the edge. // Emit an intersection lost event if we had an intersection before removing the edge.
if let Some(mut intersection) = intersection { if let Some(mut intersection) = intersection {
if intersection.intersecting { if intersection.intersecting {
let co_flag1: &ColliderFlags = colliders.index(pair.collider1.0); if (co1.flags.active_events | co2.flags.active_events)
let co_flag2: &ColliderFlags = colliders.index(pair.collider2.0);
if (co_flag1.active_events | co_flag2.active_events)
.contains(ActiveEvents::COLLISION_EVENTS) .contains(ActiveEvents::COLLISION_EVENTS)
{ {
intersection.emit_stop_event(pair.collider1, pair.collider2, events) intersection.emit_stop_event(pair.collider1, pair.collider2, events)
@@ -524,25 +508,17 @@ impl NarrowPhase {
// Also wake up the dynamic bodies that were in contact. // Also wake up the dynamic bodies that were in contact.
if let Some(mut ctct) = contact_pair { if let Some(mut ctct) = contact_pair {
if ctct.has_any_active_contact { if ctct.has_any_active_contact {
let co_parent1: Option<&ColliderParent> =
colliders.get(pair.collider1.0);
let co_parent2: Option<&ColliderParent> =
colliders.get(pair.collider2.0);
if let Some(islands) = islands { if let Some(islands) = islands {
if let Some(co_parent1) = co_parent1 { if let Some(co_parent1) = &co1.parent {
islands.wake_up(bodies, co_parent1.handle, true); islands.wake_up(bodies, co_parent1.handle, true);
} }
if let Some(co_parent2) = co_parent2 { if let Some(co_parent2) = co2.parent {
islands.wake_up(bodies, co_parent2.handle, true); islands.wake_up(bodies, co_parent2.handle, true);
} }
} }
let co_flag1: &ColliderFlags = colliders.index(pair.collider1.0); if (co1.flags.active_events | co2.flags.active_events)
let co_flag2: &ColliderFlags = colliders.index(pair.collider2.0);
if (co_flag1.active_events | co_flag2.active_events)
.contains(ActiveEvents::COLLISION_EVENTS) .contains(ActiveEvents::COLLISION_EVENTS)
{ {
ctct.emit_stop_event(events); ctct.emit_stop_event(events);
@@ -555,15 +531,11 @@ impl NarrowPhase {
} }
fn add_pair(&mut self, colliders: &ColliderSet, pair: &ColliderPair) { fn add_pair(&mut self, colliders: &ColliderSet, pair: &ColliderPair) {
let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0); if let (Some(co1), Some(co2)) =
let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0); (colliders.get(pair.collider1), colliders.get(pair.collider2))
{
if let (Some(co_type1), Some(co_type2)) = (co_type1, co_type2) { if co1.parent.map(|p| p.handle) == co2.parent.map(|p| p.handle) {
let co_parent1: Option<&ColliderParent> = colliders.get(pair.collider1.0); if co1.parent.is_some() {
let co_parent2: Option<&ColliderParent> = colliders.get(pair.collider2.0);
if co_parent1.map(|p| p.handle) == co_parent2.map(|p| p.handle) {
if co_parent1.is_some() {
// Same parents. Ignore collisions. // Same parents. Ignore collisions.
return; return;
} }
@@ -577,7 +549,7 @@ impl NarrowPhase {
ColliderGraphIndices::invalid(), ColliderGraphIndices::invalid(),
); );
if co_type1.is_sensor() || co_type2.is_sensor() { if co1.is_sensor() || co2.is_sensor() {
// NOTE: the collider won't have a graph index as long // NOTE: the collider won't have a graph index as long
// as it does not interact with anything. // as it does not interact with anything.
if !InteractionGraph::<(), ()>::is_graph_index_valid(gid1.intersection_graph_index) if !InteractionGraph::<(), ()>::is_graph_index_valid(gid1.intersection_graph_index)
@@ -682,27 +654,13 @@ impl NarrowPhase {
let handle1 = nodes[edge.source().index()].weight; let handle1 = nodes[edge.source().index()].weight;
let handle2 = nodes[edge.target().index()].weight; let handle2 = nodes[edge.target().index()].weight;
let had_intersection = edge.weight.intersecting; let had_intersection = edge.weight.intersecting;
let co1 = &colliders[handle1];
let co2 = &colliders[handle2];
// TODO: remove the `loop` once labels on blocks is stabilized. // TODO: remove the `loop` once labels on blocks is stabilized.
'emit_events: loop { 'emit_events: loop {
let co_parent1: Option<&ColliderParent> = colliders.get(handle1.0); if !co1.changes.needs_narrow_phase_update()
let (co_changes1, co_shape1, co_pos1, co_flags1): ( && !co2.changes.needs_narrow_phase_update()
&ColliderChanges,
&ColliderShape,
&ColliderPosition,
&ColliderFlags,
) = colliders.index_bundle(handle1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(handle2.0);
let (co_changes2, co_shape2, co_pos2, co_flags2): (
&ColliderChanges,
&ColliderShape,
&ColliderPosition,
&ColliderFlags,
) = colliders.index_bundle(handle2.0);
if !co_changes1.needs_narrow_phase_update()
&& !co_changes2.needs_narrow_phase_update()
{ {
// No update needed for these colliders. // No update needed for these colliders.
return; return;
@@ -712,36 +670,36 @@ impl NarrowPhase {
let mut rb_type1 = RigidBodyType::Fixed; let mut rb_type1 = RigidBodyType::Fixed;
let mut rb_type2 = RigidBodyType::Fixed; let mut rb_type2 = RigidBodyType::Fixed;
if let Some(co_parent1) = co_parent1 { if let Some(co_parent1) = &co1.parent {
rb_type1 = *bodies.index(co_parent1.handle.0); rb_type1 = bodies[co_parent1.handle].body_type;
} }
if let Some(co_parent2) = co_parent2 { if let Some(co_parent2) = &co2.parent {
rb_type2 = *bodies.index(co_parent2.handle.0); rb_type2 = bodies[co_parent2.handle].body_type;
} }
// Filter based on the rigid-body types. // Filter based on the rigid-body types.
if !co_flags1.active_collision_types.test(rb_type1, rb_type2) if !co1.flags.active_collision_types.test(rb_type1, rb_type2)
&& !co_flags2.active_collision_types.test(rb_type1, rb_type2) && !co2.flags.active_collision_types.test(rb_type1, rb_type2)
{ {
edge.weight.intersecting = false; edge.weight.intersecting = false;
break 'emit_events; break 'emit_events;
} }
// Filter based on collision groups. // Filter based on collision groups.
if !co_flags1.collision_groups.test(co_flags2.collision_groups) { if !co1.flags.collision_groups.test(co2.flags.collision_groups) {
edge.weight.intersecting = false; edge.weight.intersecting = false;
break 'emit_events; break 'emit_events;
} }
let active_hooks = co_flags1.active_hooks | co_flags2.active_hooks; let active_hooks = co1.flags.active_hooks | co2.flags.active_hooks;
if active_hooks.contains(ActiveHooks::FILTER_INTERSECTION_PAIR) { if active_hooks.contains(ActiveHooks::FILTER_INTERSECTION_PAIR) {
let context = PairFilterContext { let context = PairFilterContext {
bodies, bodies,
colliders, colliders,
rigid_body1: co_parent1.map(|p| p.handle), rigid_body1: co1.parent.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle), rigid_body2: co2.parent.map(|p| p.handle),
collider1: handle1, collider1: handle1,
collider2: handle2, collider2: handle2,
}; };
@@ -753,16 +711,14 @@ impl NarrowPhase {
} }
} }
let pos12 = co_pos1.inv_mul(co_pos2); let pos12 = co1.pos.inv_mul(&co2.pos);
edge.weight.intersecting = query_dispatcher edge.weight.intersecting = query_dispatcher
.intersection_test(&pos12, &**co_shape1, &**co_shape2) .intersection_test(&pos12, &*co1.shape, &*co2.shape)
.unwrap_or(false); .unwrap_or(false);
break 'emit_events; break 'emit_events;
} }
let co_flags1: &ColliderFlags = colliders.index(handle1.0); let active_events = co1.flags.active_events | co2.flags.active_events;
let co_flags2: &ColliderFlags = colliders.index(handle2.0);
let active_events = co_flags1.active_events | co_flags2.active_events;
if active_events.contains(ActiveEvents::COLLISION_EVENTS) if active_events.contains(ActiveEvents::COLLISION_EVENTS)
&& had_intersection != edge.weight.intersecting && had_intersection != edge.weight.intersecting
@@ -795,29 +751,13 @@ impl NarrowPhase {
par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| { par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| {
let pair = &mut edge.weight; let pair = &mut edge.weight;
let had_any_active_contact = pair.has_any_active_contact; let had_any_active_contact = pair.has_any_active_contact;
let co1 = &colliders[pair.collider1];
let co2 = &colliders[pair.collider2];
// TODO: remove the `loop` once labels on blocks are supported. // TODO: remove the `loop` once labels on blocks are supported.
'emit_events: loop { 'emit_events: loop {
let co_parent1: Option<&ColliderParent> = colliders.get(pair.collider1.0); if !co1.changes.needs_narrow_phase_update()
let (co_changes1, co_shape1, co_pos1, co_material1, co_flags1): ( && !co2.changes.needs_narrow_phase_update()
&ColliderChanges,
&ColliderShape,
&ColliderPosition,
&ColliderMaterial,
&ColliderFlags,
) = colliders.index_bundle(pair.collider1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(pair.collider2.0);
let (co_changes2, co_shape2, co_pos2, co_material2, co_flags2): (
&ColliderChanges,
&ColliderShape,
&ColliderPosition,
&ColliderMaterial,
&ColliderFlags,
) = colliders.index_bundle(pair.collider2.0);
if !co_changes1.needs_narrow_phase_update()
&& !co_changes2.needs_narrow_phase_update()
{ {
// No update needed for these colliders. // No update needed for these colliders.
return; return;
@@ -827,36 +767,36 @@ impl NarrowPhase {
let mut rb_type1 = RigidBodyType::Fixed; let mut rb_type1 = RigidBodyType::Fixed;
let mut rb_type2 = RigidBodyType::Fixed; let mut rb_type2 = RigidBodyType::Fixed;
if let Some(co_parent1) = co_parent1 { if let Some(co_parent1) = &co1.parent {
rb_type1 = *bodies.index(co_parent1.handle.0); rb_type1 = bodies[co_parent1.handle].body_type;
} }
if let Some(co_parent2) = co_parent2 { if let Some(co_parent2) = &co2.parent {
rb_type2 = *bodies.index(co_parent2.handle.0); rb_type2 = bodies[co_parent2.handle].body_type;
} }
// Filter based on the rigid-body types. // Filter based on the rigid-body types.
if !co_flags1.active_collision_types.test(rb_type1, rb_type2) if !co1.flags.active_collision_types.test(rb_type1, rb_type2)
&& !co_flags2.active_collision_types.test(rb_type1, rb_type2) && !co2.flags.active_collision_types.test(rb_type1, rb_type2)
{ {
pair.clear(); pair.clear();
break 'emit_events; break 'emit_events;
} }
// Filter based on collision groups. // Filter based on collision groups.
if !co_flags1.collision_groups.test(co_flags2.collision_groups) { if !co1.flags.collision_groups.test(co2.flags.collision_groups) {
pair.clear(); pair.clear();
break 'emit_events; break 'emit_events;
} }
let active_hooks = co_flags1.active_hooks | co_flags2.active_hooks; let active_hooks = co1.flags.active_hooks | co2.flags.active_hooks;
let mut solver_flags = if active_hooks.contains(ActiveHooks::FILTER_CONTACT_PAIRS) { let mut solver_flags = if active_hooks.contains(ActiveHooks::FILTER_CONTACT_PAIRS) {
let context = PairFilterContext { let context = PairFilterContext {
bodies, bodies,
colliders, colliders,
rigid_body1: co_parent1.map(|p| p.handle), rigid_body1: co1.parent.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle), rigid_body2: co2.parent.map(|p| p.handle),
collider1: pair.collider1, collider1: pair.collider1,
collider2: pair.collider2, collider2: pair.collider2,
}; };
@@ -872,53 +812,55 @@ impl NarrowPhase {
SolverFlags::default() SolverFlags::default()
}; };
if !co_flags1.solver_groups.test(co_flags2.solver_groups) { if !co1.flags.solver_groups.test(co2.flags.solver_groups) {
solver_flags.remove(SolverFlags::COMPUTE_IMPULSES); solver_flags.remove(SolverFlags::COMPUTE_IMPULSES);
} }
if co_changes1.contains(ColliderChanges::SHAPE) if co1.changes.contains(ColliderChanges::SHAPE)
|| co_changes2.contains(ColliderChanges::SHAPE) || co2.changes.contains(ColliderChanges::SHAPE)
{ {
// The shape changed so the workspace is no longer valid. // The shape changed so the workspace is no longer valid.
pair.workspace = None; pair.workspace = None;
} }
let pos12 = co_pos1.inv_mul(co_pos2); let pos12 = co1.pos.inv_mul(&co2.pos);
let _ = query_dispatcher.contact_manifolds( let _ = query_dispatcher.contact_manifolds(
&pos12, &pos12,
&**co_shape1, &*co1.shape,
&**co_shape2, &*co2.shape,
prediction_distance, prediction_distance,
&mut pair.manifolds, &mut pair.manifolds,
&mut pair.workspace, &mut pair.workspace,
); );
let friction = CoefficientCombineRule::combine( let friction = CoefficientCombineRule::combine(
co_material1.friction, co1.material.friction,
co_material2.friction, co2.material.friction,
co_material1.friction_combine_rule as u8, co1.material.friction_combine_rule as u8,
co_material2.friction_combine_rule as u8, co2.material.friction_combine_rule as u8,
); );
let restitution = CoefficientCombineRule::combine( let restitution = CoefficientCombineRule::combine(
co_material1.restitution, co1.material.restitution,
co_material2.restitution, co2.material.restitution,
co_material1.restitution_combine_rule as u8, co1.material.restitution_combine_rule as u8,
co_material2.restitution_combine_rule as u8, co2.material.restitution_combine_rule as u8,
); );
let zero = RigidBodyDominance(0); // The value doesn't matter, it will be MAX because of the effective groups. let zero = RigidBodyDominance(0); // The value doesn't matter, it will be MAX because of the effective groups.
let dominance1 = co_parent1 let dominance1 = co1
.map(|p1| *bodies.index(p1.handle.0)) .parent
.map(|p1| bodies[p1.handle].dominance)
.unwrap_or(zero); .unwrap_or(zero);
let dominance2 = co_parent2 let dominance2 = co2
.map(|p2| *bodies.index(p2.handle.0)) .parent
.map(|p2| bodies[p2.handle].dominance)
.unwrap_or(zero); .unwrap_or(zero);
for manifold in &mut pair.manifolds { for manifold in &mut pair.manifolds {
let world_pos1 = manifold.subshape_pos1.prepend_to(co_pos1); let world_pos1 = manifold.subshape_pos1.prepend_to(&co1.pos);
manifold.data.solver_contacts.clear(); manifold.data.solver_contacts.clear();
manifold.data.rigid_body1 = co_parent1.map(|p| p.handle); manifold.data.rigid_body1 = co1.parent.map(|p| p.handle);
manifold.data.rigid_body2 = co_parent2.map(|p| p.handle); manifold.data.rigid_body2 = co2.parent.map(|p| p.handle);
manifold.data.solver_flags = solver_flags; manifold.data.solver_flags = solver_flags;
manifold.data.relative_dominance = dominance1.effective_group(&rb_type1) manifold.data.relative_dominance = dominance1.effective_group(&rb_type1)
- dominance2.effective_group(&rb_type2); - dominance2.effective_group(&rb_type2);
@@ -960,8 +902,8 @@ impl NarrowPhase {
let mut context = ContactModificationContext { let mut context = ContactModificationContext {
bodies, bodies,
colliders, colliders,
rigid_body1: co_parent1.map(|p| p.handle), rigid_body1: co1.parent.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle), rigid_body2: co2.parent.map(|p| p.handle),
collider1: pair.collider1, collider1: pair.collider1,
collider2: pair.collider2, collider2: pair.collider2,
manifold, manifold,
@@ -981,9 +923,7 @@ impl NarrowPhase {
break 'emit_events; break 'emit_events;
} }
let co_flags1: &ColliderFlags = colliders.index(pair.collider1.0); let active_events = co1.flags.active_events | co2.flags.active_events;
let co_flags2: &ColliderFlags = colliders.index(pair.collider2.0);
let active_events = co_flags1.active_events | co_flags2.active_events;
if pair.has_any_active_contact != had_any_active_contact { if pair.has_any_active_contact != had_any_active_contact {
if active_events.contains(ActiveEvents::COLLISION_EVENTS) { if active_events.contains(ActiveEvents::COLLISION_EVENTS) {
@@ -1021,18 +961,24 @@ impl NarrowPhase {
{ {
let (active_island_id1, rb_type1, sleeping1) = let (active_island_id1, rb_type1, sleeping1) =
if let Some(handle1) = manifold.data.rigid_body1 { if let Some(handle1) = manifold.data.rigid_body1 {
let data: (&RigidBodyIds, &RigidBodyType, &RigidBodyActivation) = let rb1 = &bodies[handle1];
bodies.index_bundle(handle1.0); (
(data.0.active_island_id, *data.1, data.2.sleeping) rb1.ids.active_island_id,
rb1.body_type,
rb1.activation.sleeping,
)
} else { } else {
(0, RigidBodyType::Fixed, true) (0, RigidBodyType::Fixed, true)
}; };
let (active_island_id2, rb_type2, sleeping2) = let (active_island_id2, rb_type2, sleeping2) =
if let Some(handle2) = manifold.data.rigid_body2 { if let Some(handle2) = manifold.data.rigid_body2 {
let data: (&RigidBodyIds, &RigidBodyType, &RigidBodyActivation) = let rb2 = &bodies[handle2];
bodies.index_bundle(handle2.0); (
(data.0.active_island_id, *data.1, data.2.sleeping) rb2.ids.active_island_id,
rb2.body_type,
rb2.activation.sleeping,
)
} else { } else {
(0, RigidBodyType::Fixed, true) (0, RigidBodyType::Fixed, true)
}; };

2
src/pipeline/collision_pipeline.rs
View File

@@ -97,7 +97,7 @@ impl CollisionPipeline {
modified_colliders: &mut Vec<ColliderHandle>, modified_colliders: &mut Vec<ColliderHandle>,
) { ) {
for handle in modified_colliders.drain(..) { for handle in modified_colliders.drain(..) {
colliders.set_internal(handle.0, ColliderChanges::empty()) colliders.index_mut_internal(handle).changes = ColliderChanges::empty();
} }
} }

58
src/pipeline/physics_pipeline.rs
View File

@@ -5,8 +5,7 @@ use crate::counters::Counters;
use crate::dynamics::IslandSolver; use crate::dynamics::IslandSolver;
use crate::dynamics::{ use crate::dynamics::{
CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
RigidBodyColliders, RigidBodyForces, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyHandle, RigidBodyPosition, RigidBodyType,
RigidBodyType, RigidBodyVelocity,
}; };
#[cfg(feature = "parallel")] #[cfg(feature = "parallel")]
use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
@@ -72,7 +71,9 @@ impl PhysicsPipeline {
modified_colliders: &mut Vec<ColliderHandle>, modified_colliders: &mut Vec<ColliderHandle>,
) { ) {
for handle in modified_colliders.drain(..) { for handle in modified_colliders.drain(..) {
colliders.set_internal(handle.0, ColliderChanges::empty()) if let Some(co) = colliders.get_mut_internal(handle) {
co.changes = ColliderChanges::empty();
}
} }
} }
@@ -187,18 +188,11 @@ impl PhysicsPipeline {
self.counters.stages.update_time.resume(); self.counters.stages.update_time.resume();
for handle in islands.active_dynamic_bodies() { for handle in islands.active_dynamic_bodies() {
let poss: &RigidBodyPosition = bodies.index(handle.0); let rb = bodies.index_mut_internal(*handle);
let position = poss.position; rb.mprops.update_world_mass_properties(&rb.pos.position);
let effective_mass = rb.mprops.effective_mass();
let effective_inv_mass = bodies rb.forces
.map_mut_internal(handle.0, |mprops: &mut RigidBodyMassProps| { .compute_effective_force_and_torque(&gravity, &effective_mass);
mprops.update_world_mass_properties(&position);
mprops.effective_mass()
})
.unwrap();
bodies.map_mut_internal(handle.0, |forces: &mut RigidBodyForces| {
forces.compute_effective_force_and_torque(&gravity, &effective_inv_mass)
});
} }
for multibody in &mut multibody_joints.multibodies { for multibody in &mut multibody_joints.multibodies {
@@ -319,13 +313,10 @@ impl PhysicsPipeline {
) { ) {
// Set the rigid-bodies and kinematic bodies to their final position. // Set the rigid-bodies and kinematic bodies to their final position.
for handle in islands.iter_active_bodies() { for handle in islands.iter_active_bodies() {
bodies.map_mut_internal(handle.0, |poss: &mut RigidBodyPosition| { let rb = bodies.index_mut_internal(handle);
poss.position = poss.next_position rb.pos.position = rb.pos.next_position;
}); rb.colliders
.update_positions(colliders, modified_colliders, &rb.pos.position);
let (rb_poss, rb_colls): (&RigidBodyPosition, &RigidBodyColliders) =
bodies.index_bundle(handle.0);
rb_colls.update_positions(colliders, modified_colliders, &rb_poss.position);
} }
} }
@@ -341,29 +332,22 @@ impl PhysicsPipeline {
// there to determine if this kinematic body should wake-up dynamic // there to determine if this kinematic body should wake-up dynamic
// bodies it is touching. // bodies it is touching.
for handle in islands.active_kinematic_bodies() { for handle in islands.active_kinematic_bodies() {
let (rb_type, rb_pos, rb_vel, rb_mprops): ( let rb = bodies.index_mut_internal(*handle);
&RigidBodyType,
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
match rb_type { match rb.body_type {
RigidBodyType::KinematicPositionBased => { RigidBodyType::KinematicPositionBased => {
let rb_pos: &RigidBodyPosition = bodies.index(handle.0); rb.vels = rb.pos.interpolate_velocity(
let new_vel = rb_pos.interpolate_velocity(
integration_parameters.inv_dt(), integration_parameters.inv_dt(),
&rb_mprops.local_mprops.local_com, &rb.mprops.local_mprops.local_com,
); );
bodies.set_internal(handle.0, new_vel);
} }
RigidBodyType::KinematicVelocityBased => { RigidBodyType::KinematicVelocityBased => {
let new_pos = rb_vel.integrate( let new_pos = rb.vels.integrate(
integration_parameters.dt, integration_parameters.dt,
&rb_pos.position, &rb.pos.position,
&rb_mprops.local_mprops.local_com, &rb.mprops.local_mprops.local_com,
); );
bodies.set_internal(handle.0, RigidBodyPosition::from(new_pos)); rb.pos = RigidBodyPosition::from(new_pos);
} }
_ => {} _ => {}
} }

133
src/pipeline/query_pipeline.rs
View File

@@ -1,10 +1,6 @@
use crate::dynamics::{ use crate::dynamics::IslandManager;
IslandManager, RigidBodyColliders, RigidBodyForces, RigidBodyMassProps, RigidBodyPosition,
RigidBodyVelocity,
};
use crate::geometry::{ use crate::geometry::{
ColliderFlags, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderHandle, InteractionGroups, PointProjection, Ray, RayIntersection, AABB, QBVH,
InteractionGroups, PointProjection, Ray, RayIntersection, AABB, QBVH,
}; };
use crate::math::{Isometry, Point, Real, Vector}; use crate::math::{Isometry, Point, Real, Vector};
use parry::partitioning::QBVHDataGenerator; use parry::partitioning::QBVHDataGenerator;
@@ -75,7 +71,7 @@ impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
if co.flags.collision_groups.test(self.query_groups) if co.flags.collision_groups.test(self.query_groups)
&& self.filter.map(|f| f(shape_id)).unwrap_or(true) && self.filter.map(|f| f(shape_id)).unwrap_or(true)
{ {
f(Some(co.pos), &**co.shape) f(Some(&co.pos), &*co.shape)
} }
} }
} }
@@ -189,54 +185,35 @@ impl QueryPipeline {
fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, AABB)) { fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, AABB)) {
match self.mode { match self.mode {
QueryPipelineMode::CurrentPosition => { QueryPipelineMode::CurrentPosition => {
self.colliders.for_each(|h, co_shape: &ColliderShape| { for (h, co) in self.colliders.iter() {
let co_pos: &ColliderPosition = self.colliders.index(h); f(h, co.shape.compute_aabb(&co.pos))
f(ColliderHandle(h), co_shape.compute_aabb(&co_pos)) }
})
} }
QueryPipelineMode::SweepTestWithNextPosition => { QueryPipelineMode::SweepTestWithNextPosition => {
self.colliders.for_each(|h, co_shape: &ColliderShape| { for (h, co) in self.colliders.iter() {
let co_parent: Option<&ColliderParent> = self.colliders.get(h); if let Some(co_parent) = co.parent {
let co_pos: &ColliderPosition = self.colliders.index(h); let rb_next_pos = &self.bodies[co_parent.handle].pos.next_position;
let next_position = rb_next_pos * co_parent.pos_wrt_parent;
if let Some(co_parent) = co_parent { f(h, co.shape.compute_swept_aabb(&co.pos, &next_position))
let rb_pos: &RigidBodyPosition =
self.bodies.index(co_parent.handle.0);
let next_position = rb_pos.next_position * co_parent.pos_wrt_parent;
f(
ColliderHandle(h),
co_shape.compute_swept_aabb(&co_pos, &next_position),
)
} else { } else {
f(ColliderHandle(h), co_shape.compute_aabb(&co_pos)) f(h, co.shape.compute_aabb(&co.pos))
} }
}) }
} }
QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
self.colliders.for_each(|h, co_shape: &ColliderShape| { for (h, co) in self.colliders.iter() {
let co_parent: Option<&ColliderParent> = self.colliders.get(h); if let Some(co_parent) = co.parent {
let co_pos: &ColliderPosition = self.colliders.index(h); let rb = &self.bodies[co_parent.handle];
let predicted_pos = rb.pos.integrate_forces_and_velocities(
if let Some(co_parent) = co_parent { dt, &rb.forces, &rb.vels, &rb.mprops,
let (rb_pos, vels, forces, mprops): ( );
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyForces,
&RigidBodyMassProps,
) = self.bodies.index_bundle(co_parent.handle.0);
let predicted_pos = rb_pos
.integrate_forces_and_velocities(dt, forces, vels, mprops);
let next_position = predicted_pos * co_parent.pos_wrt_parent; let next_position = predicted_pos * co_parent.pos_wrt_parent;
f( f(h, co.shape.compute_swept_aabb(&co.pos, &next_position))
ColliderHandle(h),
co_shape.compute_swept_aabb(&co_pos, &next_position),
)
} else { } else {
f(ColliderHandle(h), co_shape.compute_aabb(&co_pos)) f(h, co.shape.compute_aabb(&co.pos))
} }
}) }
} }
} }
} }
@@ -256,8 +233,8 @@ impl QueryPipeline {
} }
for handle in islands.iter_active_bodies() { for handle in islands.iter_active_bodies() {
let body_colliders: &RigidBodyColliders = bodies.index(handle.0); let rb = &bodies[handle];
for handle in &body_colliders.0 { for handle in &rb.colliders.0 {
self.qbvh.pre_update(*handle) self.qbvh.pre_update(*handle)
} }
} }
@@ -266,9 +243,8 @@ impl QueryPipeline {
QueryPipelineMode::CurrentPosition => { QueryPipelineMode::CurrentPosition => {
self.qbvh.update( self.qbvh.update(
|handle| { |handle| {
let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) = let co = &colliders[*handle];
colliders.index_bundle(handle.0); co.shape.compute_aabb(&co.pos)
co_shape.compute_aabb(&co_pos)
}, },
self.dilation_factor, self.dilation_factor,
); );
@@ -276,10 +252,10 @@ impl QueryPipeline {
QueryPipelineMode::SweepTestWithNextPosition => { QueryPipelineMode::SweepTestWithNextPosition => {
self.qbvh.update( self.qbvh.update(
|handle| { |handle| {
let co = &colliders[handle]; let co = &colliders[*handle];
if let Some(parent) = co.parent { if let Some(parent) = &co.parent {
let rb_pos: &RigidBodyPosition = bodies.index(co.parent.handle.0); let rb_next_pos = &bodies[parent.handle].pos.next_position;
let next_position = rb_pos.next_position * co.parent.pos_wrt_parent; let next_position = rb_next_pos * parent.pos_wrt_parent;
co.shape.compute_swept_aabb(&co.pos, &next_position) co.shape.compute_swept_aabb(&co.pos, &next_position)
} else { } else {
co.shape.compute_aabb(&co.pos) co.shape.compute_aabb(&co.pos)
@@ -291,12 +267,12 @@ impl QueryPipeline {
QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
self.qbvh.update( self.qbvh.update(
|handle| { |handle| {
let co = &colliders[handle]; let co = &colliders[*handle];
if let Some(parent) = co.parent { if let Some(parent) = co.parent {
let rb = &bodies[parent.handle]; let rb = &bodies[parent.handle];
let predicted_pos = rb let predicted_pos = rb.pos.integrate_forces_and_velocities(
.pos dt, &rb.forces, &rb.vels, &rb.mprops,
.integrate_forces_and_velocities(dt, rb.forces, rb.vels, rb.mprops); );
let next_position = predicted_pos * parent.pos_wrt_parent; let next_position = predicted_pos * parent.pos_wrt_parent;
co.shape.compute_swept_aabb(&co.pos, &next_position) co.shape.compute_swept_aabb(&co.pos, &next_position)
@@ -394,7 +370,7 @@ impl QueryPipeline {
/// * `callback`: function executed on each collider for which a ray intersection has been found. /// * `callback`: function executed on each collider for which a ray intersection has been found.
/// There is no guarantees on the order the results will be yielded. If this callback returns `false`, /// There is no guarantees on the order the results will be yielded. If this callback returns `false`,
/// this method will exit early, ignore any further raycast. /// this method will exit early, ignore any further raycast.
pub fn intersections_with_ray<'a, ColliderSet>( pub fn intersections_with_ray<'a>(
&self, &self,
colliders: &'a ColliderSet, colliders: &'a ColliderSet,
ray: &Ray, ray: &Ray,
@@ -405,14 +381,13 @@ impl QueryPipeline {
mut callback: impl FnMut(ColliderHandle, RayIntersection) -> bool, mut callback: impl FnMut(ColliderHandle, RayIntersection) -> bool,
) { ) {
let mut leaf_callback = &mut |handle: &ColliderHandle| { let mut leaf_callback = &mut |handle: &ColliderHandle| {
let co_shape: Option<&ColliderShape> = colliders.get(handle.0); if let Some(co) = colliders.get(*handle) {
if let Some(co_shape) = co_shape { if co.flags.collision_groups.test(query_groups)
let (co_flags, co_pos): (&ColliderFlags, &ColliderPosition) =
colliders.index_bundle(handle.0);
if co_flags.collision_groups.test(query_groups)
&& filter.map(|f| f(*handle)).unwrap_or(true) && filter.map(|f| f(*handle)).unwrap_or(true)
{ {
if let Some(hit) = co_shape.cast_ray_and_get_normal(co_pos, ray, max_toi, solid) if let Some(hit) = co
.shape
.cast_ray_and_get_normal(&co.pos, ray, max_toi, solid)
{ {
return callback(*handle, hit); return callback(*handle, hit);
} }
@@ -502,7 +477,7 @@ impl QueryPipeline {
/// is either `None` or returns `true`. /// is either `None` or returns `true`.
/// * `callback` - A function called with each collider with a shape /// * `callback` - A function called with each collider with a shape
/// containing the `point`. /// containing the `point`.
pub fn intersections_with_point<'a, ColliderSet>( pub fn intersections_with_point<'a>(
&self, &self,
colliders: &'a ColliderSet, colliders: &'a ColliderSet,
point: &Point<Real>, point: &Point<Real>,
@@ -511,15 +486,10 @@ impl QueryPipeline {
mut callback: impl FnMut(ColliderHandle) -> bool, mut callback: impl FnMut(ColliderHandle) -> bool,
) { ) {
let mut leaf_callback = &mut |handle: &ColliderHandle| { let mut leaf_callback = &mut |handle: &ColliderHandle| {
let co_shape: Option<&ColliderShape> = colliders.get(handle.0); if let Some(co) = colliders.get(*handle) {
if co.flags.collision_groups.test(query_groups)
if let Some(co_shape) = co_shape {
let (co_flags, co_pos): (&ColliderFlags, &ColliderPosition) =
colliders.index_bundle(handle.0);
if co_flags.collision_groups.test(query_groups)
&& filter.map(|f| f(*handle)).unwrap_or(true) && filter.map(|f| f(*handle)).unwrap_or(true)
&& co_shape.contains_point(co_pos, point) && co.shape.contains_point(&co.pos, point)
{ {
return callback(*handle); return callback(*handle);
} }
@@ -677,7 +647,7 @@ impl QueryPipeline {
/// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`. /// is either `None` or returns `true`.
/// * `callback` - A function called with the handles of each collider intersecting the `shape`. /// * `callback` - A function called with the handles of each collider intersecting the `shape`.
pub fn intersections_with_shape<'a, ColliderSet>( pub fn intersections_with_shape<'a>(
&self, &self,
colliders: &'a ColliderSet, colliders: &'a ColliderSet,
shape_pos: &Isometry<Real>, shape_pos: &Isometry<Real>,
@@ -690,18 +660,13 @@ impl QueryPipeline {
let inv_shape_pos = shape_pos.inverse(); let inv_shape_pos = shape_pos.inverse();
let mut leaf_callback = &mut |handle: &ColliderHandle| { let mut leaf_callback = &mut |handle: &ColliderHandle| {
let co_shape: Option<&ColliderShape> = colliders.get(handle.0); if let Some(co) = colliders.get(*handle) {
if co.flags.collision_groups.test(query_groups)
if let Some(co_shape) = co_shape {
let (co_flags, co_pos): (&ColliderFlags, &ColliderPosition) =
colliders.index_bundle(handle.0);
if co_flags.collision_groups.test(query_groups)
&& filter.map(|f| f(*handle)).unwrap_or(true) && filter.map(|f| f(*handle)).unwrap_or(true)
{ {
let pos12 = inv_shape_pos * co_pos.as_ref(); let pos12 = inv_shape_pos * co.pos.as_ref();
if dispatcher.intersection_test(&pos12, shape, &**co_shape) == Ok(true) { if dispatcher.intersection_test(&pos12, shape, &*co.shape) == Ok(true) {
return callback(*handle); return callback(*handle);
} }
} }

57
src/pipeline/user_changes.rs
View File

@@ -1,6 +1,6 @@
use crate::dynamics::{ use crate::dynamics::{
IslandManager, RigidBodyActivation, RigidBodyChanges, RigidBodyHandle, RigidBodyIds, IslandManager, RigidBodyActivation, RigidBodyChanges, RigidBodyHandle, RigidBodyIds,
RigidBodyPosition, RigidBodySet, RigidBodyType, RigidBodySet, RigidBodyType,
}; };
use crate::geometry::{ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet}; use crate::geometry::{ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet};
use parry::utils::hashmap::HashMap; use parry::utils::hashmap::HashMap;
@@ -17,15 +17,13 @@ pub(crate) fn handle_user_changes_to_colliders(
for handle in modified_colliders { for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer // NOTE: we use `get` because the collider may no longer
// exist if it has been removed. // exist if it has been removed.
if let Some(co) = colliders.get(*handle) { if let Some(co) = colliders.get_mut_internal(*handle) {
if co.changes.contains(ColliderChanges::PARENT) { if co.changes.contains(ColliderChanges::PARENT) {
if let Some(co_parent) = co.parent { if let Some(co_parent) = co.parent {
let parent_pos: &RigidBodyPosition = bodies.index(co_parent.handle.0); let parent_rb = &bodies[co_parent.handle];
let new_pos = parent_pos.position * co_parent.pos_wrt_parent; co.pos = ColliderPosition(parent_rb.pos.position * co_parent.pos_wrt_parent);
let new_changes = co.changes | ColliderChanges::POSITION; co.changes |= ColliderChanges::POSITION;
colliders.set_internal(handle.0, ColliderPosition(new_pos));
colliders.set_internal(handle.0, new_changes);
} }
} }
@@ -38,18 +36,12 @@ pub(crate) fn handle_user_changes_to_colliders(
} }
for (to_update, _) in mprops_to_update { for (to_update, _) in mprops_to_update {
let rb = &bodies[to_update]; let rb = bodies.index_mut_internal(to_update);
let position = rb.position(); rb.mprops.recompute_mass_properties_from_colliders(
// FIXME: remove the clone once we remove the ComponentSets. colliders,
let attached_colliders = rb.colliders().clone(); &rb.colliders,
&rb.pos.position,
bodies.map_mut_internal(to_update.0, |rb_mprops| { );
rb_mprops.recompute_mass_properties_from_colliders(
colliders,
&attached_colliders,
&position,
)
});
} }
} }
@@ -73,7 +65,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
continue; continue;
} }
let rb = &bodies[handle]; let rb = bodies.index_mut_internal(*handle);
let mut changes = rb.changes; let mut changes = rb.changes;
let mut ids: RigidBodyIds = rb.ids; let mut ids: RigidBodyIds = rb.ids;
let mut activation: RigidBodyActivation = rb.activation; let mut activation: RigidBodyActivation = rb.activation;
@@ -83,7 +75,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
// it is on the correct active set. // it is on the correct active set.
if let Some(islands) = islands.as_deref_mut() { if let Some(islands) = islands.as_deref_mut() {
if changes.contains(RigidBodyChanges::TYPE) { if changes.contains(RigidBodyChanges::TYPE) {
match rb.status { match rb.body_type {
RigidBodyType::Dynamic => { RigidBodyType::Dynamic => {
// Remove from the active kinematic set if it was there. // Remove from the active kinematic set if it was there.
if islands.active_kinematic_set.get(ids.active_set_id) == Some(handle) { if islands.active_kinematic_set.get(ids.active_set_id) == Some(handle) {
@@ -162,20 +154,19 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
|| changes.contains(RigidBodyChanges::TYPE) || changes.contains(RigidBodyChanges::TYPE)
{ {
for handle in rb.colliders.0.iter() { for handle in rb.colliders.0.iter() {
colliders.map_mut_internal(handle.0, |co_changes: &mut ColliderChanges| { let co = colliders.index_mut_internal(*handle);
if !co_changes.contains(ColliderChanges::MODIFIED) { if !co.changes.contains(ColliderChanges::MODIFIED) {
modified_colliders.push(*handle); modified_colliders.push(*handle);
} }
*co_changes |= co.changes |=
ColliderChanges::MODIFIED | ColliderChanges::PARENT_EFFECTIVE_DOMINANCE; ColliderChanges::MODIFIED | ColliderChanges::PARENT_EFFECTIVE_DOMINANCE;
});
} }
} }
bodies.set_internal(handle.0, RigidBodyChanges::empty()); rb.changes = RigidBodyChanges::empty();
bodies.set_internal(handle.0, ids); rb.ids = ids;
bodies.set_internal(handle.0, activation); rb.activation = activation;
} }
// Adjust some ids, if needed. // Adjust some ids, if needed.
@@ -187,9 +178,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
}; };
if id < active_set.len() { if id < active_set.len() {
bodies.map_mut_internal(active_set[id].0, |ids2: &mut RigidBodyIds| { bodies.index_mut_internal(active_set[id]).ids.active_set_id = id;
ids2.active_set_id = id;
});
} }
} }
} }

2
src_testbed/harness/mod.rs
View File

@@ -156,7 +156,7 @@ impl Harness {
impulse_joints: ImpulseJointSet, impulse_joints: ImpulseJointSet,
multibody_joints: MultibodyJointSet, multibody_joints: MultibodyJointSet,
gravity: Vector<Real>, gravity: Vector<Real>,
hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static, hooks: impl PhysicsHooks + 'static,
) { ) {
// println!("Num bodies: {}", bodies.len()); // println!("Num bodies: {}", bodies.len());
// println!("Num impulse_joints: {}", impulse_joints.len()); // println!("Num impulse_joints: {}", impulse_joints.len());

2
src_testbed/physics/mod.rs
View File

@@ -83,7 +83,7 @@ pub struct PhysicsState {
pub query_pipeline: QueryPipeline, pub query_pipeline: QueryPipeline,
pub integration_parameters: IntegrationParameters, pub integration_parameters: IntegrationParameters,
pub gravity: Vector<Real>, pub gravity: Vector<Real>,
pub hooks: Box<dyn PhysicsHooks<RigidBodySet, ColliderSet>>, pub hooks: Box<dyn PhysicsHooks>,
} }
impl PhysicsState { impl PhysicsState {

2
src_testbed/testbed.rs
View File

@@ -493,7 +493,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
impulse_joints: ImpulseJointSet, impulse_joints: ImpulseJointSet,
multibody_joints: MultibodyJointSet, multibody_joints: MultibodyJointSet,
gravity: Vector<Real>, gravity: Vector<Real>,
hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static, hooks: impl PhysicsHooks + 'static,
) { ) {
self.harness.set_world_with_params( self.harness.set_world_with_params(
bodies, bodies,