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

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

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

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

    #[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>,
        manifold: &mut ContactManifold<N>,
        flip: bool,
    ) -> bool {
        if let (Some(plane), Some(ball)) = (g1.as_shape::<Plane<N>>(), g2.as_shape::<Ball<N>>()) {
            let plane_normal = m1 * plane.normal();
            let plane_center = Point::from(m1.translation.vector);

            let ball_center = Point::from(m2.translation.vector);
            let dist = (ball_center - plane_center).dot(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;

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

                if !flip {
                    contact = Contact::new(world1, world2, plane_normal, depth);
                    kinematic.set_approx1(f1, local1, approx_plane);
                    kinematic.set_approx2(f2, local2, approx_ball);
                    kinematic.set_dilation2(ball.radius());
                    let _ = manifold.push(contact, kinematic, Point::origin(), proc1, proc2);
                } else {
                    contact = Contact::new(world2, world1, -plane_normal, depth);
                    kinematic.set_approx1(f2, local2, approx_ball);
                    kinematic.set_dilation1(ball.radius());
                    kinematic.set_approx2(f1, local1, approx_plane);
                    let _ = manifold.push(contact, kinematic, Point::origin(), proc2, proc1);
                }
            }

            true
        } else {
            false
        }
    }
}

impl<N: RealField> ContactManifoldGenerator<N> for PlaneBallManifoldGenerator<N> {
    #[inline]
    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, manifold, false)
        } else {
            Self::do_update_to(m2, g2, proc2, m1, g1, proc1, prediction, manifold, true)
        }
    }
}