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" }
#kiss3d = { path = "../kiss3d" }
parry2d = { path = "../parry/crates/parry2d" }
parry3d = { path = "../parry/crates/parry3d" }
parry2d-f64 = { path = "../parry/crates/parry2d-f64" }
parry3d-f64 = { path = "../parry/crates/parry3d-f64" }
#parry2d = { path = "../parry/crates/parry2d" }
#parry3d = { path = "../parry/crates/parry3d" }
#parry2d-f64 = { path = "../parry/crates/parry2d-f64" }
#parry3d-f64 = { path = "../parry/crates/parry3d-f64" }
# nalgebra = { path = "../nalgebra" }
#kiss3d = { git = "https://github.com/sebcrozet/kiss3d" }
#nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" }
#parry2d = { git = "https://github.com/dimforge/parry", branch = "special_cases" }
#parry3d = { git = "https://github.com/dimforge/parry", branch = "special_cases" }
#parry2d-f64 = { git = "https://github.com/dimforge/parry", branch = "special_cases" }
#parry3d-f64 = { 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 = "split-and-qbvh" }
parry2d-f64 = { git = "https://github.com/dimforge/parry", branch = "split-and-qbvh" }
parry3d-f64 = { git = "https://github.com/dimforge/parry", branch = "split-and-qbvh" }
[profile.release]
#debug = true

7
examples2d/one_way_platforms2.rs
View File

@@ -6,11 +6,8 @@ struct OneWayPlatformHook {
platform2: ColliderHandle,
}
impl PhysicsHooks<RigidBodySet, ColliderSet> for OneWayPlatformHook {
fn modify_solver_contacts(
&self,
context: &mut ContactModificationContext<RigidBodySet, ColliderSet>,
) {
impl PhysicsHooks for OneWayPlatformHook {
fn modify_solver_contacts(&self, context: &mut ContactModificationContext) {
// 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.
//

7
examples3d/one_way_platforms3.rs
View File

@@ -6,11 +6,8 @@ struct OneWayPlatformHook {
platform2: ColliderHandle,
}
impl PhysicsHooks<RigidBodySet, ColliderSet> for OneWayPlatformHook {
fn modify_solver_contacts(
&self,
context: &mut ContactModificationContext<RigidBodySet, ColliderSet>,
) {
impl PhysicsHooks for OneWayPlatformHook {
fn modify_solver_contacts(&self, context: &mut ContactModificationContext) {
// 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.
//

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

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

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

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

80
src/dynamics/island_manager.rs
View File

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

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

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

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

23
src/dynamics/rigid_body_components.rs
View File

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

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

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

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

@@ -3,7 +3,6 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[cfg(feature = "simd-is-enabled")]
use {
crate::data::BundleSet,
crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
vec_map::VecMap,
};
@@ -90,14 +89,8 @@ impl ParallelInteractionGroups {
.zip(self.interaction_colors.iter_mut())
{
let mut body_pair = interactions[*interaction_id].body_pair();
let is_fixed1 = body_pair
.0
.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 is_fixed1 = body_pair.0.map(|b| bodies[b].is_fixed()).unwrap_or(true);
let is_fixed2 = body_pair.1.map(|b| bodies[b].is_fixed()).unwrap_or(true);
let representative = |handle: RigidBodyHandle| {
if let Some(link) = multibodies.rigid_body_link(handle).copied() {
@@ -119,28 +112,28 @@ impl ParallelInteractionGroups {
match (is_fixed1, is_fixed2) {
(false, false) => {
let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0);
let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0);
let rb1 = &bodies[body_pair.0.unwrap()];
let rb2 = &bodies[body_pair.1.unwrap()];
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_len[*color] += 1;
bcolors[rb_ids1.active_set_offset] |= 1 << *color;
bcolors[rb_ids2.active_set_offset] |= 1 << *color;
bcolors[rb1.ids.active_set_offset] |= 1 << *color;
bcolors[rb2.ids.active_set_offset] |= 1 << *color;
}
(true, false) => {
let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0);
let color_mask = bcolors[rb_ids2.active_set_offset];
let rb2 = &bodies[body_pair.1.unwrap()];
let color_mask = bcolors[rb2.ids.active_set_offset];
*color = 127 - (!color_mask).leading_zeros() as usize;
color_len[*color] += 1;
bcolors[rb_ids2.active_set_offset] |= 1 << *color;
bcolors[rb2.ids.active_set_offset] |= 1 << *color;
}
(false, true) => {
let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0);
let color_mask = bcolors[rb_ids1.active_set_offset];
let rb1 = &bodies[body_pair.0.unwrap()];
let color_mask = bcolors[rb1.ids.active_set_offset];
*color = 127 - (!color_mask).leading_zeros() as usize;
color_len[*color] += 1;
bcolors[rb_ids1.active_set_offset] |= 1 << *color;
bcolors[rb1.ids.active_set_offset] |= 1 << *color;
}
(true, true) => unreachable!(),
}
@@ -258,13 +251,11 @@ impl InteractionGroups {
for interaction_i in interaction_indices {
let interaction = &interactions[*interaction_i].weight;
let (status1, ids1): (&RigidBodyType, &RigidBodyIds) =
bodies.index_bundle(interaction.body1.0);
let (status2, ids2): (&RigidBodyType, &RigidBodyIds) =
bodies.index_bundle(interaction.body2.0);
let rb1 = &bodies[interaction.body1];
let rb2 = &bodies[interaction.body2];
let is_fixed1 = !status1.is_dynamic();
let is_fixed2 = !status2.is_dynamic();
let is_fixed1 = !rb1.is_dynamic();
let is_fixed2 = !rb2.is_dynamic();
if is_fixed1 && is_fixed2 {
continue;
@@ -277,8 +268,8 @@ impl InteractionGroups {
}
let ijoint = interaction.data.locked_axes.bits() as usize;
let i1 = ids1.active_set_offset;
let i2 = ids2.active_set_offset;
let i1 = rb1.ids.active_set_offset;
let i2 = rb2.ids.active_set_offset;
let conflicts =
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.
@@ -421,15 +412,15 @@ impl InteractionGroups {
let (status1, active_set_offset1) = if let Some(rb1) = interaction.data.rigid_body1
{
let data: (_, &RigidBodyIds) = bodies.index_bundle(rb1.0);
(*data.0, data.1.active_set_offset)
let rb1 = &bodies[rb1];
(rb1.body_type, rb1.ids.active_set_offset)
} else {
(RigidBodyType::Fixed, usize::MAX)
};
let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2
{
let data: (_, &RigidBodyIds) = bodies.index_bundle(rb2.0);
(*data.0, data.1.active_set_offset)
let rb2 = &bodies[rb2];
(rb2.body_type, rb2.ids.active_set_offset)
} else {
(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::{
ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyType, RigidBodyVelocity,
ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
};
use crate::math::{Real, SPATIAL_DIM};
use crate::prelude::MultibodyJointSet;
@@ -63,39 +62,26 @@ impl AnyJointVelocityConstraint {
) {
let local_frame1 = joint.data.local_frame1;
let local_frame2 = joint.data.local_frame2;
let rb1: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&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 rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
let frame1 = rb1.pos.position * local_frame1;
let frame2 = rb2.pos.position * local_frame2;
let body1 = SolverBody {
linvel: rb_vel1.linvel,
angvel: rb_vel1.angvel,
im: rb_mprops1.effective_inv_mass,
sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt,
world_com: rb_mprops1.world_com,
mj_lambda: [rb_ids1.active_set_offset],
linvel: rb1.vels.linvel,
angvel: rb1.vels.angvel,
im: rb1.mprops.effective_inv_mass,
sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
world_com: rb1.mprops.world_com,
mj_lambda: [rb1.ids.active_set_offset],
};
let body2 = SolverBody {
linvel: rb_vel2.linvel,
angvel: rb_vel2.angvel,
im: rb_mprops2.effective_inv_mass,
sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt,
world_com: rb_mprops2.world_com,
mj_lambda: [rb_ids2.active_set_offset],
linvel: rb2.vels.linvel,
angvel: rb2.vels.angvel,
im: rb2.mprops.effective_inv_mass,
sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
world_com: rb2.mprops.world_com,
mj_lambda: [rb2.ids.active_set_offset],
};
let mb1 = multibodies
@@ -186,16 +172,20 @@ impl AnyJointVelocityConstraint {
out: &mut Vec<Self>,
insert_at: Option<usize>,
) {
use crate::dynamics::{
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
let rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
) = (
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
gather![|ii| &bodies[impulse_joints[ii].body1].pos],
gather![|ii| &bodies[impulse_joints[ii].body1].vels],
gather![|ii| &bodies[impulse_joints[ii].body1].mprops],
gather![|ii| &bodies[impulse_joints[ii].body1].ids],
);
let rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
@@ -203,10 +193,10 @@ impl AnyJointVelocityConstraint {
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
) = (
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
gather![|ii| &bodies[impulse_joints[ii].body2].pos],
gather![|ii| &bodies[impulse_joints[ii].body2].vels],
gather![|ii| &bodies[impulse_joints[ii].body2].mprops],
gather![|ii| &bodies[impulse_joints[ii].body2].ids],
);
let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rbs1;
@@ -276,8 +266,7 @@ impl AnyJointVelocityConstraint {
) {
let mut handle1 = joint.body1;
let mut handle2 = joint.body2;
let status2: &RigidBodyType = bodies.index(handle2.0);
let flipped = !status2.is_dynamic();
let flipped = !bodies[handle2].is_dynamic();
let (local_frame1, local_frame2) = if flipped {
std::mem::swap(&mut handle1, &mut handle2);
@@ -286,35 +275,27 @@ impl AnyJointVelocityConstraint {
(joint.data.local_frame1, joint.data.local_frame2)
};
let rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle1.0);
let rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
) = bodies.index_bundle(handle2.0);
let rb1 = &bodies[handle1];
let rb2 = &bodies[handle2];
let (rb_pos1, rb_vel1, rb_mprops1) = 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 frame1 = rb1.pos.position * local_frame1;
let frame2 = rb2.pos.position * local_frame2;
let body1 = SolverBody {
linvel: rb_vel1.linvel,
angvel: rb_vel1.angvel,
im: rb_mprops1.effective_inv_mass,
sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt,
world_com: rb_mprops1.world_com,
linvel: rb1.vels.linvel,
angvel: rb1.vels.angvel,
im: rb1.mprops.effective_inv_mass,
sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
world_com: rb1.mprops.world_com,
mj_lambda: [crate::INVALID_USIZE],
};
let body2 = SolverBody {
linvel: rb_vel2.linvel,
angvel: rb_vel2.angvel,
im: rb_mprops2.effective_inv_mass,
sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt,
world_com: rb_mprops2.world_com,
mj_lambda: [rb_ids2.active_set_offset],
linvel: rb2.vels.linvel,
angvel: rb2.vels.angvel,
im: rb2.mprops.effective_inv_mass,
sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
world_com: rb2.mprops.world_com,
mj_lambda: [rb2.ids.active_set_offset],
};
if let Some(mb2) = multibodies
@@ -399,9 +380,13 @@ impl AnyJointVelocityConstraint {
out: &mut Vec<Self>,
insert_at: Option<usize>,
) {
use crate::dynamics::{
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
};
let mut handles1 = gather![|ii| impulse_joints[ii].body1];
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];
for ii in 0..SIMD_WIDTH {
@@ -429,9 +414,9 @@ impl AnyJointVelocityConstraint {
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
) = (
gather![|ii| bodies.index(handles1[ii].0)],
gather![|ii| bodies.index(handles1[ii].0)],
gather![|ii| bodies.index(handles1[ii].0)],
gather![|ii| &bodies[handles1[ii]].pos],
gather![|ii| &bodies[handles1[ii]].vels],
gather![|ii| &bodies[handles1[ii]].mprops],
);
let rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
@@ -439,10 +424,10 @@ impl AnyJointVelocityConstraint {
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
) = (
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| &bodies[handles2[ii]].pos],
gather![|ii| &bodies[handles2[ii]].vels],
gather![|ii| &bodies[handles2[ii]].mprops],
gather![|ii| &bodies[handles2[ii]].ids],
);
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::{
IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,
RigidBodyDamping, RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
RigidBodyType, RigidBodyVelocity,
RigidBodySet,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use na::DVector;
@@ -316,15 +315,13 @@ impl ParallelIslandSolver {
mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
}
} else {
let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) =
bodies.index_bundle(handle.0);
let dvel = &mut velocity_solver.mj_lambdas[ids.active_set_offset];
let rb = &bodies[*handle];
let dvel = &mut velocity_solver.mj_lambdas[rb.ids.active_set_offset];
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor:
dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt;
dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt;
dvel.angular += 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;
}
}
}

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

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

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

@@ -3,9 +3,7 @@ use crate::dynamics::solver::{
};
#[cfg(feature = "simd-is-enabled")]
use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot};
@@ -160,13 +158,14 @@ impl VelocityConstraint {
let handle1 = manifold.data.rigid_body1.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 mj_lambda2 = ids2.active_set_offset;
let rb1 = &bodies[handle1];
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;
#[cfg(feature = "dim2")]

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

@@ -1,7 +1,9 @@
use super::{
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::math::{
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 handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
let vels1: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let ids1: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let vels1: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].vels];
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].vels];
let ids1: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].ids];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].ids];
let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].mprops];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].mprops];
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
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::{self, WAngularInertia, WCross, WDot};
use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::dynamics::{IntegrationParameters, RigidBodySet, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[derive(Copy, Clone, Debug)]
@@ -51,15 +49,15 @@ impl VelocityGroundConstraint {
};
let (vels1, world_com1) = if let Some(handle1) = handle1 {
let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle1.0);
(*vels1, mprops1.world_com)
let rb1 = &bodies[handle1];
(rb1.vels, rb1.mprops.world_com)
} else {
(RigidBodyVelocity::zero(), Point::origin())
};
let (ids2, vels2, mprops2): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle2.unwrap().0);
let rb2 = &bodies[handle2.unwrap()];
let vels2 = &rb2.vels;
let mprops2 = &rb2.mprops;
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
@@ -67,7 +65,7 @@ impl VelocityGroundConstraint {
let tangents1 =
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
.data

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

@@ -2,7 +2,9 @@ use super::{
AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement,
VelocityGroundConstraintNormalPart,
};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
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| {
handles1[ii]
.map(|h| *bodies.index(h.0))
.map(|h| bodies[h].vels)
.unwrap_or_else(RigidBodyVelocity::zero)
}];
let world_com1 = Point::from(gather![|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)
}]);
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] =
gather![|ii| bodies.index(handles2[ii].unwrap().0)];
let ids2: [&RigidBodyIds; 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[handles2[ii].unwrap()].ids];
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);

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

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

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

@@ -451,7 +451,7 @@ impl BroadPhase {
for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer
// 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() {
continue;
}
@@ -471,12 +471,9 @@ impl BroadPhase {
// Make sure we have the new proxy index in case
// the collider was added for the first time.
colliders.set_internal(
handle.0,
ColliderBroadPhaseData {
co.bf_data = 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
// if this rigid-body was obtained by cloning another one.
coll.reset_internal_references();
coll.co_parent = None;
coll.parent = None;
let handle = ColliderHandle(self.colliders.insert(coll));
self.modified_colliders.push(handle);
handle
@@ -91,12 +91,12 @@ impl ColliderSet {
// if this collider was obtained by cloning another one.
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;
} else {
coll.co_parent = Some(ColliderParent {
coll.parent = Some(ColliderParent {
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();
parent.add_collider(
handle,
coll.co_parent.as_mut().unwrap(),
&mut coll.co_pos,
&coll.co_shape,
&coll.co_mprops,
coll.parent.as_mut().unwrap(),
&mut coll.pos,
&coll.shape,
&coll.mprops,
);
handle
}
@@ -128,12 +128,12 @@ impl ColliderSet {
bodies: &mut RigidBodySet,
) {
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 {
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(rb) = bodies.get_mut(parent_handle) {
@@ -143,10 +143,10 @@ impl ColliderSet {
match new_parent_handle {
Some(new_parent_handle) => {
if let Some(co_parent) = &mut collider.co_parent {
co_parent.handle = new_parent_handle;
if let Some(parent) = &mut collider.parent {
parent.handle = new_parent_handle;
} else {
collider.co_parent = Some(ColliderParent {
collider.parent = Some(ColliderParent {
handle: new_parent_handle,
pos_wrt_parent: Isometry::identity(),
})
@@ -155,14 +155,14 @@ impl ColliderSet {
if let Some(rb) = bodies.get_mut(new_parent_handle) {
rb.add_collider(
handle,
collider.co_parent.as_ref().unwrap(),
&mut collider.co_pos,
&collider.co_shape,
&collider.co_mprops,
collider.parent.as_ref().unwrap(),
&mut collider.pos,
&collider.shape,
&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
// modification flag is updated properly.
if let Some(co_parent) = &collider.co_parent {
if let Some(parent) =
bodies.get_mut_internal_with_modification_tracking(co_parent.handle)
if let Some(parent) = &collider.parent {
if let Some(parent_rb) =
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 {
islands.wake_up(bodies, co_parent.handle, true);
islands.wake_up(bodies, parent.handle, true);
}
}
}
@@ -247,8 +247,8 @@ impl ColliderSet {
collider: &mut Collider,
modified_colliders: &mut Vec<ColliderHandle>,
) {
if !collider.co_changes.contains(ColliderChanges::MODIFIED) {
collider.co_changes = ColliderChanges::MODIFIED;
if !collider.changes.contains(ColliderChanges::MODIFIED) {
collider.changes = ColliderChanges::MODIFIED;
modified_colliders.push(handle);
}
}
@@ -261,6 +261,10 @@ impl ColliderSet {
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> {
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::dynamics::{
CoefficientCombineRule, IslandManager, RigidBodyActivation, RigidBodyDominance, RigidBodyIds,
RigidBodySet, RigidBodyType,
CoefficientCombineRule, IslandManager, RigidBodyDominance, RigidBodySet, RigidBodyType,
};
use crate::geometry::{
BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderMaterial,
ColliderPair, ColliderParent, ColliderPosition, ColliderSet, ColliderShape, ColliderType,
CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair,
BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair,
ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair,
InteractionGraph, IntersectionPair, SolverContact, SolverFlags,
};
use crate::math::{Real, Vector};
@@ -17,7 +15,6 @@ use crate::pipeline::{
ActiveEvents, ActiveHooks, ContactModificationContext, EventHandler, PairFilterContext,
PhysicsHooks,
};
use crate::prelude::ColliderFlags;
use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher};
use parry::utils::IsometryOpt;
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.
if let Some(islands) = islands.as_deref_mut() {
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) {
islands.wake_up(bodies, parent, true)
if let Some(parent) = colliders.get(a).and_then(|c| c.parent.as_ref()) {
islands.wake_up(bodies, parent.handle, true)
}
if let Some(parent) = colliders.get(b.0).map(|c| c.handle) {
islands.wake_up(bodies, parent, true)
if let Some(parent) = colliders.get(b).and_then(|c| c.parent.as_ref()) {
islands.wake_up(bodies, parent.handle, true)
}
if pair.start_event_emited {
@@ -379,10 +376,8 @@ impl NarrowPhase {
for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer
// exist if it has been removed.
let co_changes: Option<&ColliderChanges> = colliders.get(handle.0);
if let Some(co_changes) = co_changes {
if co_changes.needs_narrow_phase_update() {
if let Some(co) = colliders.get(*handle) {
if co.changes.needs_narrow_phase_update() {
// No flag relevant to the narrow-phase is enabled for this collider.
continue;
}
@@ -392,13 +387,8 @@ impl NarrowPhase {
// 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
// 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(co_parent) = co_parent {
if let Some(co_parent) = &co.parent {
islands.wake_up(bodies, co_parent.handle, true);
}
@@ -407,8 +397,9 @@ impl NarrowPhase {
.interactions_with(gid.contact_graph_index)
{
let other_handle = if *handle == inter.0 { inter.1 } else { inter.0 };
let other_parent: Option<&ColliderParent> =
colliders.get(other_handle.0);
let other_parent = colliders
.get(other_handle)
.and_then(|co| co.parent.as_ref());
if let Some(other_parent) = other_parent {
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 achieve this we will remove the relevant contact/intersection pairs form the
// contact/intersection graphs, and then add them into the other graph.
if co_changes.contains(ColliderChanges::TYPE) {
if co_type.is_sensor() {
if co.changes.contains(ColliderChanges::TYPE) {
if co.is_sensor() {
// Find the contact pairs for this collider and
// push them to `pairs_to_remove`.
for inter in self
@@ -441,9 +432,7 @@ impl NarrowPhase {
.intersection_graph
.interactions_with(gid.intersection_graph_index)
.filter(|(h1, h2, _)| {
let co_type1: &ColliderType = colliders.index(h1.0);
let co_type2: &ColliderType = colliders.index(h2.0);
!co_type1.is_sensor() && !co_type2.is_sensor()
!colliders[*h1].is_sensor() && !colliders[*h2].is_sensor()
})
{
pairs_to_remove.push((
@@ -484,10 +473,9 @@ impl NarrowPhase {
events: &dyn EventHandler,
mode: PairRemovalMode,
) {
let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0);
let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0);
if let (Some(co_type1), Some(co_type2)) = (co_type1, co_type2) {
if let (Some(co1), Some(co2)) =
(colliders.get(pair.collider1), colliders.get(pair.collider2))
{
// TODO: could we just unwrap here?
// Don't we have the guarantee that we will get a `AddPair` before a `DeletePair`?
if let (Some(gid1), Some(gid2)) = (
@@ -495,8 +483,7 @@ impl NarrowPhase {
self.graph_indices.get(pair.collider2.0),
) {
if mode == PairRemovalMode::FromIntersectionGraph
|| (mode == PairRemovalMode::Auto
&& (co_type1.is_sensor() || co_type2.is_sensor()))
|| (mode == PairRemovalMode::Auto && (co1.is_sensor() || co2.is_sensor()))
{
let intersection = self
.intersection_graph
@@ -505,10 +492,7 @@ impl NarrowPhase {
// Emit an intersection lost event if we had an intersection before removing the edge.
if let Some(mut intersection) = intersection {
if intersection.intersecting {
let co_flag1: &ColliderFlags = colliders.index(pair.collider1.0);
let co_flag2: &ColliderFlags = colliders.index(pair.collider2.0);
if (co_flag1.active_events | co_flag2.active_events)
if (co1.flags.active_events | co2.flags.active_events)
.contains(ActiveEvents::COLLISION_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.
if let Some(mut ctct) = contact_pair {
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(co_parent1) = co_parent1 {
if let Some(co_parent1) = &co1.parent {
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);
}
}
let co_flag1: &ColliderFlags = colliders.index(pair.collider1.0);
let co_flag2: &ColliderFlags = colliders.index(pair.collider2.0);
if (co_flag1.active_events | co_flag2.active_events)
if (co1.flags.active_events | co2.flags.active_events)
.contains(ActiveEvents::COLLISION_EVENTS)
{
ctct.emit_stop_event(events);
@@ -555,15 +531,11 @@ impl NarrowPhase {
}
fn add_pair(&mut self, colliders: &ColliderSet, pair: &ColliderPair) {
let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0);
let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0);
if let (Some(co_type1), Some(co_type2)) = (co_type1, co_type2) {
let co_parent1: Option<&ColliderParent> = colliders.get(pair.collider1.0);
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() {
if let (Some(co1), Some(co2)) =
(colliders.get(pair.collider1), colliders.get(pair.collider2))
{
if co1.parent.map(|p| p.handle) == co2.parent.map(|p| p.handle) {
if co1.parent.is_some() {
// Same parents. Ignore collisions.
return;
}
@@ -577,7 +549,7 @@ impl NarrowPhase {
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
// as it does not interact with anything.
if !InteractionGraph::<(), ()>::is_graph_index_valid(gid1.intersection_graph_index)
@@ -682,27 +654,13 @@ impl NarrowPhase {
let handle1 = nodes[edge.source().index()].weight;
let handle2 = nodes[edge.target().index()].weight;
let had_intersection = edge.weight.intersecting;
let co1 = &colliders[handle1];
let co2 = &colliders[handle2];
// TODO: remove the `loop` once labels on blocks is stabilized.
'emit_events: loop {
let co_parent1: Option<&ColliderParent> = colliders.get(handle1.0);
let (co_changes1, co_shape1, co_pos1, co_flags1): (
&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()
if !co1.changes.needs_narrow_phase_update()
&& !co2.changes.needs_narrow_phase_update()
{
// No update needed for these colliders.
return;
@@ -712,36 +670,36 @@ impl NarrowPhase {
let mut rb_type1 = RigidBodyType::Fixed;
let mut rb_type2 = RigidBodyType::Fixed;
if let Some(co_parent1) = co_parent1 {
rb_type1 = *bodies.index(co_parent1.handle.0);
if let Some(co_parent1) = &co1.parent {
rb_type1 = bodies[co_parent1.handle].body_type;
}
if let Some(co_parent2) = co_parent2 {
rb_type2 = *bodies.index(co_parent2.handle.0);
if let Some(co_parent2) = &co2.parent {
rb_type2 = bodies[co_parent2.handle].body_type;
}
// Filter based on the rigid-body types.
if !co_flags1.active_collision_types.test(rb_type1, rb_type2)
&& !co_flags2.active_collision_types.test(rb_type1, rb_type2)
if !co1.flags.active_collision_types.test(rb_type1, rb_type2)
&& !co2.flags.active_collision_types.test(rb_type1, rb_type2)
{
edge.weight.intersecting = false;
break 'emit_events;
}
// 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;
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) {
let context = PairFilterContext {
bodies,
colliders,
rigid_body1: co_parent1.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle),
rigid_body1: co1.parent.map(|p| p.handle),
rigid_body2: co2.parent.map(|p| p.handle),
collider1: handle1,
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
.intersection_test(&pos12, &**co_shape1, &**co_shape2)
.intersection_test(&pos12, &*co1.shape, &*co2.shape)
.unwrap_or(false);
break 'emit_events;
}
let co_flags1: &ColliderFlags = colliders.index(handle1.0);
let co_flags2: &ColliderFlags = colliders.index(handle2.0);
let active_events = co_flags1.active_events | co_flags2.active_events;
let active_events = co1.flags.active_events | co2.flags.active_events;
if active_events.contains(ActiveEvents::COLLISION_EVENTS)
&& had_intersection != edge.weight.intersecting
@@ -795,29 +751,13 @@ impl NarrowPhase {
par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| {
let pair = &mut edge.weight;
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.
'emit_events: loop {
let co_parent1: Option<&ColliderParent> = colliders.get(pair.collider1.0);
let (co_changes1, co_shape1, co_pos1, co_material1, co_flags1): (
&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()
if !co1.changes.needs_narrow_phase_update()
&& !co2.changes.needs_narrow_phase_update()
{
// No update needed for these colliders.
return;
@@ -827,36 +767,36 @@ impl NarrowPhase {
let mut rb_type1 = RigidBodyType::Fixed;
let mut rb_type2 = RigidBodyType::Fixed;
if let Some(co_parent1) = co_parent1 {
rb_type1 = *bodies.index(co_parent1.handle.0);
if let Some(co_parent1) = &co1.parent {
rb_type1 = bodies[co_parent1.handle].body_type;
}
if let Some(co_parent2) = co_parent2 {
rb_type2 = *bodies.index(co_parent2.handle.0);
if let Some(co_parent2) = &co2.parent {
rb_type2 = bodies[co_parent2.handle].body_type;
}
// Filter based on the rigid-body types.
if !co_flags1.active_collision_types.test(rb_type1, rb_type2)
&& !co_flags2.active_collision_types.test(rb_type1, rb_type2)
if !co1.flags.active_collision_types.test(rb_type1, rb_type2)
&& !co2.flags.active_collision_types.test(rb_type1, rb_type2)
{
pair.clear();
break 'emit_events;
}
// 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();
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 context = PairFilterContext {
bodies,
colliders,
rigid_body1: co_parent1.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle),
rigid_body1: co1.parent.map(|p| p.handle),
rigid_body2: co2.parent.map(|p| p.handle),
collider1: pair.collider1,
collider2: pair.collider2,
};
@@ -872,53 +812,55 @@ impl NarrowPhase {
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);
}
if co_changes1.contains(ColliderChanges::SHAPE)
|| co_changes2.contains(ColliderChanges::SHAPE)
if co1.changes.contains(ColliderChanges::SHAPE)
|| co2.changes.contains(ColliderChanges::SHAPE)
{
// The shape changed so the workspace is no longer valid.
pair.workspace = None;
}
let pos12 = co_pos1.inv_mul(co_pos2);
let pos12 = co1.pos.inv_mul(&co2.pos);
let _ = query_dispatcher.contact_manifolds(
&pos12,
&**co_shape1,
&**co_shape2,
&*co1.shape,
&*co2.shape,
prediction_distance,
&mut pair.manifolds,
&mut pair.workspace,
);
let friction = CoefficientCombineRule::combine(
co_material1.friction,
co_material2.friction,
co_material1.friction_combine_rule as u8,
co_material2.friction_combine_rule as u8,
co1.material.friction,
co2.material.friction,
co1.material.friction_combine_rule as u8,
co2.material.friction_combine_rule as u8,
);
let restitution = CoefficientCombineRule::combine(
co_material1.restitution,
co_material2.restitution,
co_material1.restitution_combine_rule as u8,
co_material2.restitution_combine_rule as u8,
co1.material.restitution,
co2.material.restitution,
co1.material.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 dominance1 = co_parent1
.map(|p1| *bodies.index(p1.handle.0))
let dominance1 = co1
.parent
.map(|p1| bodies[p1.handle].dominance)
.unwrap_or(zero);
let dominance2 = co_parent2
.map(|p2| *bodies.index(p2.handle.0))
let dominance2 = co2
.parent
.map(|p2| bodies[p2.handle].dominance)
.unwrap_or(zero);
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.rigid_body1 = co_parent1.map(|p| p.handle);
manifold.data.rigid_body2 = co_parent2.map(|p| p.handle);
manifold.data.rigid_body1 = co1.parent.map(|p| p.handle);
manifold.data.rigid_body2 = co2.parent.map(|p| p.handle);
manifold.data.solver_flags = solver_flags;
manifold.data.relative_dominance = dominance1.effective_group(&rb_type1)
- dominance2.effective_group(&rb_type2);
@@ -960,8 +902,8 @@ impl NarrowPhase {
let mut context = ContactModificationContext {
bodies,
colliders,
rigid_body1: co_parent1.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle),
rigid_body1: co1.parent.map(|p| p.handle),
rigid_body2: co2.parent.map(|p| p.handle),
collider1: pair.collider1,
collider2: pair.collider2,
manifold,
@@ -981,9 +923,7 @@ impl NarrowPhase {
break 'emit_events;
}
let co_flags1: &ColliderFlags = colliders.index(pair.collider1.0);
let co_flags2: &ColliderFlags = colliders.index(pair.collider2.0);
let active_events = co_flags1.active_events | co_flags2.active_events;
let active_events = co1.flags.active_events | co2.flags.active_events;
if pair.has_any_active_contact != had_any_active_contact {
if active_events.contains(ActiveEvents::COLLISION_EVENTS) {
@@ -1021,18 +961,24 @@ impl NarrowPhase {
{
let (active_island_id1, rb_type1, sleeping1) =
if let Some(handle1) = manifold.data.rigid_body1 {
let data: (&RigidBodyIds, &RigidBodyType, &RigidBodyActivation) =
bodies.index_bundle(handle1.0);
(data.0.active_island_id, *data.1, data.2.sleeping)
let rb1 = &bodies[handle1];
(
rb1.ids.active_island_id,
rb1.body_type,
rb1.activation.sleeping,
)
} else {
(0, RigidBodyType::Fixed, true)
};
let (active_island_id2, rb_type2, sleeping2) =
if let Some(handle2) = manifold.data.rigid_body2 {
let data: (&RigidBodyIds, &RigidBodyType, &RigidBodyActivation) =
bodies.index_bundle(handle2.0);
(data.0.active_island_id, *data.1, data.2.sleeping)
let rb2 = &bodies[handle2];
(
rb2.ids.active_island_id,
rb2.body_type,
rb2.activation.sleeping,
)
} else {
(0, RigidBodyType::Fixed, true)
};

2
src/pipeline/collision_pipeline.rs
View File

@@ -97,7 +97,7 @@ impl CollisionPipeline {
modified_colliders: &mut Vec<ColliderHandle>,
) {
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::{
CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
RigidBodyColliders, RigidBodyForces, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition,
RigidBodyType, RigidBodyVelocity,
RigidBodyHandle, RigidBodyPosition, RigidBodyType,
};
#[cfg(feature = "parallel")]
use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
@@ -72,7 +71,9 @@ impl PhysicsPipeline {
modified_colliders: &mut Vec<ColliderHandle>,
) {
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();
for handle in islands.active_dynamic_bodies() {
let poss: &RigidBodyPosition = bodies.index(handle.0);
let position = poss.position;
let effective_inv_mass = bodies
.map_mut_internal(handle.0, |mprops: &mut RigidBodyMassProps| {
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)
});
let rb = bodies.index_mut_internal(*handle);
rb.mprops.update_world_mass_properties(&rb.pos.position);
let effective_mass = rb.mprops.effective_mass();
rb.forces
.compute_effective_force_and_torque(&gravity, &effective_mass);
}
for multibody in &mut multibody_joints.multibodies {
@@ -319,13 +313,10 @@ impl PhysicsPipeline {
) {
// Set the rigid-bodies and kinematic bodies to their final position.
for handle in islands.iter_active_bodies() {
bodies.map_mut_internal(handle.0, |poss: &mut RigidBodyPosition| {
poss.position = poss.next_position
});
let (rb_poss, rb_colls): (&RigidBodyPosition, &RigidBodyColliders) =
bodies.index_bundle(handle.0);
rb_colls.update_positions(colliders, modified_colliders, &rb_poss.position);
let rb = bodies.index_mut_internal(handle);
rb.pos.position = rb.pos.next_position;
rb.colliders
.update_positions(colliders, modified_colliders, &rb.pos.position);
}
}
@@ -341,29 +332,22 @@ impl PhysicsPipeline {
// there to determine if this kinematic body should wake-up dynamic
// bodies it is touching.
for handle in islands.active_kinematic_bodies() {
let (rb_type, rb_pos, rb_vel, rb_mprops): (
&RigidBodyType,
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let rb = bodies.index_mut_internal(*handle);
match rb_type {
match rb.body_type {
RigidBodyType::KinematicPositionBased => {
let rb_pos: &RigidBodyPosition = bodies.index(handle.0);
let new_vel = rb_pos.interpolate_velocity(
rb.vels = rb.pos.interpolate_velocity(
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 => {
let new_pos = rb_vel.integrate(
let new_pos = rb.vels.integrate(
integration_parameters.dt,
&rb_pos.position,
&rb_mprops.local_mprops.local_com,
&rb.pos.position,
&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::{
IslandManager, RigidBodyColliders, RigidBodyForces, RigidBodyMassProps, RigidBodyPosition,
RigidBodyVelocity,
};
use crate::dynamics::IslandManager;
use crate::geometry::{
ColliderFlags, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape,
InteractionGroups, PointProjection, Ray, RayIntersection, AABB, QBVH,
ColliderHandle, InteractionGroups, PointProjection, Ray, RayIntersection, AABB, QBVH,
};
use crate::math::{Isometry, Point, Real, Vector};
use parry::partitioning::QBVHDataGenerator;
@@ -75,7 +71,7 @@ impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
if co.flags.collision_groups.test(self.query_groups)
&& 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)) {
match self.mode {
QueryPipelineMode::CurrentPosition => {
self.colliders.for_each(|h, co_shape: &ColliderShape| {
let co_pos: &ColliderPosition = self.colliders.index(h);
f(ColliderHandle(h), co_shape.compute_aabb(&co_pos))
})
for (h, co) in self.colliders.iter() {
f(h, co.shape.compute_aabb(&co.pos))
}
}
QueryPipelineMode::SweepTestWithNextPosition => {
self.colliders.for_each(|h, co_shape: &ColliderShape| {
let co_parent: Option<&ColliderParent> = self.colliders.get(h);
let co_pos: &ColliderPosition = self.colliders.index(h);
if let Some(co_parent) = co_parent {
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),
)
for (h, co) in self.colliders.iter() {
if let Some(co_parent) = co.parent {
let rb_next_pos = &self.bodies[co_parent.handle].pos.next_position;
let next_position = rb_next_pos * co_parent.pos_wrt_parent;
f(h, co.shape.compute_swept_aabb(&co.pos, &next_position))
} else {
f(ColliderHandle(h), co_shape.compute_aabb(&co_pos))
f(h, co.shape.compute_aabb(&co.pos))
}
}
})
}
QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
self.colliders.for_each(|h, co_shape: &ColliderShape| {
let co_parent: Option<&ColliderParent> = self.colliders.get(h);
let co_pos: &ColliderPosition = self.colliders.index(h);
if let Some(co_parent) = co_parent {
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);
for (h, co) in self.colliders.iter() {
if let Some(co_parent) = co.parent {
let rb = &self.bodies[co_parent.handle];
let predicted_pos = rb.pos.integrate_forces_and_velocities(
dt, &rb.forces, &rb.vels, &rb.mprops,
);
let next_position = predicted_pos * co_parent.pos_wrt_parent;
f(
ColliderHandle(h),
co_shape.compute_swept_aabb(&co_pos, &next_position),
)
f(h, co.shape.compute_swept_aabb(&co.pos, &next_position))
} 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() {
let body_colliders: &RigidBodyColliders = bodies.index(handle.0);
for handle in &body_colliders.0 {
let rb = &bodies[handle];
for handle in &rb.colliders.0 {
self.qbvh.pre_update(*handle)
}
}
@@ -266,9 +243,8 @@ impl QueryPipeline {
QueryPipelineMode::CurrentPosition => {
self.qbvh.update(
|handle| {
let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) =
colliders.index_bundle(handle.0);
co_shape.compute_aabb(&co_pos)
let co = &colliders[*handle];
co.shape.compute_aabb(&co.pos)
},
self.dilation_factor,
);
@@ -276,10 +252,10 @@ impl QueryPipeline {
QueryPipelineMode::SweepTestWithNextPosition => {
self.qbvh.update(
|handle| {
let co = &colliders[handle];
if let Some(parent) = co.parent {
let rb_pos: &RigidBodyPosition = bodies.index(co.parent.handle.0);
let next_position = rb_pos.next_position * co.parent.pos_wrt_parent;
let co = &colliders[*handle];
if let Some(parent) = &co.parent {
let rb_next_pos = &bodies[parent.handle].pos.next_position;
let next_position = rb_next_pos * parent.pos_wrt_parent;
co.shape.compute_swept_aabb(&co.pos, &next_position)
} else {
co.shape.compute_aabb(&co.pos)
@@ -291,12 +267,12 @@ impl QueryPipeline {
QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
self.qbvh.update(
|handle| {
let co = &colliders[handle];
let co = &colliders[*handle];
if let Some(parent) = co.parent {
let rb = &bodies[parent.handle];
let predicted_pos = rb
.pos
.integrate_forces_and_velocities(dt, rb.forces, rb.vels, rb.mprops);
let predicted_pos = rb.pos.integrate_forces_and_velocities(
dt, &rb.forces, &rb.vels, &rb.mprops,
);
let next_position = predicted_pos * parent.pos_wrt_parent;
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.
/// 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.
pub fn intersections_with_ray<'a, ColliderSet>(
pub fn intersections_with_ray<'a>(
&self,
colliders: &'a ColliderSet,
ray: &Ray,
@@ -405,14 +381,13 @@ impl QueryPipeline {
mut callback: impl FnMut(ColliderHandle, RayIntersection) -> bool,
) {
let mut leaf_callback = &mut |handle: &ColliderHandle| {
let co_shape: Option<&ColliderShape> = colliders.get(handle.0);
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)
if let Some(co) = colliders.get(*handle) {
if co.flags.collision_groups.test(query_groups)
&& 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);
}
@@ -502,7 +477,7 @@ impl QueryPipeline {
/// is either `None` or returns `true`.
/// * `callback` - A function called with each collider with a shape
/// containing the `point`.
pub fn intersections_with_point<'a, ColliderSet>(
pub fn intersections_with_point<'a>(
&self,
colliders: &'a ColliderSet,
point: &Point<Real>,
@@ -511,15 +486,10 @@ impl QueryPipeline {
mut callback: impl FnMut(ColliderHandle) -> bool,
) {
let mut leaf_callback = &mut |handle: &ColliderHandle| {
let co_shape: Option<&ColliderShape> = colliders.get(handle.0);
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)
if let Some(co) = colliders.get(*handle) {
if co.flags.collision_groups.test(query_groups)
&& 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);
}
@@ -677,7 +647,7 @@ impl QueryPipeline {
/// its `contact_group` is compatible with the `query_groups`, and if this `filter`
/// is either `None` or returns `true`.
/// * `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,
colliders: &'a ColliderSet,
shape_pos: &Isometry<Real>,
@@ -690,18 +660,13 @@ impl QueryPipeline {
let inv_shape_pos = shape_pos.inverse();
let mut leaf_callback = &mut |handle: &ColliderHandle| {
let co_shape: Option<&ColliderShape> = colliders.get(handle.0);
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)
if let Some(co) = colliders.get(*handle) {
if co.flags.collision_groups.test(query_groups)
&& 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);
}
}

49
src/pipeline/user_changes.rs
View File

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

2
src_testbed/harness/mod.rs
View File

@@ -156,7 +156,7 @@ impl Harness {
impulse_joints: ImpulseJointSet,
multibody_joints: MultibodyJointSet,
gravity: Vector<Real>,
hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
hooks: impl PhysicsHooks + 'static,
) {
// println!("Num bodies: {}", bodies.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 integration_parameters: IntegrationParameters,
pub gravity: Vector<Real>,
pub hooks: Box<dyn PhysicsHooks<RigidBodySet, ColliderSet>>,
pub hooks: Box<dyn PhysicsHooks>,
}
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,
multibody_joints: MultibodyJointSet,
gravity: Vector<Real>,
hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
hooks: impl PhysicsHooks + 'static,
) {
self.harness.set_world_with_params(
bodies,