parry3d/query/contact/
contact_ball_convex_polyhedron.rs

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

parry3d/query/contact/
contact_ball_convex_polyhedron.rs
