parry2d/query/contact/
contact_ball_convex_polyhedron.rs

1use crate::math::{Pose, Real, Vector};
2use crate::query::Contact;
3use crate::shape::{Ball, Shape};
4
5/// Contact between a ball and a convex polyhedron.
6///
7/// This function panics if the input shape does not implement
8/// both the ConvexPolyhedron and PointQuery traits.
9#[inline]
10pub fn contact_ball_convex_polyhedron(
11    pos12: &Pose,
12    ball1: &Ball,
13    shape2: &(impl Shape + ?Sized),
14    prediction: Real,
15) -> Option<Contact> {
16    contact_convex_polyhedron_ball(&pos12.inverse(), shape2, ball1, prediction).map(|c| c.flipped())
17}
18
19/// Contact between a convex polyhedron and a ball.
20///
21/// This function panics if the input shape does not implement
22/// both the ConvexPolyhedron and PointQuery traits.
23#[inline]
24pub fn contact_convex_polyhedron_ball(
25    pos12: &Pose,
26    shape1: &(impl Shape + ?Sized),
27    ball2: &Ball,
28    prediction: Real,
29) -> Option<Contact> {
30    let center2_1 = pos12.translation;
31    let (proj, f1) = shape1.project_local_point_and_get_feature(center2_1);
32
33    let dist;
34    let normal1;
35    let (dir1, len) = (proj.point - center2_1).normalize_and_length();
36    if len >= crate::math::DEFAULT_EPSILON {
37        if proj.is_inside {
38            dist = -len - ball2.radius;
39            normal1 = dir1;
40        } else {
41            dist = len - ball2.radius;
42            normal1 = -dir1;
43        }
44    } else {
45        dist = -ball2.radius;
46        normal1 = shape1
47            .feature_normal_at_point(f1, proj.point)
48            .or_else(|| (proj.point).try_normalize())
49            .unwrap_or(Vector::Y);
50    }
51
52    if dist <= prediction {
53        let normal2 = pos12.rotation.inverse() * -normal1;
54        let point2 = normal2 * ball2.radius;
55        let point1 = proj.point;
56        return Some(Contact::new(point1, point2, normal1, normal2, dist));
57    }
58
59    None
60}
parry2d/query/contact/contact_ball_convex_polyhedron.rs

parry2d/query/contact/
contact_ball_convex_polyhedron.rs
