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

avian3d/collision/collider/parry/
contact_query.rs
