avian2d/collision/collider/parry/
contact_query.rs

1//! Geometric queries for computing information about contacts between two [`Collider`]s.
2//!
3//! This module contains the following contact queries:
4//!
5//! | Contact query         | Description                                                               |
6//! | --------------------- | ------------------------------------------------------------------------- |
7//! | [`contact`]           | Computes one pair of contact points between two [`Collider`]s.            |
8//! | [`contact_manifolds`] | Computes all [`ContactManifold`]s between two [`Collider`]s.              |
9//! | [`closest_points`]    | Computes the closest points between two [`Collider`]s.                    |
10//! | [`distance`]          | Computes the minimum distance separating two [`Collider`]s.               |
11//! | [`intersection_test`] | Tests whether two [`Collider`]s are intersecting each other.              |
12//! | [`time_of_impact`]    | Computes when two moving [`Collider`]s hit each other for the first time. |
13//!
14//! For geometric queries that query the entire world for intersections, like raycasting, shapecasting
15//! and point projection, see [spatial queries](spatial_query).
16
17use crate::{collision::contact_types::SingleContact, prelude::*};
18use bevy::prelude::*;
19use parry::query::{PersistentQueryDispatcher, ShapeCastOptions, Unsupported};
20
21/// An error indicating that a [contact query](self) is not supported for one of the [`Collider`] shapes.
22pub type UnsupportedShape = Unsupported;
23
24/// Computes one pair of contact points between two [`Collider`]s.
25///
26/// Returns `None` if the colliders are separated by a distance greater than `prediction_distance`
27/// or if the given shapes are invalid.
28///
29/// # Example
30///
31/// ```
32/// # #[cfg(feature = "2d")]
33/// # use avian2d::{collision::collider::contact_query::contact, prelude::*};
34/// # #[cfg(feature = "3d")]
35/// use avian3d::{collision::collider::contact_query::contact, prelude::*};
36/// use bevy::prelude::*;
37///
38/// # #[cfg(all(feature = "3d", feature = "f32"))]
39/// # {
40/// let collider1 = Collider::sphere(0.5);
41/// let collider2 = Collider::cuboid(1.0, 1.0, 1.0);
42///
43/// // Compute a contact that should have a penetration depth of 0.5
44/// let contact = contact(
45///     // First collider
46///     &collider1,
47///     Vec3::default(),
48///     Quat::default(),
49///     // Second collider
50///     &collider2,
51///     Vec3::X * 0.5,
52///     Quat::default(),
53///     // Prediction distance
54///     0.0,
55/// )
56/// .expect("Unsupported collider shape");
57///
58/// assert_eq!(
59///     contact.is_some_and(|contact| contact.penetration == 0.5),
60///     true
61/// );
62/// # }
63/// ```
64pub fn contact(
65    collider1: &Collider,
66    position1: impl Into<Position>,
67    rotation1: impl Into<Rotation>,
68    collider2: &Collider,
69    position2: impl Into<Position>,
70    rotation2: impl Into<Rotation>,
71    prediction_distance: Scalar,
72) -> Result<Option<SingleContact>, UnsupportedShape> {
73    let rotation1: Rotation = rotation1.into();
74    let rotation2: Rotation = rotation2.into();
75    let isometry1 = make_pose(position1, rotation1);
76    let isometry2 = make_pose(position2, rotation2);
77
78    parry::query::contact(
79        &isometry1,
80        collider1.shape_scaled().0.as_ref(),
81        &isometry2,
82        collider2.shape_scaled().0.as_ref(),
83        prediction_distance,
84    )
85    .map(|contact| {
86        if let Some(contact) = contact {
87            // Transform contact data into local space
88            let point1: Vector = rotation1.inverse() * contact.point1;
89            let point2: Vector = rotation2.inverse() * contact.point2;
90            let normal1: Vector = (rotation1.inverse() * contact.normal1).normalize();
91            let normal2: Vector = (rotation2.inverse() * contact.normal2).normalize();
92
93            // Make sure the normals are valid
94            if !normal1.is_normalized() || !normal2.is_normalized() {
95                return None;
96            }
97
98            Some(SingleContact::new(
99                point1,
100                point2,
101                normal1,
102                normal2,
103                -contact.dist,
104            ))
105        } else {
106            None
107        }
108    })
109}
110
111// TODO: Add a persistent version of this that tries to reuse previous contact manifolds
112// by exploiting spatial and temporal coherence. This is supported by Parry's contact_manifolds,
113// but requires using Parry's `ContactManifold` type.
114/// Computes all [`ContactManifold`]s between two [`Collider`]s.
115///
116/// The contact points are expressed in world space, relative to the origin
117/// of the first and second shape respectively.
118///
119/// Returns an empty vector if the colliders are separated by a distance greater than `prediction_distance`
120/// or if the given shapes are invalid.
121///
122/// # Example
123///
124/// ```
125/// # #[cfg(feature = "2d")]
126/// # use avian2d::{collision::collider::contact_query::contact_manifolds, prelude::*};
127/// # #[cfg(feature = "3d")]
128/// use avian3d::{collision::collider::contact_query::contact_manifolds, prelude::*};
129/// use bevy::prelude::*;
130///
131/// # #[cfg(all(feature = "3d", feature = "f32"))]
132/// # {
133/// let collider1 = Collider::sphere(0.5);
134/// let collider2 = Collider::cuboid(1.0, 1.0, 1.0);
135///
136/// // Compute contact manifolds a collision that should be penetrating
137/// let mut manifolds = Vec::new();
138/// contact_manifolds(
139///     // First collider
140///     &collider1,
141///     Vec3::default(),
142///     Quat::default(),
143///     // Second collider
144///     &collider2,
145///     Vec3::X * 0.25,
146///     Quat::default(),
147///     // Prediction distance
148///     0.0,
149///     // Output manifolds
150///     &mut manifolds,
151/// );
152///
153/// assert_eq!(manifolds.is_empty(), false);
154/// # }
155/// ```
156pub fn contact_manifolds(
157    collider1: &Collider,
158    position1: impl Into<Position>,
159    rotation1: impl Into<Rotation>,
160    collider2: &Collider,
161    position2: impl Into<Position>,
162    rotation2: impl Into<Rotation>,
163    prediction_distance: Scalar,
164    manifolds: &mut Vec<ContactManifold>,
165) {
166    let position1: Position = position1.into();
167    let position2: Position = position2.into();
168    let rotation1: Rotation = rotation1.into();
169    let rotation2: Rotation = rotation2.into();
170    let isometry1 = make_pose(position1, rotation1);
171    let isometry2 = make_pose(position2, rotation2);
172    let isometry12 = isometry1.inv_mul(&isometry2);
173
174    // TODO: Reuse manifolds from previous frame to improve performance
175    let mut new_manifolds =
176        Vec::<parry::query::ContactManifold<(), ()>>::with_capacity(manifolds.len());
177    let result = parry::query::DefaultQueryDispatcher.contact_manifolds(
178        &isometry12,
179        collider1.shape_scaled().0.as_ref(),
180        collider2.shape_scaled().0.as_ref(),
181        prediction_distance,
182        &mut new_manifolds,
183        &mut None,
184    );
185
186    // Clear the old manifolds.
187    manifolds.clear();
188
189    // Fall back to support map contacts for unsupported (custom) shapes.
190    if result.is_err()
191        && let (Some(shape1), Some(shape2)) = (
192            collider1.shape_scaled().as_support_map(),
193            collider2.shape_scaled().as_support_map(),
194        )
195        && let Some(contact) = parry::query::contact::contact_support_map_support_map(
196            &isometry12,
197            shape1,
198            shape2,
199            prediction_distance,
200        )
201    {
202        let normal = rotation1 * contact.normal1;
203
204        // Make sure the normal is valid
205        if !normal.is_normalized() {
206            return;
207        }
208
209        let local_point1: Vector = contact.point1;
210
211        // The contact point is the midpoint of the two points in world space.
212        // The anchors are relative to the positions of the colliders.
213        let point1 = rotation1 * local_point1;
214        let anchor1 = point1 + normal * contact.dist * 0.5;
215        let anchor2 = anchor1 + (position1.0 - position2.0);
216        let world_point = position1.0 + anchor1;
217        let points = [ContactPoint::new(
218            anchor1,
219            anchor2,
220            world_point,
221            -contact.dist,
222        )];
223
224        manifolds.push(ContactManifold::new(points, normal));
225    }
226
227    manifolds.extend(new_manifolds.iter().filter_map(|manifold| {
228        // Skip empty manifolds.
229        if manifold.contacts().is_empty() {
230            return None;
231        }
232
233        let subpos1 = manifold.subshape_pos1.unwrap_or_default();
234        let local_normal: Vector = (subpos1.rotation * manifold.local_n1).normalize();
235        let normal = rotation1 * local_normal;
236
237        // Make sure the normal is valid
238        if !normal.is_normalized() {
239            return None;
240        }
241
242        let points = manifold.contacts().iter().map(|contact| {
243            // The contact point is the midpoint of the two points in world space.
244            // The anchors are relative to the positions of the colliders.
245            let point1 = rotation1 * subpos1.transform_point(contact.local_p1);
246            let anchor1 = point1 + normal * contact.dist * 0.5;
247            let anchor2 = anchor1 + (position1.0 - position2.0);
248            let world_point = position1.0 + anchor1;
249            ContactPoint::new(anchor1, anchor2, world_point, -contact.dist)
250                .with_feature_ids(contact.fid1.into(), contact.fid2.into())
251        });
252
253        let manifold = ContactManifold::new(points, normal);
254
255        Some(manifold)
256    }));
257}
258
259/// Information about the closest points between two [`Collider`]s.
260///
261/// The closest points can be computed using [`closest_points`].
262#[derive(Reflect, Clone, Copy, Debug, PartialEq)]
263#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
264#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))]
265#[reflect(Debug, PartialEq)]
266pub enum ClosestPoints {
267    /// The two shapes are intersecting each other.
268    Intersecting,
269    /// The two shapes are not intersecting each other but the distance between the closest points
270    /// is below the user-defined maximum distance.
271    ///
272    /// The points are expressed in world space.
273    WithinMargin(Vector, Vector),
274    /// The two shapes are not intersecting each other and the distance between the closest points
275    /// exceeds the user-defined maximum distance.
276    OutsideMargin,
277}
278
279/// Computes the [`ClosestPoints`] between two [`Collider`]s.
280///
281/// Returns `Err(UnsupportedShape)` if either of the collider shapes is not supported.
282///
283/// # Example
284///
285/// ```
286/// # #[cfg(feature = "2d")]
287/// # use avian2d::{collision::collider::contact_query::*, prelude::*};
288/// # #[cfg(feature = "3d")]
289/// use avian3d::{collision::collider::contact_query::*, prelude::*};
290/// use bevy::prelude::*;
291///
292/// # #[cfg(all(feature = "3d", feature = "f32"))]
293/// # {
294/// let collider1 = Collider::sphere(0.5);
295/// let collider2 = Collider::cuboid(1.0, 1.0, 1.0);
296///
297/// // The shapes are intersecting
298/// assert_eq!(
299///     closest_points(
300///         &collider1,
301///         Vec3::default(),
302///         Quat::default(),
303///         &collider2,
304///         Vec3::default(),
305///         Quat::default(),
306///         2.0,
307///     )
308///     .expect("Unsupported collider shape"),
309///     ClosestPoints::Intersecting,
310/// );
311///
312/// // The shapes are not intersecting but the distance between the closest points is below 2.0
313/// assert_eq!(
314///     closest_points(
315///         &collider1,
316///         Vec3::default(),
317///         Quat::default(),
318///         &collider2,
319///         Vec3::X * 1.5,
320///         Quat::default(),
321///         2.0,
322///     )
323///     .expect("Unsupported collider shape"),
324///     ClosestPoints::WithinMargin(Vec3::X * 0.5, Vec3::X * 1.0),
325/// );
326///
327/// // The shapes are not intersecting and the distance between the closest points exceeds 2.0
328/// assert_eq!(
329///     closest_points(
330///         &collider1,
331///         Vec3::default(),
332///         Quat::default(),
333///         &collider2,
334///         Vec3::X * 5.0,
335///         Quat::default(),
336///         2.0,
337///     )
338///     .expect("Unsupported collider shape"),
339///     ClosestPoints::OutsideMargin,
340/// );
341/// # }
342/// ```
343pub fn closest_points(
344    collider1: &Collider,
345    position1: impl Into<Position>,
346    rotation1: impl Into<Rotation>,
347    collider2: &Collider,
348    position2: impl Into<Position>,
349    rotation2: impl Into<Rotation>,
350    max_distance: Scalar,
351) -> Result<ClosestPoints, UnsupportedShape> {
352    let rotation1: Rotation = rotation1.into();
353    let rotation2: Rotation = rotation2.into();
354    let isometry1 = make_pose(position1, rotation1);
355    let isometry2 = make_pose(position2, rotation2);
356
357    parry::query::closest_points(
358        &isometry1,
359        collider1.shape_scaled().0.as_ref(),
360        &isometry2,
361        collider2.shape_scaled().0.as_ref(),
362        max_distance,
363    )
364    .map(|closest_points| match closest_points {
365        parry::query::ClosestPoints::Intersecting => ClosestPoints::Intersecting,
366        parry::query::ClosestPoints::WithinMargin(point1, point2) => {
367            ClosestPoints::WithinMargin(point1, point2)
368        }
369        parry::query::ClosestPoints::Disjoint => ClosestPoints::OutsideMargin,
370    })
371}
372
373/// Computes the minimum distance separating two [`Collider`]s.
374///
375/// Returns `0.0` if the colliders are touching or penetrating, and `Err(UnsupportedShape)`
376/// if either of the collider shapes is not supported.
377///
378/// # Example
379///
380/// ```
381/// # #[cfg(feature = "2d")]
382/// # use avian2d::{collision::collider::contact_query::distance, prelude::*};
383/// # #[cfg(feature = "3d")]
384/// use avian3d::{collision::collider::contact_query::distance, prelude::*};
385/// use bevy::prelude::*;
386///
387/// # #[cfg(all(feature = "3d", feature = "f32"))]
388/// # {
389/// let collider1 = Collider::sphere(0.5);
390/// let collider2 = Collider::cuboid(1.0, 1.0, 1.0);
391///
392/// // The distance is 1.0
393/// assert_eq!(
394///     distance(
395///         &collider1,
396///         Vec3::default(),
397///         Quat::default(),
398///         &collider2,
399///         Vec3::X * 2.0,
400///         Quat::default(),
401///     )
402///     .expect("Unsupported collider shape"),
403///     1.0,
404/// );
405///
406/// // The colliders are penetrating, so the distance is 0.0
407/// assert_eq!(
408///     distance(
409///         &collider1,
410///         Vec3::default(),
411///         Quat::default(),
412///         &collider2,
413///         Vec3::default(),
414///         Quat::default(),
415///     )
416///     .expect("Unsupported collider shape"),
417///     0.0,
418/// );
419/// # }
420/// ```
421pub fn distance(
422    collider1: &Collider,
423    position1: impl Into<Position>,
424    rotation1: impl Into<Rotation>,
425    collider2: &Collider,
426    position2: impl Into<Position>,
427    rotation2: impl Into<Rotation>,
428) -> Result<Scalar, UnsupportedShape> {
429    let rotation1: Rotation = rotation1.into();
430    let rotation2: Rotation = rotation2.into();
431    let isometry1 = make_pose(position1, rotation1);
432    let isometry2 = make_pose(position2, rotation2);
433
434    parry::query::distance(
435        &isometry1,
436        collider1.shape_scaled().0.as_ref(),
437        &isometry2,
438        collider2.shape_scaled().0.as_ref(),
439    )
440}
441
442/// Tests whether two [`Collider`]s are intersecting each other.
443///
444/// Returns `Err(UnsupportedShape)` if either of the collider shapes is not supported.
445///
446/// # Example
447///
448/// ```
449/// # #[cfg(feature = "2d")]
450/// # use avian2d::{collision::collider::contact_query::intersection_test, prelude::*};
451/// # #[cfg(feature = "3d")]
452/// use avian3d::{collision::collider::contact_query::intersection_test, prelude::*};
453/// use bevy::prelude::*;
454///
455/// # #[cfg(all(feature = "3d", feature = "f32"))]
456/// # {
457/// let collider1 = Collider::sphere(0.5);
458/// let collider2 = Collider::cuboid(1.0, 1.0, 1.0);
459///
460/// // These colliders should be intersecting
461/// assert_eq!(
462///     intersection_test(
463///         &collider1,
464///         Vec3::default(),
465///         Quat::default(),
466///         &collider2,
467///         Vec3::default(),
468///         Quat::default(),
469///     )
470///     .expect("Unsupported collider shape"),
471///     true,
472/// );
473///
474/// // These colliders shouldn't be intersecting
475/// assert_eq!(
476///     intersection_test(
477///         &collider1,
478///         Vec3::default(),
479///         Quat::default(),
480///         &collider2,
481///         Vec3::X * 5.0,
482///         Quat::default(),
483///     )
484///     .expect("Unsupported collider shape"),
485///     false,
486/// );
487/// # }
488/// ```
489pub fn intersection_test(
490    collider1: &Collider,
491    position1: impl Into<Position>,
492    rotation1: impl Into<Rotation>,
493    collider2: &Collider,
494    position2: impl Into<Position>,
495    rotation2: impl Into<Rotation>,
496) -> Result<bool, UnsupportedShape> {
497    let rotation1: Rotation = rotation1.into();
498    let rotation2: Rotation = rotation2.into();
499    let isometry1 = make_pose(position1, rotation1);
500    let isometry2 = make_pose(position2, rotation2);
501
502    parry::query::intersection_test(
503        &isometry1,
504        collider1.shape_scaled().0.as_ref(),
505        &isometry2,
506        collider2.shape_scaled().0.as_ref(),
507    )
508}
509
510/// The way the [time of impact](time_of_impact) computation was terminated.
511pub type TimeOfImpactStatus = parry::query::details::ShapeCastStatus;
512
513/// The result of a [time of impact](time_of_impact) computation between two moving [`Collider`]s.
514#[derive(Clone, Copy, Debug, PartialEq)]
515pub struct TimeOfImpact {
516    /// The time at which the colliders come into contact.
517    pub time_of_impact: Scalar,
518    /// The closest point on the first collider, at the time of impact,
519    /// expressed in local space.
520    pub point1: Vector,
521    /// The closest point on the second collider, at the time of impact,
522    /// expressed in local space.
523    pub point2: Vector,
524    /// The outward normal on the first collider, at the time of impact,
525    /// expressed in local space.
526    pub normal1: Vector,
527    /// The outward normal on the second collider, at the time of impact,
528    /// expressed in local space.
529    pub normal2: Vector,
530    /// The way the time of impact computation was terminated.
531    pub status: TimeOfImpactStatus,
532}
533
534/// Computes when two moving [`Collider`]s hit each other for the first time.
535///
536/// Returns `Ok(None)` if the time of impact is greater than `max_time_of_impact`
537/// and `Err(UnsupportedShape)` if either of the collider shapes is not supported.
538///
539/// # Example
540///
541/// ```
542/// # #[cfg(feature = "2d")]
543/// # use avian2d::{collision::collider::contact_query::time_of_impact, prelude::*};
544/// # #[cfg(feature = "3d")]
545/// use avian3d::{collision::collider::contact_query::time_of_impact, prelude::*};
546/// use bevy::prelude::*;
547///
548/// # #[cfg(all(feature = "3d", feature = "f32"))]
549/// # {
550/// let collider1 = Collider::sphere(0.5);
551/// let collider2 = Collider::cuboid(1.0, 1.0, 1.0);
552///
553/// let result = time_of_impact(
554///     &collider1,        // Collider 1
555///     Vec3::NEG_X * 5.0, // Position 1
556///     Quat::default(),   // Rotation 1
557///     Vec3::X,           // Linear velocity 1
558///     &collider2,        // Collider 2
559///     Vec3::X * 5.0,     // Position 2
560///     Quat::default(),   // Rotation 2
561///     Vec3::NEG_X,       // Linear velocity 2
562///     100.0,             // Maximum time of impact
563/// )
564/// .expect("Unsupported collider shape");
565///
566/// assert_eq!(result.unwrap().time_of_impact, 4.5);
567/// # }
568/// ```
569#[allow(clippy::too_many_arguments, clippy::type_complexity)]
570pub fn time_of_impact(
571    collider1: &Collider,
572    position1: impl Into<Position>,
573    rotation1: impl Into<Rotation>,
574    velocity1: impl Into<LinearVelocity>,
575    collider2: &Collider,
576    position2: impl Into<Position>,
577    rotation2: impl Into<Rotation>,
578    velocity2: impl Into<LinearVelocity>,
579    max_time_of_impact: Scalar,
580) -> Result<Option<TimeOfImpact>, UnsupportedShape> {
581    let rotation1: Rotation = rotation1.into();
582    let rotation2: Rotation = rotation2.into();
583
584    let velocity1: LinearVelocity = velocity1.into();
585    let velocity2: LinearVelocity = velocity2.into();
586
587    let isometry1 = make_pose(position1, rotation1);
588    let isometry2 = make_pose(position2, rotation2);
589
590    parry::query::cast_shapes(
591        &isometry1,
592        velocity1.0,
593        collider1.shape_scaled().0.as_ref(),
594        &isometry2,
595        velocity2.0,
596        collider2.shape_scaled().0.as_ref(),
597        ShapeCastOptions {
598            max_time_of_impact,
599            stop_at_penetration: true,
600            ..default()
601        },
602    )
603    .map(|toi| {
604        toi.map(|toi| TimeOfImpact {
605            time_of_impact: toi.time_of_impact,
606            point1: toi.witness1,
607            point2: toi.witness2,
608            normal1: toi.normal1,
609            normal2: toi.normal2,
610            status: toi.status,
611        })
612    })
613}
avian2d/collision/collider/parry/contact_query.rs

avian2d/collision/collider/parry/
contact_query.rs
