ncollide3d/pipeline/narrow_phase/contact_generator/
plane_ball_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, Plane, Shape};
8use na::{self, RealField};
9use std::marker::PhantomData;
10
11/// Collision detector between g1 plane and g1 shape implementing the `SupportMap` trait.
12#[derive(Clone)]
13pub struct PlaneBallManifoldGenerator<N: RealField + Copy> {
14    flip: bool,
15    phantom: PhantomData<N>,
16}
17
18impl<N: RealField + Copy> PlaneBallManifoldGenerator<N> {
19    /// Creates g1 new persistent collision detector between g1 plane and g1 shape with g1 support
20    /// mapping function.
21    #[inline]
22    pub fn new(flip: bool) -> PlaneBallManifoldGenerator<N> {
23        PlaneBallManifoldGenerator {
24            flip,
25            phantom: PhantomData,
26        }
27    }
28
29    #[inline]
30    fn do_update_to(
31        m1: &Isometry<N>,
32        g1: &dyn Shape<N>,
33        proc1: Option<&dyn ContactPreprocessor<N>>,
34        m2: &Isometry<N>,
35        g2: &dyn Shape<N>,
36        proc2: Option<&dyn ContactPreprocessor<N>>,
37        prediction: &ContactPrediction<N>,
38        manifold: &mut ContactManifold<N>,
39        flip: bool,
40    ) -> bool {
41        if let (Some(plane), Some(ball)) = (g1.as_shape::<Plane<N>>(), g2.as_shape::<Ball<N>>()) {
42            let plane_normal = m1 * plane.normal;
43            let plane_center = Point::from(m1.translation.vector);
44
45            let ball_center = Point::from(m2.translation.vector);
46            let dist = (ball_center - plane_center).dot(plane_normal.as_ref());
47            let depth = -dist + ball.radius;
48
49            if depth > -prediction.linear() {
50                let world1 = ball_center + *plane_normal * (-dist);
51                let world2 = ball_center + *plane_normal * (-ball.radius);
52
53                let local1 = m1.inverse_transform_point(&world1);
54                let local2 = Point::origin();
55
56                let f1 = FeatureId::Face(0);
57                let f2 = FeatureId::Face(0);
58                let mut kinematic = ContactKinematic::new();
59                let contact;
60
61                let approx_ball = NeighborhoodGeometry::Point;
62                let approx_plane = NeighborhoodGeometry::Plane(plane.normal);
63
64                if !flip {
65                    contact = Contact::new(world1, world2, plane_normal, depth);
66                    kinematic.set_approx1(f1, local1, approx_plane);
67                    kinematic.set_approx2(f2, local2, approx_ball);
68                    kinematic.set_dilation2(ball.radius);
69                    let _ = manifold.push(contact, kinematic, Point::origin(), proc1, proc2);
70                } else {
71                    contact = Contact::new(world2, world1, -plane_normal, depth);
72                    kinematic.set_approx1(f2, local2, approx_ball);
73                    kinematic.set_dilation1(ball.radius);
74                    kinematic.set_approx2(f1, local1, approx_plane);
75                    let _ = manifold.push(contact, kinematic, Point::origin(), proc2, proc1);
76                }
77            }
78
79            true
80        } else {
81            false
82        }
83    }
84}
85
86impl<N: RealField + Copy> ContactManifoldGenerator<N> for PlaneBallManifoldGenerator<N> {
87    #[inline]
88    fn generate_contacts(
89        &mut self,
90        _: &dyn ContactDispatcher<N>,
91        m1: &Isometry<N>,
92        g1: &dyn Shape<N>,
93        proc1: Option<&dyn ContactPreprocessor<N>>,
94        m2: &Isometry<N>,
95        g2: &dyn Shape<N>,
96        proc2: Option<&dyn ContactPreprocessor<N>>,
97        prediction: &ContactPrediction<N>,
98        manifold: &mut ContactManifold<N>,
99    ) -> bool {
100        if !self.flip {
101            Self::do_update_to(m1, g1, proc1, m2, g2, proc2, prediction, manifold, false)
102        } else {
103            Self::do_update_to(m2, g2, proc2, m1, g1, proc1, prediction, manifold, true)
104        }
105    }
106}
ncollide3d/pipeline/narrow_phase/contact_generator/plane_ball_manifold_generator.rs

ncollide3d/pipeline/narrow_phase/contact_generator/
plane_ball_manifold_generator.rs
