parry3d/query/shape_cast/
shape_cast_support_map_support_map.rs

1use na::Unit;
2
3use crate::math::{Isometry, Real, Vector};
4use crate::query::details;
5use crate::query::details::ShapeCastOptions;
6use crate::query::gjk::{self, VoronoiSimplex};
7use crate::query::{ShapeCastHit, ShapeCastStatus};
8use crate::shape::{RoundShapeRef, SupportMap};
9use num::Zero;
10
11/// Time of impacts between two support-mapped shapes under translational movement.
12pub fn cast_shapes_support_map_support_map<G1, G2>(
13    pos12: &Isometry<Real>,
14    vel12: &Vector<Real>,
15    g1: &G1,
16    g2: &G2,
17    options: ShapeCastOptions,
18) -> Option<ShapeCastHit>
19where
20    G1: ?Sized + SupportMap,
21    G2: ?Sized + SupportMap,
22{
23    let gjk_result = if options.target_distance > 0.0 {
24        let round_g1 = RoundShapeRef {
25            inner_shape: g1,
26            border_radius: options.target_distance,
27        };
28        gjk::directional_distance(pos12, &round_g1, g2, vel12, &mut VoronoiSimplex::new())
29    } else {
30        gjk::directional_distance(pos12, g1, g2, vel12, &mut VoronoiSimplex::new())
31    };
32
33    gjk_result.and_then(|(time_of_impact, normal1, witness1, witness2)| {
34        if time_of_impact > options.max_time_of_impact {
35            None
36        } else if (options.compute_impact_geometry_on_penetration || !options.stop_at_penetration)
37            && time_of_impact < 1.0e-5
38        {
39            let contact = details::contact_support_map_support_map(pos12, g1, g2, Real::MAX)?;
40            let normal_vel = contact.normal1.dot(vel12);
41
42            if !options.stop_at_penetration && normal_vel >= 0.0 {
43                None
44            } else {
45                Some(ShapeCastHit {
46                    time_of_impact,
47                    normal1: contact.normal1,
48                    normal2: contact.normal2,
49                    witness1: contact.point1,
50                    witness2: contact.point2,
51                    status: ShapeCastStatus::PenetratingOrWithinTargetDist,
52                })
53            }
54        } else {
55            Some(ShapeCastHit {
56                time_of_impact,
57                normal1: Unit::new_unchecked(normal1),
58                normal2: Unit::new_unchecked(pos12.inverse_transform_vector(&-normal1)),
59                witness1: witness1 - normal1 * options.target_distance,
60                witness2: pos12.inverse_transform_point(&witness2),
61                status: if time_of_impact.is_zero() {
62                    ShapeCastStatus::PenetratingOrWithinTargetDist
63                } else {
64                    ShapeCastStatus::Converged
65                },
66            })
67        }
68    })
69}
parry3d/query/shape_cast/shape_cast_support_map_support_map.rs

parry3d/query/shape_cast/
shape_cast_support_map_support_map.rs
