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

feat: switch to the new Bvh from parry for the broad-phase (#853)

Browse Source 
* feat: switch to the new Bvh from parry for the broad-phase

* chore: cargo fmt + update testbed

* chore: remove the multi-grid SAP broad-phase

* fix soft-ccd handling in broad-phase

* Fix contact cleanup in broad-phase after collider removal

* chore: clippy fixes

* fix CCD regression

* chore: update changelog

* fix build with the parallel feature enabled

* chore: remove the now useless broad-phase proxy index from colliders

* fix tests
This commit is contained in:
Sébastien Crozet
2025-07-11 22:36:40 +02:00
committed by GitHub
parent 86a257d4f1
commit 95bd6fcfeb
212 changed files with 2140 additions and 3953 deletions
Download Patch File Download Diff File Expand all files Collapse all files

382
src/control/character_controller.rs
View File

@@ -1,7 +1,6 @@
use crate::dynamics::RigidBodySet;
use crate::geometry::{ColliderHandle, ColliderSet, ContactManifold, Shape, ShapeCastHit};
use crate::geometry::{ColliderHandle, ContactManifold, Shape, ShapeCastHit};
use crate::math::{Isometry, Point, Real, UnitVector, Vector};
use crate::pipeline::{QueryFilter, QueryFilterFlags, QueryPipeline};
use crate::pipeline::{QueryFilterFlags, QueryPipeline, QueryPipelineMut};
use crate::utils;
use na::{RealField, Vector2};
use parry::bounding_volume::BoundingVolume;
@@ -215,13 +214,10 @@ impl KinematicCharacterController {
pub fn move_shape(
&self,
dt: Real,
bodies: &RigidBodySet,
colliders: &ColliderSet,
queries: &QueryPipeline,
character_shape: &dyn Shape,
character_pos: &Isometry<Real>,
desired_translation: Vector<Real>,
filter: QueryFilter,
mut events: impl FnMut(CharacterCollision),
) -> EffectiveCharacterMovement {
let mut result = EffectiveCharacterMovement {
@@ -238,13 +234,10 @@ impl KinematicCharacterController {
let grounded_at_starting_pos = self.detect_grounded_status_and_apply_friction(
dt,
bodies,
colliders,
queries,
character_shape,
character_pos,
&dims,
filter,
None,
None,
);
@@ -266,8 +259,6 @@ impl KinematicCharacterController {
// 2. Cast towards the movement direction.
if let Some((handle, hit)) = queries.cast_shape(
bodies,
colliders,
&(Translation::from(result.translation) * character_pos),
&translation_dir,
character_shape,
@@ -277,7 +268,6 @@ impl KinematicCharacterController {
max_time_of_impact: translation_dist,
compute_impact_geometry_on_penetration: true,
},
filter,
) {
// We hit something, compute and apply the allowed interference-free translation.
let allowed_dist = hit.time_of_impact;
@@ -297,13 +287,10 @@ impl KinematicCharacterController {
// Try to go upstairs.
if !self.handle_stairs(
bodies,
colliders,
queries,
*queries,
character_shape,
&(Translation::from(result.translation) * character_pos),
&dims,
filter,
handle,
&hit_info,
&mut translation_remaining,
@@ -324,13 +311,10 @@ impl KinematicCharacterController {
translation_remaining.fill(0.0);
result.grounded = self.detect_grounded_status_and_apply_friction(
dt,
bodies,
colliders,
queries,
character_shape,
&(Translation::from(result.translation) * character_pos),
&dims,
filter,
None,
None,
);
@@ -338,13 +322,10 @@ impl KinematicCharacterController {
}
result.grounded = self.detect_grounded_status_and_apply_friction(
dt,
bodies,
colliders,
queries,
character_shape,
&(Translation::from(result.translation) * character_pos),
&dims,
filter,
Some(&mut kinematic_friction_translation),
Some(&mut translation_remaining),
);
@@ -358,13 +339,10 @@ impl KinematicCharacterController {
if !is_moving {
result.grounded = self.detect_grounded_status_and_apply_friction(
dt,
bodies,
colliders,
queries,
character_shape,
&(Translation::from(result.translation) * character_pos),
&dims,
filter,
None,
None,
);
@@ -372,13 +350,10 @@ impl KinematicCharacterController {
// If needed, and if we are not already grounded, snap to the ground.
if grounded_at_starting_pos {
self.snap_to_ground(
bodies,
colliders,
queries,
character_shape,
&(Translation::from(result.translation) * character_pos),
&dims,
filter,
&mut result,
);
}
@@ -389,13 +364,10 @@ impl KinematicCharacterController {
fn snap_to_ground(
&self,
bodies: &RigidBodySet,
colliders: &ColliderSet,
queries: &QueryPipeline,
character_shape: &dyn Shape,
character_pos: &Isometry<Real>,
dims: &Vector2<Real>,
filter: QueryFilter,
result: &mut EffectiveCharacterMovement,
) -> Option<(ColliderHandle, ShapeCastHit)> {
if let Some(snap_distance) = self.snap_to_ground {
@@ -403,8 +375,6 @@ impl KinematicCharacterController {
let snap_distance = snap_distance.eval(dims.y);
let offset = self.offset.eval(dims.y);
if let Some((hit_handle, hit)) = queries.cast_shape(
bodies,
colliders,
character_pos,
&-self.up,
character_shape,
@@ -414,7 +384,6 @@ impl KinematicCharacterController {
max_time_of_impact: snap_distance,
compute_impact_geometry_on_penetration: true,
},
filter,
) {
// Apply the snap.
result.translation -= *self.up * hit.time_of_impact;
@@ -435,13 +404,10 @@ impl KinematicCharacterController {
fn detect_grounded_status_and_apply_friction(
&self,
dt: Real,
bodies: &RigidBodySet,
colliders: &ColliderSet,
queries: &QueryPipeline,
character_shape: &dyn Shape,
character_pos: &Isometry<Real>,
dims: &Vector2<Real>,
filter: QueryFilter,
mut kinematic_friction_translation: Option<&mut Vector<Real>>,
mut translation_remaining: Option<&mut Vector<Real>>,
) -> bool {
@@ -457,88 +423,82 @@ impl KinematicCharacterController {
let mut grounded = false;
queries.colliders_with_aabb_intersecting_aabb(&character_aabb, |handle| {
if let Some(collider) = colliders.get(*handle) {
if filter.test(bodies, *handle, collider) {
manifolds.clear();
let pos12 = character_pos.inv_mul(collider.position());
let _ = dispatcher.contact_manifolds(
&pos12,
character_shape,
collider.shape(),
prediction,
&mut manifolds,
&mut None,
);
'outer: for (_, collider) in queries.intersect_aabb_conservative(&character_aabb) {
manifolds.clear();
let pos12 = character_pos.inv_mul(collider.position());
let _ = dispatcher.contact_manifolds(
&pos12,
character_shape,
collider.shape(),
prediction,
&mut manifolds,
&mut None,
);
if let (Some(kinematic_friction_translation), Some(translation_remaining)) = (
kinematic_friction_translation.as_deref_mut(),
translation_remaining.as_deref_mut(),
) {
let init_kinematic_friction_translation = *kinematic_friction_translation;
let kinematic_parent = collider
.parent
.and_then(|p| bodies.get(p.handle))
.filter(|rb| rb.is_kinematic());
if let (Some(kinematic_friction_translation), Some(translation_remaining)) = (
kinematic_friction_translation.as_deref_mut(),
translation_remaining.as_deref_mut(),
) {
let init_kinematic_friction_translation = *kinematic_friction_translation;
let kinematic_parent = collider
.parent
.and_then(|p| queries.bodies.get(p.handle))
.filter(|rb| rb.is_kinematic());
for m in &manifolds {
if self.is_grounded_at_contact_manifold(m, character_pos, dims) {
grounded = true;
}
for m in &manifolds {
if self.is_grounded_at_contact_manifold(m, character_pos, dims) {
grounded = true;
}
if let Some(kinematic_parent) = kinematic_parent {
let mut num_active_contacts = 0;
let mut manifold_center = Point::origin();
let normal = -(character_pos * m.local_n1);
if let Some(kinematic_parent) = kinematic_parent {
let mut num_active_contacts = 0;
let mut manifold_center = Point::origin();
let normal = -(character_pos * m.local_n1);
for contact in &m.points {
if contact.dist <= prediction {
num_active_contacts += 1;
let contact_point = collider.position() * contact.local_p2;
let target_vel =
kinematic_parent.velocity_at_point(&contact_point);
for contact in &m.points {
if contact.dist <= prediction {
num_active_contacts += 1;
let contact_point = collider.position() * contact.local_p2;
let target_vel = kinematic_parent.velocity_at_point(&contact_point);
let normal_target_mvt = target_vel.dot(&normal) * dt;
let normal_current_mvt = translation_remaining.dot(&normal);
let normal_target_mvt = target_vel.dot(&normal) * dt;
let normal_current_mvt = translation_remaining.dot(&normal);
manifold_center += contact_point.coords;
*translation_remaining +=
normal * (normal_target_mvt - normal_current_mvt);
}
}
if num_active_contacts > 0 {
let target_vel = kinematic_parent.velocity_at_point(
&(manifold_center / num_active_contacts as Real),
);
let tangent_platform_mvt =
(target_vel - normal * target_vel.dot(&normal)) * dt;
kinematic_friction_translation.zip_apply(
&tangent_platform_mvt,
|y, x| {
if x.abs() > (*y).abs() {
*y = x;
}
},
);
}
manifold_center += contact_point.coords;
*translation_remaining +=
normal * (normal_target_mvt - normal_current_mvt);
}
}
*translation_remaining +=
*kinematic_friction_translation - init_kinematic_friction_translation;
} else {
for m in &manifolds {
if self.is_grounded_at_contact_manifold(m, character_pos, dims) {
grounded = true;
return false; // We can stop the search early.
}
if num_active_contacts > 0 {
let target_vel = kinematic_parent.velocity_at_point(
&(manifold_center / num_active_contacts as Real),
);
let tangent_platform_mvt =
(target_vel - normal * target_vel.dot(&normal)) * dt;
kinematic_friction_translation.zip_apply(
&tangent_platform_mvt,
|y, x| {
if x.abs() > (*y).abs() {
*y = x;
}
},
);
}
}
}
*translation_remaining +=
*kinematic_friction_translation - init_kinematic_friction_translation;
} else {
for m in &manifolds {
if self.is_grounded_at_contact_manifold(m, character_pos, dims) {
grounded = true;
break 'outer; // We can stop the search early.
}
}
}
true
});
}
grounded
}
@@ -662,13 +622,10 @@ impl KinematicCharacterController {
#[profiling::function]
fn handle_stairs(
&self,
bodies: &RigidBodySet,
colliders: &ColliderSet,
queries: &QueryPipeline,
mut queries: QueryPipeline,
character_shape: &dyn Shape,
character_pos: &Isometry<Real>,
dims: &Vector2<Real>,
mut filter: QueryFilter,
stair_handle: ColliderHandle,
hit: &HitInfo,
translation_remaining: &mut Vector<Real>,
@@ -688,10 +645,11 @@ impl KinematicCharacterController {
let max_height = autostep.max_height.eval(dims.y) + offset;
if !autostep.include_dynamic_bodies {
if colliders
if queries
.colliders
.get(stair_handle)
.and_then(|co| co.parent)
.and_then(|p| bodies.get(p.handle))
.and_then(|p| queries.bodies.get(p.handle))
.map(|b| b.is_dynamic())
== Some(true)
{
@@ -699,7 +657,7 @@ impl KinematicCharacterController {
return false;
}
filter.flags |= QueryFilterFlags::EXCLUDE_DYNAMIC;
queries.filter.flags |= QueryFilterFlags::EXCLUDE_DYNAMIC;
}
let shifted_character_pos = Translation::from(*self.up * max_height) * character_pos;
@@ -712,8 +670,6 @@ impl KinematicCharacterController {
if queries
.cast_shape(
bodies,
colliders,
character_pos,
&self.up,
character_shape,
@@ -723,7 +679,6 @@ impl KinematicCharacterController {
max_time_of_impact: max_height,
compute_impact_geometry_on_penetration: true,
},
filter,
)
.is_some()
{
@@ -733,8 +688,6 @@ impl KinematicCharacterController {
if queries
.cast_shape(
bodies,
colliders,
&shifted_character_pos,
&horizontal_dir,
character_shape,
@@ -744,7 +697,6 @@ impl KinematicCharacterController {
max_time_of_impact: min_width,
compute_impact_geometry_on_penetration: true,
},
filter,
)
.is_some()
{
@@ -755,8 +707,6 @@ impl KinematicCharacterController {
// Check that we are not getting into a ramp that is too steep
// after stepping.
if let Some((_, hit)) = queries.cast_shape(
bodies,
colliders,
&(Translation::from(horizontal_dir * min_width) * shifted_character_pos),
&-self.up,
character_shape,
@@ -766,7 +716,6 @@ impl KinematicCharacterController {
max_time_of_impact: max_height,
compute_impact_geometry_on_penetration: true,
},
filter,
) {
let [vertical_slope_translation, horizontal_slope_translation] = self
.split_into_components(translation_remaining)
@@ -786,8 +735,6 @@ impl KinematicCharacterController {
let step_height = max_height
- queries
.cast_shape(
bodies,
colliders,
&(Translation::from(horizontal_dir * min_width) * shifted_character_pos),
&-self.up,
character_shape,
@@ -797,7 +744,6 @@ impl KinematicCharacterController {
max_time_of_impact: max_height,
compute_impact_geometry_on_penetration: true,
},
filter,
)
.map(|hit| hit.1.time_of_impact)
.unwrap_or(max_height);
@@ -824,24 +770,18 @@ impl KinematicCharacterController {
pub fn solve_character_collision_impulses<'a>(
&self,
dt: Real,
bodies: &mut RigidBodySet,
colliders: &ColliderSet,
queries: &QueryPipeline,
mut queries: QueryPipelineMut,
character_shape: &dyn Shape,
character_mass: Real,
collisions: impl IntoIterator<Item = &'a CharacterCollision>,
filter: QueryFilter,
) {
for collision in collisions {
self.solve_single_character_collision_impulse(
dt,
bodies,
colliders,
queries,
&mut queries,
character_shape,
character_mass,
collision,
filter,
);
}
}
@@ -854,13 +794,10 @@ impl KinematicCharacterController {
fn solve_single_character_collision_impulse(
&self,
dt: Real,
bodies: &mut RigidBodySet,
colliders: &ColliderSet,
queries: &QueryPipeline,
queries: &mut QueryPipelineMut,
character_shape: &dyn Shape,
character_mass: Real,
collision: &CharacterCollision,
filter: QueryFilter,
) {
let extents = character_shape.compute_local_aabb().extents();
let up_extent = extents.dot(&self.up.abs());
@@ -876,41 +813,39 @@ impl KinematicCharacterController {
.compute_aabb(&collision.character_pos)
.loosened(prediction);
queries.colliders_with_aabb_intersecting_aabb(&character_aabb, |handle| {
if let Some(collider) = colliders.get(*handle) {
if let Some(parent) = collider.parent {
if filter.test(bodies, *handle, collider) {
if let Some(body) = bodies.get(parent.handle) {
if body.is_dynamic() {
manifolds.clear();
let pos12 = collision.character_pos.inv_mul(collider.position());
let prev_manifolds_len = manifolds.len();
let _ = dispatcher.contact_manifolds(
&pos12,
character_shape,
collider.shape(),
prediction,
&mut manifolds,
&mut None,
);
for (_, collider) in queries
.as_ref()
.intersect_aabb_conservative(&character_aabb)
{
if let Some(parent) = collider.parent {
if let Some(body) = queries.bodies.get(parent.handle) {
if body.is_dynamic() {
manifolds.clear();
let pos12 = collision.character_pos.inv_mul(collider.position());
let prev_manifolds_len = manifolds.len();
let _ = dispatcher.contact_manifolds(
&pos12,
character_shape,
collider.shape(),
prediction,
&mut manifolds,
&mut None,
);
for m in &mut manifolds[prev_manifolds_len..] {
m.data.rigid_body2 = Some(parent.handle);
m.data.normal = collision.character_pos * m.local_n1;
}
}
for m in &mut manifolds[prev_manifolds_len..] {
m.data.rigid_body2 = Some(parent.handle);
m.data.normal = collision.character_pos * m.local_n1;
}
}
}
}
true
});
}
let velocity_to_transfer = movement_to_transfer * utils::inv(dt);
for manifold in &manifolds {
let body_handle = manifold.data.rigid_body2.unwrap();
let body = &mut bodies[body_handle];
let body = &mut queries.bodies[body_handle];
for pt in &manifold.points {
if pt.dist <= prediction {
@@ -953,10 +888,9 @@ mod test {
let mut impulse_joints = ImpulseJointSet::new();
let mut multibody_joints = MultibodyJointSet::new();
let mut pipeline = PhysicsPipeline::new();
let mut bf = BroadPhaseMultiSap::new();
let mut bf = BroadPhaseBvh::new();
let mut nf = NarrowPhase::new();
let mut islands = IslandManager::new();
let mut query_pipeline = QueryPipeline::new();
let mut bodies = RigidBodySet::new();
@@ -1019,8 +953,23 @@ mod test {
let character_shape = collider.shape();
colliders.insert_with_parent(collider.clone(), character_handle_cannot_climb, &mut bodies);
query_pipeline.update(&colliders);
for i in 0..200 {
// Step once
pipeline.step(
&gravity,
&integration_parameters,
&mut islands,
&mut bf,
&mut nf,
&mut bodies,
&mut colliders,
&mut impulse_joints,
&mut multibody_joints,
&mut CCDSolver::new(),
&(),
&(),
);
let mut update_character_controller =
|controller: KinematicCharacterController, handle: RigidBodyHandle| {
let character_body = bodies.get(handle).unwrap();
@@ -1028,15 +977,18 @@ mod test {
// the character is being moved.
let mut collisions = vec![];
let filter_character_controller = QueryFilter::new().exclude_rigid_body(handle);
let effective_movement = controller.move_shape(
integration_parameters.dt,
let query_pipeline = bf.as_query_pipeline(
nf.query_dispatcher(),
&bodies,
&colliders,
filter_character_controller,
);
let effective_movement = controller.move_shape(
integration_parameters.dt,
&query_pipeline,
character_shape,
character_body.position(),
Vector::new(0.1, -0.1, 0.0),
filter_character_controller,
|collision| collisions.push(collision),
);
let character_body = bodies.get_mut(handle).unwrap();
@@ -1044,7 +996,8 @@ mod test {
assert!(
effective_movement.grounded,
"movement should be grounded at all times for current setup (iter: {}), pos: {}.",
i, translation + effective_movement.translation
i,
translation + effective_movement.translation
);
character_body.set_next_kinematic_translation(
translation + effective_movement.translation,
@@ -1064,22 +1017,6 @@ mod test {
character_handle_cannot_climb,
);
update_character_controller(character_controller_can_climb, character_handle_can_climb);
// Step once
pipeline.step(
&gravity,
&integration_parameters,
&mut islands,
&mut bf,
&mut nf,
&mut bodies,
&mut colliders,
&mut impulse_joints,
&mut multibody_joints,
&mut CCDSolver::new(),
Some(&mut query_pipeline),
&(),
&(),
);
}
let character_body = bodies.get(character_handle_can_climb).unwrap();
assert!(character_body.translation().x > 6.0);
@@ -1095,10 +1032,9 @@ mod test {
let mut impulse_joints = ImpulseJointSet::new();
let mut multibody_joints = MultibodyJointSet::new();
let mut pipeline = PhysicsPipeline::new();
let mut bf = BroadPhaseMultiSap::new();
let mut bf = BroadPhaseBvh::new();
let mut nf = NarrowPhase::new();
let mut islands = IslandManager::new();
let mut query_pipeline = QueryPipeline::new();
let mut bodies = RigidBodySet::new();
@@ -1146,40 +1082,7 @@ mod test {
let character_shape = collider.shape();
colliders.insert_with_parent(collider.clone(), character_handle_no_snap, &mut bodies);
query_pipeline.update(&colliders);
for i in 0..10000 {
let mut update_character_controller =
|controller: KinematicCharacterController, handle: RigidBodyHandle| {
let character_body = bodies.get(handle).unwrap();
// Use a closure to handle or collect the collisions while
// the character is being moved.
let mut collisions = vec![];
let filter_character_controller = QueryFilter::new().exclude_rigid_body(handle);
let effective_movement = controller.move_shape(
integration_parameters.dt,
&bodies,
&colliders,
&query_pipeline,
character_shape,
character_body.position(),
Vector::new(0.1, -0.1, 0.1),
filter_character_controller,
|collision| collisions.push(collision),
);
let character_body = bodies.get_mut(handle).unwrap();
let translation = character_body.translation();
assert!(
effective_movement.grounded,
"movement should be grounded at all times for current setup (iter: {}), pos: {}.",
i, translation + effective_movement.translation
);
character_body.set_next_kinematic_translation(
translation + effective_movement.translation,
);
};
update_character_controller(character_controller_no_snap, character_handle_no_snap);
update_character_controller(character_controller_snap, character_handle_snap);
// Step once
pipeline.step(
&gravity,
@@ -1192,11 +1095,48 @@ mod test {
&mut impulse_joints,
&mut multibody_joints,
&mut CCDSolver::new(),
Some(&mut query_pipeline),
&(),
&(),
);
let mut update_character_controller =
|controller: KinematicCharacterController, handle: RigidBodyHandle| {
let character_body = bodies.get(handle).unwrap();
// Use a closure to handle or collect the collisions while
// the character is being moved.
let mut collisions = vec![];
let filter_character_controller = QueryFilter::new().exclude_rigid_body(handle);
let query_pipeline = bf.as_query_pipeline(
nf.query_dispatcher(),
&bodies,
&colliders,
filter_character_controller,
);
let effective_movement = controller.move_shape(
integration_parameters.dt,
&query_pipeline,
character_shape,
character_body.position(),
Vector::new(0.1, -0.1, 0.1),
|collision| collisions.push(collision),
);
let character_body = bodies.get_mut(handle).unwrap();
let translation = character_body.translation();
assert!(
effective_movement.grounded,
"movement should be grounded at all times for current setup (iter: {}), pos: {}.",
i,
translation + effective_movement.translation
);
character_body.set_next_kinematic_translation(
translation + effective_movement.translation,
);
};
update_character_controller(character_controller_no_snap, character_handle_no_snap);
update_character_controller(character_controller_snap, character_handle_snap);
}
let character_body = bodies.get_mut(character_handle_no_snap).unwrap();
let translation = character_body.translation();