parry3d/query/shape_cast/
shape_cast_halfspace_support_map.rs

1use crate::math::{Isometry, Real, Vector};
2use crate::query::details::ShapeCastOptions;
3use crate::query::{Ray, RayCast, ShapeCastHit, ShapeCastStatus};
4use crate::shape::{HalfSpace, RoundShapeRef, SupportMap};
5
6/// Time Of Impact of a halfspace with a support-mapped shape under translational movement.
7pub fn cast_shapes_halfspace_support_map<G: ?Sized + SupportMap>(
8    pos12: &Isometry<Real>,
9    vel12: &Vector<Real>,
10    halfspace: &HalfSpace,
11    other: &G,
12    options: ShapeCastOptions,
13) -> Option<ShapeCastHit> {
14    // TODO: add method to get only the local support point.
15    // This would avoid the `inverse_transform_point` later.
16    if !options.stop_at_penetration && vel12.dot(&halfspace.normal) > 0.0 {
17        return None;
18    }
19
20    let support_point = if options.target_distance > 0.0 {
21        let round_other = RoundShapeRef {
22            inner_shape: other,
23            border_radius: options.target_distance,
24        };
25        round_other.support_point(pos12, &-halfspace.normal)
26    } else {
27        other.support_point(pos12, &-halfspace.normal)
28    };
29    let closest_point = support_point;
30    let ray = Ray::new(closest_point, *vel12);
31
32    if let Some(time_of_impact) = halfspace.cast_local_ray(&ray, options.max_time_of_impact, true) {
33        if time_of_impact > options.max_time_of_impact {
34            return None;
35        }
36
37        let witness2 = support_point + *halfspace.normal * options.target_distance;
38        let mut witness1 = ray.point_at(time_of_impact);
39        // Project the witness point to the halfspace.
40        // Note that witness1 is already in the halfspace's local-space.
41        witness1 -= *halfspace.normal * witness1.coords.dot(&halfspace.normal);
42
43        let status = if support_point.coords.dot(&halfspace.normal) < 0.0 {
44            ShapeCastStatus::PenetratingOrWithinTargetDist
45        } else {
46            ShapeCastStatus::Converged
47        };
48
49        Some(ShapeCastHit {
50            time_of_impact,
51            normal1: halfspace.normal,
52            normal2: pos12.inverse_transform_unit_vector(&-halfspace.normal),
53            witness1,
54            witness2: pos12.inverse_transform_point(&witness2),
55            status,
56        })
57    } else {
58        None
59    }
60}
61
62/// Time Of Impact of a halfspace with a support-mapped shape under translational movement.
63pub fn cast_shapes_support_map_halfspace<G: ?Sized + SupportMap>(
64    pos12: &Isometry<Real>,
65    vel12: &Vector<Real>,
66    other: &G,
67    halfspace: &HalfSpace,
68    options: ShapeCastOptions,
69) -> Option<ShapeCastHit> {
70    cast_shapes_halfspace_support_map(
71        &pos12.inverse(),
72        &-pos12.inverse_transform_vector(vel12),
73        halfspace,
74        other,
75        options,
76    )
77    .map(|hit| hit.swapped())
78}
parry3d/query/shape_cast/shape_cast_halfspace_support_map.rs

parry3d/query/shape_cast/
shape_cast_halfspace_support_map.rs
