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
120
121
122
123
124
125
126
127

use crate::math::{Isometry, Point};
use crate::pipeline::narrow_phase::{ContactDispatcher, ContactManifoldGenerator};
use crate::query::{
    Contact, ContactKinematic, ContactManifold, ContactPrediction, ContactPreprocessor,
    NeighborhoodGeometry,
};
use crate::shape::{ConvexPolygonalFeature, FeatureId, Plane, Shape};
use na::{self, RealField};

/// Collision detector between g1 plane and g1 shape implementing the `SupportMap` trait.
#[derive(Clone)]
pub struct PlaneConvexPolyhedronManifoldGenerator<N: RealField + Copy> {
    flip: bool,
    feature: ConvexPolygonalFeature<N>,
}

impl<N: RealField + Copy> PlaneConvexPolyhedronManifoldGenerator<N> {
    /// Creates g1 new persistent collision detector between g1 plane and g1 shape with g1 support
    /// mapping function.
    #[inline]
    pub fn new(flip: bool) -> PlaneConvexPolyhedronManifoldGenerator<N> {
        PlaneConvexPolyhedronManifoldGenerator {
            flip,
            feature: ConvexPolygonalFeature::new(),
        }
    }

    #[inline]
    fn do_update_to(
        m1: &Isometry<N>,
        g1: &dyn Shape<N>,
        proc1: Option<&dyn ContactPreprocessor<N>>,
        m2: &Isometry<N>,
        g2: &dyn Shape<N>,
        proc2: Option<&dyn ContactPreprocessor<N>>,
        prediction: &ContactPrediction<N>,
        poly_feature: &mut ConvexPolygonalFeature<N>,
        manifold: &mut ContactManifold<N>,
        flip: bool,
    ) -> bool {
        if let (Some(plane), Some(cp)) = (g1.as_shape::<Plane<N>>(), g2.as_convex_polyhedron()) {
            let plane_normal = m1 * plane.normal;
            let plane_center = Point::from(m1.translation.vector);

            cp.support_face_toward(m2, &-plane_normal, poly_feature);

            for (i, world2) in poly_feature.vertices.iter().enumerate() {
                let dpt = *world2 - plane_center;
                let dist = dpt.dot(plane_normal.as_ref());

                if dist <= prediction.linear() {
                    let world1 = *world2 + (-*plane_normal * dist);
                    let local1 = m1.inverse_transform_point(&world1);
                    let local2 = m2.inverse_transform_point(&world2);
                    let f1 = FeatureId::Face(0);
                    let f2 = poly_feature.vertices_id[i];
                    let mut kinematic = ContactKinematic::new();
                    let contact;

                    let approx_plane = NeighborhoodGeometry::Plane(plane.normal);
                    let approx2 = NeighborhoodGeometry::Point;

                    if !flip {
                        contact = Contact::new(world1, *world2, plane_normal, -dist);
                        kinematic.set_approx1(f1, local1, approx_plane);
                        kinematic.set_approx2(f2, local2, approx2);
                        let _ = manifold.push(contact, kinematic, local2, proc1, proc2);
                    } else {
                        contact = Contact::new(*world2, world1, -plane_normal, -dist);
                        kinematic.set_approx1(f2, local2, approx2);
                        kinematic.set_approx2(f1, local1, approx_plane);
                        let _ = manifold.push(contact, kinematic, local2, proc2, proc1);
                    }
                }
            }

            true
        } else {
            false
        }
    }
}

impl<N: RealField + Copy> ContactManifoldGenerator<N>
    for PlaneConvexPolyhedronManifoldGenerator<N>
{
    fn generate_contacts(
        &mut self,
        _: &dyn ContactDispatcher<N>,
        m1: &Isometry<N>,
        g1: &dyn Shape<N>,
        proc1: Option<&dyn ContactPreprocessor<N>>,
        m2: &Isometry<N>,
        g2: &dyn Shape<N>,
        proc2: Option<&dyn ContactPreprocessor<N>>,
        prediction: &ContactPrediction<N>,
        manifold: &mut ContactManifold<N>,
    ) -> bool {
        if !self.flip {
            Self::do_update_to(
                m1,
                g1,
                proc1,
                m2,
                g2,
                proc2,
                prediction,
                &mut self.feature,
                manifold,
                false,
            )
        } else {
            Self::do_update_to(
                m2,
                g2,
                proc2,
                m1,
                g1,
                proc1,
                prediction,
                &mut self.feature,
                manifold,
                true,
            )
        }
    }
}