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

QueryPipeline: add shape casting, point projection, and intersection queries.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-12-31 16:30:38 +01:00
parent 1feac2e02d
commit 7b098606c2
4 changed files with 216 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/geometry/mod.rs
View File

@@ -86,6 +86,15 @@ pub(crate) use self::narrow_phase::ContactManifoldIndex;
pub(crate) use cdl::partitioning::SimdQuadTree;
pub use cdl::shape::*;
pub(crate) fn default_persistent_query_dispatcher(
) -> std::sync::Arc<dyn cdl::query::PersistentQueryDispatcher<ContactManifoldData, ContactData>> {
std::sync::Arc::new(cdl::query::DefaultQueryDispatcher)
}
pub(crate) fn default_query_dispatcher() -> std::sync::Arc<dyn cdl::query::QueryDispatcher> {
std::sync::Arc::new(cdl::query::DefaultQueryDispatcher)
}
mod broad_phase_multi_sap;
mod collider;
mod collider_set;

7
src/geometry/narrow_phase.rs
View File

@@ -38,7 +38,7 @@ impl ColliderGraphIndices {
pub struct NarrowPhase {
#[cfg_attr(
feature = "serde-serialize",
serde(skip, default = "default_query_dispatcher")
serde(skip, default = "crate::geometry::default_persistent_query_dispatcher")
)]
query_dispatcher: Arc<dyn PersistentQueryDispatcher<ContactManifoldData, ContactData>>,
contact_graph: InteractionGraph<ContactPair>,
@@ -47,11 +47,6 @@ pub struct NarrowPhase {
removed_colliders: Option<Subscription<RemovedCollider>>,
}
fn default_query_dispatcher() -> Arc<dyn PersistentQueryDispatcher<ContactManifoldData, ContactData>>
{
Arc::new(DefaultQueryDispatcher)
}
pub(crate) type ContactManifoldIndex = usize;
impl NarrowPhase {

239
src/pipeline/query_pipeline.rs
View File

@@ -1,15 +1,68 @@
use crate::cdl::motion::RigidMotion;
use crate::dynamics::RigidBodySet;
use crate::geometry::{
Collider, ColliderHandle, ColliderSet, InteractionGroups, Ray, RayIntersection, SimdQuadTree,
Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray,
RayIntersection, SimdQuadTree,
};
use crate::math::{Isometry, Point, Real, Vector};
use cdl::query::details::{
IntersectionCompositeShapeShapeBestFirstVisitor,
NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor,
PointCompositeShapeProjWithFeatureBestFirstVisitor,
RayCompositeShapeToiAndNormalBestFirstVisitor, RayCompositeShapeToiBestFirstVisitor,
TOICompositeShapeShapeBestFirstVisitor,
};
use cdl::query::{DefaultQueryDispatcher, QueryDispatcher, TOI};
use cdl::shape::{FeatureId, Shape, TypedSimdCompositeShape};
use std::sync::Arc;
/// A pipeline for performing queries on all the colliders of a scene.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct QueryPipeline {
#[cfg_attr(
feature = "serde-serialize",
serde(skip, default = "crate::geometry::default_query_dispatcher")
)]
query_dispatcher: Arc<dyn QueryDispatcher>,
quadtree: SimdQuadTree<ColliderHandle>,
tree_built: bool,
dilation_factor: f32,
dilation_factor: Real,
}
struct QueryPipelineAsCompositeShape<'a> {
query_pipeline: &'a QueryPipeline,
colliders: &'a ColliderSet,
groups: InteractionGroups,
}
impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
type PartShape = dyn Shape;
type PartId = ColliderHandle;
fn map_typed_part_at(
&self,
shape_id: Self::PartId,
mut f: impl FnMut(Option<&Isometry<Real>>, &Self::PartShape),
) {
if let Some(collider) = self.colliders.get(shape_id) {
if collider.collision_groups.test(self.groups) {
f(Some(collider.position()), collider.shape())
}
}
}
fn map_untyped_part_at(
&self,
shape_id: Self::PartId,
mut f: impl FnMut(Option<&Isometry<Real>>, &Self::PartShape),
) {
self.map_typed_part_at(shape_id, f);
}
fn typed_quadtree(&self) -> &SimdQuadTree<ColliderHandle> {
&self.query_pipeline.quadtree
}
}
impl Default for QueryPipeline {
@@ -21,7 +74,27 @@ impl Default for QueryPipeline {
impl QueryPipeline {
/// Initializes an empty query pipeline.
pub fn new() -> Self {
Self::with_query_dispatcher(DefaultQueryDispatcher)
}
fn as_composite_shape<'a>(
&'a self,
colliders: &'a ColliderSet,
groups: InteractionGroups,
) -> QueryPipelineAsCompositeShape<'a> {
QueryPipelineAsCompositeShape {
query_pipeline: self,
colliders,
groups,
}
}
pub fn with_query_dispatcher<D>(d: D) -> Self
where
D: 'static + QueryDispatcher,
{
Self {
query_dispatcher: Arc::new(d),
quadtree: SimdQuadTree::new(),
tree_built: false,
dilation_factor: 0.01,
@@ -59,40 +132,46 @@ impl QueryPipeline {
/// - `position`: the position of this shape.
/// - `ray`: the ray to cast.
/// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
/// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray.
pub fn cast_ray<'a>(
/// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `Real::MAX` for an unbounded ray.
pub fn cast_ray(
&self,
colliders: &'a ColliderSet,
colliders: &ColliderSet,
ray: &Ray,
max_toi: f32,
max_toi: Real,
solid: bool,
groups: InteractionGroups,
) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> {
// TODO: avoid allocation?
let mut inter = Vec::new();
self.quadtree.cast_ray(ray, max_toi, &mut inter);
) -> Option<(ColliderHandle, Real)> {
let pipeline_shape = self.as_composite_shape(colliders, groups);
let mut visitor =
RayCompositeShapeToiBestFirstVisitor::new(&pipeline_shape, ray, max_toi, solid);
let mut best = f32::MAX;
let mut result = None;
self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1)
}
for handle in inter {
if let Some(collider) = colliders.get(handle) {
if collider.collision_groups.test(groups) {
if let Some(inter) = collider.shape().cast_ray_and_get_normal(
collider.position(),
ray,
max_toi,
true,
) {
if inter.toi < best {
best = inter.toi;
result = Some((handle, collider, inter));
}
}
}
}
}
/// Find the closest intersection between a ray and a set of collider.
///
/// # Parameters
/// - `position`: the position of this shape.
/// - `ray`: the ray to cast.
/// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
/// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `Real::MAX` for an unbounded ray.
pub fn cast_ray_and_get_normal(
&self,
colliders: &ColliderSet,
ray: &Ray,
max_toi: Real,
solid: bool,
groups: InteractionGroups,
) -> Option<(ColliderHandle, RayIntersection)> {
let pipeline_shape = self.as_composite_shape(colliders, groups);
let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new(
&pipeline_shape,
ray,
max_toi,
solid,
);
result
self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1)
}
/// Find the all intersections between a ray and a set of collider and passes them to a callback.
@@ -101,7 +180,7 @@ impl QueryPipeline {
/// - `position`: the position of this shape.
/// - `ray`: the ray to cast.
/// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
/// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray.
/// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `Real::MAX` for an unbounded ray.
/// - `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, ignory any further raycast.
@@ -109,7 +188,7 @@ impl QueryPipeline {
&self,
colliders: &'a ColliderSet,
ray: &Ray,
max_toi: f32,
max_toi: Real,
groups: InteractionGroups,
mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool,
) {
@@ -135,18 +214,106 @@ impl QueryPipeline {
}
}
/*
/// Find up to one collider intersecting the given shape.
fn intersection_with_shape(
&self,
colliders: &ColliderSet,
shape_pos: &Isometry<Real>,
shape: &dyn Shape,
groups: InteractionGroups,
) -> Option<ColliderHandle> {
let pipeline_shape = self.as_composite_shape(colliders, groups);
let mut visitor = IntersectionCompositeShapeShapeBestFirstVisitor::new(
&*self.query_dispatcher,
shape_pos,
&pipeline_shape,
shape,
);
self.quadtree
.traverse_best_first(&mut visitor)
.map(|h| (h.1 .0))
}
// TODO: intersections_with_point (collect all colliders containing the point).
/// Projects a point on the scene.
fn project_point(
&self,
colliders: &ColliderSet,
point: &Point<Real>,
solid: bool,
groups: InteractionGroups,
) -> Option<(ColliderHandle, PointProjection)> {
let pipeline_shape = self.as_composite_shape(colliders, groups);
let mut visitor =
PointCompositeShapeProjBestFirstVisitor::new(&pipeline_shape, point, solid);
self.quadtree
.traverse_best_first(&mut visitor)
.map(|h| (h.1 .1, h.1 .0))
}
/// Projects a point on the scene and get
fn project_point_and_get_feature(
&self,
colliders: &ColliderSet,
point: &Point<Real>,
groups: InteractionGroups,
) -> Option<(ColliderHandle, PointProjection, FeatureId)> {
let pipeline_shape = self.as_composite_shape(colliders, groups);
let mut visitor =
PointCompositeShapeProjWithFeatureBestFirstVisitor::new(&pipeline_shape, point, false);
self.quadtree
.traverse_best_first(&mut visitor)
.map(|h| (h.1 .1 .0, h.1 .0, h.1 .1 .1))
}
pub fn cast_shape<'a>(
&self,
colliders: &'a ColliderSet,
shape_pos: &Isometry<Real>,
shape_vel: &Vector<Real>,
shape: &dyn Shape,
max_toi: f32,
max_toi: Real,
target_distance: Real,
groups: InteractionGroups,
) -> Option<(ColliderHandle, &'a Collider, TOI)> {
unimplemented!()
) -> Option<(ColliderHandle, TOI)> {
let pipeline_shape = self.as_composite_shape(colliders, groups);
let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new(
&*self.query_dispatcher,
shape_pos,
shape_vel,
&pipeline_shape,
shape,
max_toi,
target_distance,
);
self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1)
}
pub fn nonlinear_cast_shape(
&self,
colliders: &ColliderSet,
shape_motion: &dyn RigidMotion,
shape: &dyn Shape,
max_toi: Real,
target_distance: Real,
groups: InteractionGroups,
) -> Option<(ColliderHandle, TOI)> {
let pipeline_shape = self.as_composite_shape(colliders, groups);
let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new(
&*self.query_dispatcher,
shape_motion,
&pipeline_shape,
shape,
max_toi,
target_distance,
);
self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1)
}
/*
/// Gets all the colliders with a shape intersecting the given `shape`.
pub fn intersections_with_shape<'a>(
&self,