ncollide2d/pipeline/narrow_phase/contact_generator/
ball_convex_polyhedron_manifold_generator.rs

1use crate::math::{Isometry, Point};
2use crate::pipeline::narrow_phase::{ContactDispatcher, ContactManifoldGenerator};
3use crate::query::{
4    Contact, ContactKinematic, ContactManifold, ContactPrediction, ContactPreprocessor,
5    NeighborhoodGeometry,
6};
7use crate::shape::{Ball, FeatureId, Shape};
8use na::{RealField, Unit};
9use std::marker::PhantomData;
10
11/// Collision detector between two balls.
12#[derive(Clone)]
13pub struct BallConvexPolyhedronManifoldGenerator<N: RealField + Copy> {
14    phantom: PhantomData<N>,
15    flip: bool,
16}
17
18impl<N: RealField + Copy> BallConvexPolyhedronManifoldGenerator<N> {
19    /// Creates a new persistent collision detector between two balls.
20    #[inline]
21    pub fn new(flip: bool) -> BallConvexPolyhedronManifoldGenerator<N> {
22        BallConvexPolyhedronManifoldGenerator {
23            phantom: PhantomData,
24            flip,
25        }
26    }
27
28    fn do_generate(
29        &mut self,
30        m1: &Isometry<N>,
31        a: &dyn Shape<N>,
32        proc1: Option<&dyn ContactPreprocessor<N>>,
33        m2: &Isometry<N>,
34        b: &dyn Shape<N>,
35        proc2: Option<&dyn ContactPreprocessor<N>>,
36        prediction: &ContactPrediction<N>,
37        manifold: &mut ContactManifold<N>,
38    ) -> bool {
39        // NOTE: we use an underscore to silence a warning
40        // for _cp2 because it is used in 3D but not in 2D.
41        if let (Some(ball), Some(pq2), Some(cp2)) = (
42            a.as_shape::<Ball<N>>(),
43            b.as_point_query(),
44            b.as_convex_polyhedron(),
45        ) {
46            let ball_center = Point::from(m1.translation.vector);
47            let (proj, f2) = pq2.project_point_with_feature(m2, &ball_center);
48            let world2 = proj.point;
49            let dpt = world2 - ball_center;
50
51            let depth;
52            let normal;
53            if let Some((dir, dist)) = Unit::try_new_and_get(dpt, N::default_epsilon()) {
54                if proj.is_inside {
55                    depth = dist + ball.radius;
56                    normal = -dir;
57                } else {
58                    depth = -dist + ball.radius;
59                    normal = dir;
60                }
61            } else {
62                if f2 == FeatureId::Unknown {
63                    // We cant do anything more at this point.
64                    return true;
65                }
66
67                depth = ball.radius;
68                normal = -cp2.feature_normal(f2);
69            }
70
71            if depth >= -prediction.linear() {
72                let mut kinematic = ContactKinematic::new();
73                let f1 = FeatureId::Face(0);
74                let world1 = ball_center + normal.into_inner() * ball.radius;
75
76                let contact;
77
78                if !self.flip {
79                    contact = Contact::new(world1, world2, normal, depth);
80                    kinematic.set_approx1(f1, Point::origin(), NeighborhoodGeometry::Point);
81                    kinematic.set_dilation1(ball.radius);
82                } else {
83                    contact = Contact::new(world2, world1, -normal, depth);
84                    kinematic.set_approx2(f1, Point::origin(), NeighborhoodGeometry::Point);
85                    kinematic.set_dilation2(ball.radius);
86                }
87
88                let local2 = m2.inverse_transform_point(&world2);
89                let geom2;
90
91                match f2 {
92                    FeatureId::Face { .. } => {
93                        let n = m2.inverse_transform_unit_vector(&-normal);
94                        geom2 = NeighborhoodGeometry::Plane(n);
95                    }
96                    #[cfg(feature = "dim3")]
97                    FeatureId::Edge { .. } => {
98                        let edge = cp2.edge(f2);
99                        let dir = Unit::new_normalize(edge.1 - edge.0);
100                        geom2 = NeighborhoodGeometry::Line(dir);
101                    }
102                    FeatureId::Vertex { .. } => {
103                        geom2 = NeighborhoodGeometry::Point;
104                    }
105                    FeatureId::Unknown => panic!("Feature id cannot be unknown."),
106                }
107
108                if !self.flip {
109                    kinematic.set_approx2(f2, local2, geom2);
110                    let _ = manifold.push(contact, kinematic, Point::origin(), proc1, proc2);
111                } else {
112                    kinematic.set_approx1(f2, local2, geom2);
113                    let _ = manifold.push(contact, kinematic, Point::origin(), proc2, proc1);
114                }
115            }
116
117            true
118        } else {
119            false
120        }
121    }
122}
123
124impl<N: RealField + Copy> ContactManifoldGenerator<N> for BallConvexPolyhedronManifoldGenerator<N> {
125    fn generate_contacts(
126        &mut self,
127        _: &dyn ContactDispatcher<N>,
128        m1: &Isometry<N>,
129        a: &dyn Shape<N>,
130        proc1: Option<&dyn ContactPreprocessor<N>>,
131        m2: &Isometry<N>,
132        b: &dyn Shape<N>,
133        proc2: Option<&dyn ContactPreprocessor<N>>,
134        prediction: &ContactPrediction<N>,
135        manifold: &mut ContactManifold<N>,
136    ) -> bool {
137        if !self.flip {
138            self.do_generate(m1, a, proc1, m2, b, proc2, prediction, manifold)
139        } else {
140            self.do_generate(m2, b, proc2, m1, a, proc1, prediction, manifold)
141        }
142    }
143}
ncollide2d/pipeline/narrow_phase/contact_generator/ball_convex_polyhedron_manifold_generator.rs

ncollide2d/pipeline/narrow_phase/contact_generator/
ball_convex_polyhedron_manifold_generator.rs
