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-5
36        {
37            let contact = details::contact_support_map_support_map(pos12, g1, g2, Real::MAX)?;
38            let normal_vel = contact.normal1.dot(vel12);
39
40            if !options.stop_at_penetration && normal_vel >= 0.0 {
41                None
42            } else {
43                Some(ShapeCastHit {
44                    time_of_impact,
45                    normal1: contact.normal1,
46                    normal2: contact.normal2,
47                    witness1: contact.point1,
48                    witness2: contact.point2,
49                    status: ShapeCastStatus::PenetratingOrWithinTargetDist,
50                })
51            }
52        } else {
53            Some(ShapeCastHit {
54                time_of_impact,
55                normal1,
56                normal2: pos12.rotation.inverse() * -normal1,
57                witness1: witness1 - normal1 * options.target_distance,
58                witness2: pos12.inverse_transform_point(witness2),
59                status: if time_of_impact.is_zero() {
60                    ShapeCastStatus::PenetratingOrWithinTargetDist
61                } else {
62                    ShapeCastStatus::Converged
63                },
64            })
65        }
66    })
67}
parry3d/query/shape_cast/shape_cast_support_map_support_map.rs

parry3d/query/shape_cast/
shape_cast_support_map_support_map.rs
