1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119

use na::{self, Real};

use math::{Isometry, Point};
use utils::{IdAllocator, IsometryOps};
use shape::{Ball, FeatureId, Plane, Shape};
use bounding_volume::PolyhedralCone;
use query::{Contact, ContactKinematic, ContactManifold, ContactPrediction};
use pipeline::narrow_phase::{ContactDispatcher, ContactManifoldGenerator};

/// Collision detector between g1 plane and g1 shape implementing the `SupportMap` trait.
#[derive(Clone)]
pub struct PlaneBallManifoldGenerator<N: Real> {
    flip: bool,
    manifold: ContactManifold<N>,
}

impl<N: Real> PlaneBallManifoldGenerator<N> {
    /// Creates g1 new persistent collision detector between g1 plane and g1 shape with g1 support
    /// mapping function.
    #[inline]
    pub fn new(flip: bool) -> PlaneBallManifoldGenerator<N> {
        PlaneBallManifoldGenerator {
            flip,
            manifold: ContactManifold::new(),
        }
    }

    #[inline]
    fn do_update(
        &mut self,
        m1: &Isometry<N>,
        g1: &Shape<N>,
        m2: &Isometry<N>,
        g2: &Shape<N>,
        prediction: &ContactPrediction<N>,
        id_alloc: &mut IdAllocator,
        flip: bool,
    ) -> bool {
        if let (Some(plane), Some(ball)) = (
            g1.as_shape::<Plane<N>>(),
            g2.as_shape::<Ball<N>>(),
        ) {
            self.manifold.save_cache_and_clear(id_alloc);

            let plane_normal = m1 * plane.normal();
            let plane_center = Point::from_coordinates(m1.translation.vector);

            let ball_center = Point::from_coordinates(m2.translation.vector);
            let dist = na::dot(&(ball_center - plane_center), plane_normal.as_ref());
            let depth = -dist + ball.radius();

            if depth > -prediction.linear {
                let world1 = ball_center + *plane_normal * (-dist);
                let world2 = ball_center + *plane_normal * (-ball.radius());

                let local1 = m1.inverse_transform_point(&world1);
                let local2 = Point::origin();

                let f1 = FeatureId::Face(0);
                let f2 = FeatureId::Face(0);
                let mut kinematic = ContactKinematic::new();
                let contact;

                if !flip {
                    contact = Contact::new(world1, world2, plane_normal, depth);
                    kinematic.set_plane1(f1, local1, *plane.normal());
                    kinematic.set_point2(f2, local2, PolyhedralCone::Full);
                    kinematic.set_dilation2(ball.radius());
                } else {
                    contact = Contact::new(world2, world1, -plane_normal, depth);
                    kinematic.set_point1(f2, local2, PolyhedralCone::Full);
                    kinematic.set_dilation1(ball.radius());
                    kinematic.set_plane2(f1, local1, *plane.normal());
                }

                let _ = self.manifold.push(contact, kinematic, id_alloc);
            }

            true
        } else {
            false
        }
    }
}

impl<N: Real> ContactManifoldGenerator<N> for PlaneBallManifoldGenerator<N> {
    #[inline]
    fn update(
        &mut self,
        _: &ContactDispatcher<N>,
        id1: usize,
        m1: &Isometry<N>,
        g1: &Shape<N>,
        id2: usize,
        m2: &Isometry<N>,
        g2: &Shape<N>,
        prediction: &ContactPrediction<N>,
        id_alloc: &mut IdAllocator,
    ) -> bool {
        self.manifold.set_subshape_id1(id1);
        self.manifold.set_subshape_id2(id2);

        if !self.flip {
            self.do_update(m1, g1, m2, g2, prediction, id_alloc, false)
        } else {
            self.do_update(m2, g2, m1, g1, prediction, id_alloc, true)
        }
    }

    #[inline]
    fn num_contacts(&self) -> usize {
        self.manifold.len()
    }

    #[inline]
    fn contacts<'a: 'b, 'b>(&'a self, out: &'b mut Vec<&'a ContactManifold<N>>) {
        out.push(&self.manifold)
    }
}