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

avian3d/collision/collider/parry/
contact_query.rs
