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

297
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)
if bh1 == bh2 // Ignore self-intersection.
|| (co1.is_sensor() || co2.is_sensor()) // Ignore sensors.
|| !co1.flags.collision_groups.test(co2.flags.collision_groups) // Apply collision groups.
|| !co1.flags.solver_groups.test(co2.flags.solver_groups)
// Apply solver groups.
{
return true;
@@ -208,16 +187,16 @@ impl CCDSolver {
.map(|c| c.1.dist)
.unwrap_or(0.0);
let b2 = bh2.map(|h| bodies.index_bundle(h.0));
let rb2 = bh2.and_then(|h| bodies.get(h));
if let Some(toi) = TOIEntry::try_from_colliders(
self.query_pipeline.query_dispatcher(),
*ch1,
*ch2,
(c1.0, c1.1, c1.2, c1.3, co_parent1),
(c2.0, c2.1, c2.2, c2.3, co_parent2),
Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)),
b2,
co1,
co2,
Some(rb1),
rb2,
None,
None,
0.0,
@@ -271,29 +250,27 @@ impl CCDSolver {
*/
// TODO: don't iterate through all the colliders.
for handle in islands.active_dynamic_bodies() {
let rb_ccd1: &RigidBodyCcd = bodies.index(handle.0);
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.