parry3d/query/shape_cast/
shape_cast_support_map_support_map.rs

1use crate::math::{Pose, Real, Vector};
2use crate::query::details;
3use crate::query::details::ShapeCastOptions;
4use crate::query::gjk::{self, VoronoiSimplex};
5use crate::query::{ShapeCastHit, ShapeCastStatus};
6use crate::shape::{RoundShapeRef, SupportMap};
7use num::Zero;
8
9/// Time of impacts between two support-mapped shapes under translational movement.
10pub fn cast_shapes_support_map_support_map<G1, G2>(
11    pos12: &Pose,
12    vel12: Vector,
13    g1: &G1,
14    g2: &G2,
15    options: ShapeCastOptions,
16) -> Option<ShapeCastHit>
17where
18    G1: ?Sized + SupportMap,
19    G2: ?Sized + SupportMap,
20{
21    let gjk_result = if options.target_distance > 0.0 {
22        let round_g1 = RoundShapeRef {
23            inner_shape: g1,
24            border_radius: options.target_distance,
25        };
26        gjk::directional_distance(pos12, &round_g1, g2, vel12, &mut VoronoiSimplex::new())
27    } else {
28        gjk::directional_distance(pos12, g1, g2, vel12, &mut VoronoiSimplex::new())
29    };
30
31    gjk_result.and_then(|(time_of_impact, normal1, witness1, witness2)| {
32        if time_of_impact > options.max_time_of_impact {
33            None
34        } else if (options.compute_impact_geometry_on_penetration || !options.stop_at_penetration)
35            && time_of_impact < 1.0e-4
36        {
37            // The GJK-derived normal becomes unreliable for very small TOIs — typically
38            // when the cast starts at (or very close to) the `target_distance` boundary
39            // with a direction nearly tangent to the contact. Fall back on the contact
40            // query, which gives a robust closest-point normal and lets us detect the
41            // case where the cast direction is actually moving away from the contact.
42            let contact = details::contact_support_map_support_map(pos12, g1, g2, Real::MAX)?;
43            let normal_vel = contact.normal1.dot(vel12);
44
45            if !options.stop_at_penetration && normal_vel >= 0.0 {
46                None
47            } else {
48                Some(ShapeCastHit {
49                    time_of_impact,
50                    normal1: contact.normal1,
51                    normal2: contact.normal2,
52                    witness1: contact.point1,
53                    witness2: contact.point2,
54                    // A cast starting exactly touching, neither penetrating nor within
55                    // the target distance, converged like ball-ball does.
56                    status: if contact.dist < options.target_distance {
57                        ShapeCastStatus::PenetratingOrWithinTargetDist
58                    } else {
59                        ShapeCastStatus::Converged
60                    },
61                    subshape1: 0,
62                    subshape2: 0,
63                })
64            }
65        } else {
66            Some(ShapeCastHit {
67                time_of_impact,
68                normal1,
69                normal2: pos12.rotation.inverse() * -normal1,
70                witness1: witness1 - normal1 * options.target_distance,
71                witness2: pos12.inverse_transform_point(witness2),
72                status: if time_of_impact.is_zero() {
73                    // A zero TOI means either a real penetration (or a start within the
74                    // target distance) or a mere touching contact; only the former is
75                    // `PenetratingOrWithinTargetDist`, as for ball-ball.
76                    match details::contact_support_map_support_map(pos12, g1, g2, Real::MAX) {
77                        Some(contact) if contact.dist >= options.target_distance => {
78                            ShapeCastStatus::Converged
79                        }
80                        _ => ShapeCastStatus::PenetratingOrWithinTargetDist,
81                    }
82                } else {
83                    ShapeCastStatus::Converged
84                },
85                subshape1: 0,
86                subshape2: 0,
87            })
88        }
89    })
90}
parry3d/query/shape_cast/shape_cast_support_map_support_map.rs

parry3d/query/shape_cast/
shape_cast_support_map_support_map.rs
