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_isometry(position1.into(), rotation1);
76    let isometry2 = make_isometry(position2.into(), 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() * Vector::from(contact.point1);
89            let point2: Vector = rotation2.inverse() * Vector::from(contact.point2);
90            let normal1: Vector = (rotation1.inverse() * Vector::from(contact.normal1)).normalize();
91            let normal2: Vector = (rotation2.inverse() * Vector::from(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_isometry(position1, rotation1);
171    let isometry2 = make_isometry(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 * Vector::from(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.into();
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
235            .rotation
236            .transform_vector(&manifold.local_n1)
237            .normalize()
238            .into();
239        let normal = rotation1 * local_normal;
240
241        // Make sure the normal is valid
242        if !normal.is_normalized() {
243            return None;
244        }
245
246        let points = manifold.contacts().iter().map(|contact| {
247            // The contact point is the midpoint of the two points in world space.
248            // The anchors are relative to the positions of the colliders.
249            let point1 = rotation1 * Vector::from(subpos1.transform_point(&contact.local_p1));
250            let anchor1 = point1 + normal * contact.dist * 0.5;
251            let anchor2 = anchor1 + (position1.0 - position2.0);
252            let world_point = position1.0 + anchor1;
253            ContactPoint::new(anchor1, anchor2, world_point, -contact.dist)
254                .with_feature_ids(contact.fid1.into(), contact.fid2.into())
255        });
256
257        let manifold = ContactManifold::new(points, normal);
258
259        Some(manifold)
260    }));
261}
262
263/// Information about the closest points between two [`Collider`]s.
264///
265/// The closest points can be computed using [`closest_points`].
266#[derive(Reflect, Clone, Copy, Debug, PartialEq)]
267#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
268#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))]
269#[reflect(Debug, PartialEq)]
270pub enum ClosestPoints {
271    /// The two shapes are intersecting each other.
272    Intersecting,
273    /// The two shapes are not intersecting each other but the distance between the closest points
274    /// is below the user-defined maximum distance.
275    ///
276    /// The points are expressed in world space.
277    WithinMargin(Vector, Vector),
278    /// The two shapes are not intersecting each other and the distance between the closest points
279    /// exceeds the user-defined maximum distance.
280    OutsideMargin,
281}
282
283/// Computes the [`ClosestPoints`] between two [`Collider`]s.
284///
285/// Returns `Err(UnsupportedShape)` if either of the collider shapes is not supported.
286///
287/// # Example
288///
289/// ```
290/// # #[cfg(feature = "2d")]
291/// # use avian2d::{collision::collider::contact_query::*, prelude::*};
292/// # #[cfg(feature = "3d")]
293/// use avian3d::{collision::collider::contact_query::*, prelude::*};
294/// use bevy::prelude::*;
295///
296/// # #[cfg(all(feature = "3d", feature = "f32"))]
297/// # {
298/// let collider1 = Collider::sphere(0.5);
299/// let collider2 = Collider::cuboid(1.0, 1.0, 1.0);
300///
301/// // The shapes are intersecting
302/// assert_eq!(
303///     closest_points(
304///         &collider1,
305///         Vec3::default(),
306///         Quat::default(),
307///         &collider2,
308///         Vec3::default(),
309///         Quat::default(),
310///         2.0,
311///     )
312///     .expect("Unsupported collider shape"),
313///     ClosestPoints::Intersecting,
314/// );
315///
316/// // The shapes are not intersecting but the distance between the closest points is below 2.0
317/// assert_eq!(
318///     closest_points(
319///         &collider1,
320///         Vec3::default(),
321///         Quat::default(),
322///         &collider2,
323///         Vec3::X * 1.5,
324///         Quat::default(),
325///         2.0,
326///     )
327///     .expect("Unsupported collider shape"),
328///     ClosestPoints::WithinMargin(Vec3::X * 0.5, Vec3::X * 1.0),
329/// );
330///
331/// // The shapes are not intersecting and the distance between the closest points exceeds 2.0
332/// assert_eq!(
333///     closest_points(
334///         &collider1,
335///         Vec3::default(),
336///         Quat::default(),
337///         &collider2,
338///         Vec3::X * 5.0,
339///         Quat::default(),
340///         2.0,
341///     )
342///     .expect("Unsupported collider shape"),
343///     ClosestPoints::OutsideMargin,
344/// );
345/// # }
346/// ```
347pub fn closest_points(
348    collider1: &Collider,
349    position1: impl Into<Position>,
350    rotation1: impl Into<Rotation>,
351    collider2: &Collider,
352    position2: impl Into<Position>,
353    rotation2: impl Into<Rotation>,
354    max_distance: Scalar,
355) -> Result<ClosestPoints, UnsupportedShape> {
356    let rotation1: Rotation = rotation1.into();
357    let rotation2: Rotation = rotation2.into();
358    let isometry1 = make_isometry(position1.into(), rotation1);
359    let isometry2 = make_isometry(position2.into(), rotation2);
360
361    parry::query::closest_points(
362        &isometry1,
363        collider1.shape_scaled().0.as_ref(),
364        &isometry2,
365        collider2.shape_scaled().0.as_ref(),
366        max_distance,
367    )
368    .map(|closest_points| match closest_points {
369        parry::query::ClosestPoints::Intersecting => ClosestPoints::Intersecting,
370        parry::query::ClosestPoints::WithinMargin(point1, point2) => {
371            ClosestPoints::WithinMargin(point1.into(), point2.into())
372        }
373        parry::query::ClosestPoints::Disjoint => ClosestPoints::OutsideMargin,
374    })
375}
376
377/// Computes the minimum distance separating two [`Collider`]s.
378///
379/// Returns `0.0` if the colliders are touching or penetrating, and `Err(UnsupportedShape)`
380/// if either of the collider shapes is not supported.
381///
382/// # Example
383///
384/// ```
385/// # #[cfg(feature = "2d")]
386/// # use avian2d::{collision::collider::contact_query::distance, prelude::*};
387/// # #[cfg(feature = "3d")]
388/// use avian3d::{collision::collider::contact_query::distance, prelude::*};
389/// use bevy::prelude::*;
390///
391/// # #[cfg(all(feature = "3d", feature = "f32"))]
392/// # {
393/// let collider1 = Collider::sphere(0.5);
394/// let collider2 = Collider::cuboid(1.0, 1.0, 1.0);
395///
396/// // The distance is 1.0
397/// assert_eq!(
398///     distance(
399///         &collider1,
400///         Vec3::default(),
401///         Quat::default(),
402///         &collider2,
403///         Vec3::X * 2.0,
404///         Quat::default(),
405///     )
406///     .expect("Unsupported collider shape"),
407///     1.0,
408/// );
409///
410/// // The colliders are penetrating, so the distance is 0.0
411/// assert_eq!(
412///     distance(
413///         &collider1,
414///         Vec3::default(),
415///         Quat::default(),
416///         &collider2,
417///         Vec3::default(),
418///         Quat::default(),
419///     )
420///     .expect("Unsupported collider shape"),
421///     0.0,
422/// );
423/// # }
424/// ```
425pub fn distance(
426    collider1: &Collider,
427    position1: impl Into<Position>,
428    rotation1: impl Into<Rotation>,
429    collider2: &Collider,
430    position2: impl Into<Position>,
431    rotation2: impl Into<Rotation>,
432) -> Result<Scalar, UnsupportedShape> {
433    let rotation1: Rotation = rotation1.into();
434    let rotation2: Rotation = rotation2.into();
435    let isometry1 = make_isometry(position1.into(), rotation1);
436    let isometry2 = make_isometry(position2.into(), rotation2);
437
438    parry::query::distance(
439        &isometry1,
440        collider1.shape_scaled().0.as_ref(),
441        &isometry2,
442        collider2.shape_scaled().0.as_ref(),
443    )
444}
445
446/// Tests whether two [`Collider`]s are intersecting each other.
447///
448/// Returns `Err(UnsupportedShape)` if either of the collider shapes is not supported.
449///
450/// # Example
451///
452/// ```
453/// # #[cfg(feature = "2d")]
454/// # use avian2d::{collision::collider::contact_query::intersection_test, prelude::*};
455/// # #[cfg(feature = "3d")]
456/// use avian3d::{collision::collider::contact_query::intersection_test, prelude::*};
457/// use bevy::prelude::*;
458///
459/// # #[cfg(all(feature = "3d", feature = "f32"))]
460/// # {
461/// let collider1 = Collider::sphere(0.5);
462/// let collider2 = Collider::cuboid(1.0, 1.0, 1.0);
463///
464/// // These colliders should be intersecting
465/// assert_eq!(
466///     intersection_test(
467///         &collider1,
468///         Vec3::default(),
469///         Quat::default(),
470///         &collider2,
471///         Vec3::default(),
472///         Quat::default(),
473///     )
474///     .expect("Unsupported collider shape"),
475///     true,
476/// );
477///
478/// // These colliders shouldn't be intersecting
479/// assert_eq!(
480///     intersection_test(
481///         &collider1,
482///         Vec3::default(),
483///         Quat::default(),
484///         &collider2,
485///         Vec3::X * 5.0,
486///         Quat::default(),
487///     )
488///     .expect("Unsupported collider shape"),
489///     false,
490/// );
491/// # }
492/// ```
493pub fn intersection_test(
494    collider1: &Collider,
495    position1: impl Into<Position>,
496    rotation1: impl Into<Rotation>,
497    collider2: &Collider,
498    position2: impl Into<Position>,
499    rotation2: impl Into<Rotation>,
500) -> Result<bool, UnsupportedShape> {
501    let rotation1: Rotation = rotation1.into();
502    let rotation2: Rotation = rotation2.into();
503    let isometry1 = make_isometry(position1.into(), rotation1);
504    let isometry2 = make_isometry(position2.into(), rotation2);
505
506    parry::query::intersection_test(
507        &isometry1,
508        collider1.shape_scaled().0.as_ref(),
509        &isometry2,
510        collider2.shape_scaled().0.as_ref(),
511    )
512}
513
514/// The way the [time of impact](time_of_impact) computation was terminated.
515pub type TimeOfImpactStatus = parry::query::details::ShapeCastStatus;
516
517/// The result of a [time of impact](time_of_impact) computation between two moving [`Collider`]s.
518#[derive(Clone, Copy, Debug, PartialEq)]
519pub struct TimeOfImpact {
520    /// The time at which the colliders come into contact.
521    pub time_of_impact: Scalar,
522    /// The closest point on the first collider, at the time of impact,
523    /// expressed in local space.
524    pub point1: Vector,
525    /// The closest point on the second collider, at the time of impact,
526    /// expressed in local space.
527    pub point2: Vector,
528    /// The outward normal on the first collider, at the time of impact,
529    /// expressed in local space.
530    pub normal1: Vector,
531    /// The outward normal on the second collider, at the time of impact,
532    /// expressed in local space.
533    pub normal2: Vector,
534    /// The way the time of impact computation was terminated.
535    pub status: TimeOfImpactStatus,
536}
537
538/// Computes when two moving [`Collider`]s hit each other for the first time.
539///
540/// Returns `Ok(None)` if the time of impact is greater than `max_time_of_impact`
541/// and `Err(UnsupportedShape)` if either of the collider shapes is not supported.
542///
543/// # Example
544///
545/// ```
546/// # #[cfg(feature = "2d")]
547/// # use avian2d::{collision::collider::contact_query::time_of_impact, prelude::*};
548/// # #[cfg(feature = "3d")]
549/// use avian3d::{collision::collider::contact_query::time_of_impact, prelude::*};
550/// use bevy::prelude::*;
551///
552/// # #[cfg(all(feature = "3d", feature = "f32"))]
553/// # {
554/// let collider1 = Collider::sphere(0.5);
555/// let collider2 = Collider::cuboid(1.0, 1.0, 1.0);
556///
557/// let result = time_of_impact(
558///     &collider1,        // Collider 1
559///     Vec3::NEG_X * 5.0, // Position 1
560///     Quat::default(),   // Rotation 1
561///     Vec3::X,           // Linear velocity 1
562///     &collider2,        // Collider 2
563///     Vec3::X * 5.0,     // Position 2
564///     Quat::default(),   // Rotation 2
565///     Vec3::NEG_X,       // Linear velocity 2
566///     100.0,             // Maximum time of impact
567/// )
568/// .expect("Unsupported collider shape");
569///
570/// assert_eq!(result.unwrap().time_of_impact, 4.5);
571/// # }
572/// ```
573#[allow(clippy::too_many_arguments, clippy::type_complexity)]
574pub fn time_of_impact(
575    collider1: &Collider,
576    position1: impl Into<Position>,
577    rotation1: impl Into<Rotation>,
578    velocity1: impl Into<LinearVelocity>,
579    collider2: &Collider,
580    position2: impl Into<Position>,
581    rotation2: impl Into<Rotation>,
582    velocity2: impl Into<LinearVelocity>,
583    max_time_of_impact: Scalar,
584) -> Result<Option<TimeOfImpact>, UnsupportedShape> {
585    let rotation1: Rotation = rotation1.into();
586    let rotation2: Rotation = rotation2.into();
587
588    let velocity1: LinearVelocity = velocity1.into();
589    let velocity2: LinearVelocity = velocity2.into();
590
591    let isometry1 = make_isometry(position1.into(), rotation1);
592    let isometry2 = make_isometry(position2.into(), rotation2);
593
594    parry::query::cast_shapes(
595        &isometry1,
596        &velocity1.0.into(),
597        collider1.shape_scaled().0.as_ref(),
598        &isometry2,
599        &velocity2.0.into(),
600        collider2.shape_scaled().0.as_ref(),
601        ShapeCastOptions {
602            max_time_of_impact,
603            stop_at_penetration: true,
604            ..default()
605        },
606    )
607    .map(|toi| {
608        toi.map(|toi| TimeOfImpact {
609            time_of_impact: toi.time_of_impact,
610            point1: toi.witness1.into(),
611            point2: toi.witness2.into(),
612            normal1: toi.normal1.into(),
613            normal2: toi.normal2.into(),
614            status: toi.status,
615        })
616    })
617}
avian2d/collision/collider/parry/contact_query.rs

avian2d/collision/collider/parry/
contact_query.rs
