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

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> {
    flip: bool,
    feature: ConvexPolygonalFeature<N>,
}

impl<N: RealField> 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> 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,
            )
        }
    }
}