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

First working version of non-linear CCD based on single-substep motion-clamping.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-03-26 18:16:27 +01:00
parent 326469a1df
commit 97157c9423
29 changed files with 696 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/pipeline/collision_pipeline.rs
View File

@@ -69,21 +69,18 @@ impl CollisionPipeline {
// // Update kinematic bodies velocities.
// bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
// body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
// body.compute_velocity_from_next_position(integration_parameters.inv_dt());
// });
// Update colliders positions and kinematic bodies positions.
bodies.foreach_active_body_mut_internal(|_, rb| {
if rb.is_kinematic() {
rb.position = rb.predicted_position;
} else {
rb.update_predicted_position(0.0);
}
rb.position = rb.next_position;
rb.update_colliders_positions(colliders);
for handle in &rb.colliders {
let collider = &mut colliders[*handle];
collider.prev_position = collider.position;
collider.position = rb.position * collider.delta;
collider.predicted_position = rb.predicted_position * collider.delta;
}
});

25
src/pipeline/physics_pipeline.rs
View File

@@ -3,7 +3,7 @@
use crate::counters::Counters;
#[cfg(not(feature = "parallel"))]
use crate::dynamics::IslandSolver;
use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
use crate::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet};
#[cfg(feature = "parallel")]
use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
use crate::geometry::{
@@ -68,6 +68,7 @@ impl PhysicsPipeline {
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
ccd_solver: Option<&mut CCDSolver>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) {
@@ -81,7 +82,7 @@ impl PhysicsPipeline {
// there to determine if this kinematic body should wake-up dynamic
// bodies it is touching.
bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
body.compute_velocity_from_next_position(integration_parameters.inv_dt());
});
self.counters.stages.collision_detection_time.start();
@@ -218,23 +219,33 @@ impl PhysicsPipeline {
});
}
// Update colliders positions and kinematic bodies positions.
// FIXME: do this in the solver?
// Handle CCD
if let Some(ccd_solver) = ccd_solver {
let impacts = ccd_solver.predict_next_impacts(
integration_parameters,
bodies,
colliders,
integration_parameters.dt,
events,
);
ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts);
}
// Set the rigid-bodies and kinematic bodies to their final position.
bodies.foreach_active_body_mut_internal(|_, rb| {
if rb.is_kinematic() {
rb.position = rb.predicted_position;
rb.linvel = na::zero();
rb.angvel = na::zero();
} else {
rb.update_predicted_position(integration_parameters.dt);
}
rb.position = rb.next_position;
rb.update_colliders_positions(colliders);
});
self.counters.stages.solver_time.pause();
bodies.modified_inactive_set.clear();
self.counters.step_completed();
}
}

74
src/pipeline/query_pipeline.rs
View File

@@ -1,10 +1,9 @@
use crate::dynamics::RigidBodySet;
use crate::geometry::{
Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray,
RayIntersection, SimdQuadTree,
RayIntersection, SimdQuadTree, AABB,
};
use crate::math::{Isometry, Point, Real, Vector};
use crate::parry::motion::RigidMotion;
use parry::query::details::{
IntersectionCompositeShapeShapeBestFirstVisitor,
NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor,
@@ -15,7 +14,7 @@ use parry::query::details::{
use parry::query::visitors::{
BoundingVolumeIntersectionsVisitor, PointIntersectionsVisitor, RayIntersectionsVisitor,
};
use parry::query::{DefaultQueryDispatcher, QueryDispatcher, TOI};
use parry::query::{DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher, TOI};
use parry::shape::{FeatureId, Shape, TypedSimdCompositeShape};
use std::sync::Arc;
@@ -95,7 +94,7 @@ impl QueryPipeline {
/// Initializes an empty query pipeline with a custom `QueryDispatcher`.
///
/// Use this constructor in order to use a custom `QueryDispatcher` that is
/// awary of your own user-defined shapes.
/// aware of your own user-defined shapes.
pub fn with_query_dispatcher<D>(d: D) -> Self
where
D: 'static + QueryDispatcher,
@@ -108,11 +107,26 @@ impl QueryPipeline {
}
}
/// The query dispatcher used by this query pipeline for running scene queries.
pub fn query_dispatcher(&self) -> &dyn QueryDispatcher {
&*self.query_dispatcher
}
/// Update the acceleration structure on the query pipeline.
pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) {
pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet, use_swept_aabb: bool) {
if !self.tree_built {
let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
self.quadtree.clear_and_rebuild(data, self.dilation_factor);
if !use_swept_aabb {
let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
self.quadtree.clear_and_rebuild(data, self.dilation_factor);
} else {
let data = colliders.iter().map(|(h, co)| {
let next_position =
bodies[co.parent()].next_position * co.position_wrt_parent();
(h, co.compute_swept_aabb(&next_position))
});
self.quadtree.clear_and_rebuild(data, self.dilation_factor);
}
// FIXME: uncomment this once we handle insertion/removals properly.
// self.tree_built = true;
return;
@@ -127,10 +141,22 @@ impl QueryPipeline {
}
}
self.quadtree.update(
|handle| colliders[*handle].compute_aabb(),
self.dilation_factor,
);
if !use_swept_aabb {
self.quadtree.update(
|handle| colliders[*handle].compute_aabb(),
self.dilation_factor,
);
} else {
self.quadtree.update(
|handle| {
let co = &colliders[*handle];
let next_position =
bodies[co.parent()].next_position * co.position_wrt_parent();
co.compute_swept_aabb(&next_position)
},
self.dilation_factor,
);
}
}
/// Find the closest intersection between a ray and a set of collider.
@@ -336,6 +362,16 @@ impl QueryPipeline {
.map(|h| (h.1 .1 .0, h.1 .0, h.1 .1 .1))
}
/// Finds all handles of all the colliders with an AABB intersecting the given AABB.
pub fn colliders_with_aabb_intersecting_aabb(
&self,
aabb: &AABB,
mut callback: impl FnMut(&ColliderHandle) -> bool,
) {
let mut visitor = BoundingVolumeIntersectionsVisitor::new(aabb, &mut callback);
self.quadtree.traverse_depth_first(&mut visitor);
}
/// Casts a shape at a constant linear velocity and retrieve the first collider it hits.
///
/// This is similar to ray-casting except that we are casting a whole shape instead of
@@ -386,20 +422,24 @@ impl QueryPipeline {
pub fn nonlinear_cast_shape(
&self,
colliders: &ColliderSet,
shape_motion: &dyn RigidMotion,
shape_motion: &NonlinearRigidMotion,
shape: &dyn Shape,
max_toi: Real,
target_distance: Real,
start_time: Real,
end_time: Real,
stop_at_penetration: bool,
groups: InteractionGroups,
) -> Option<(ColliderHandle, TOI)> {
let pipeline_shape = self.as_composite_shape(colliders, groups);
let pipeline_motion = NonlinearRigidMotion::identity();
let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new(
&*self.query_dispatcher,
shape_motion,
&pipeline_motion,
&pipeline_shape,
shape_motion,
shape,
max_toi,
target_distance,
start_time,
end_time,
stop_at_penetration,
);
self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1)
}