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

use crate::math::Isometry;
use crate::pipeline::{ContactAlgorithm, ContactDispatcher, ContactManifoldGenerator};
use crate::query::{ContactManifold, ContactPrediction, ContactPreprocessor};
use crate::shape::{Capsule, Shape};
use na::{self, RealField};

/// Collision detector between a concave shape and another shape.
pub struct CapsuleShapeManifoldGenerator<N: RealField> {
    sub_detector: Option<ContactAlgorithm<N>>,
    flip: bool,
}

impl<N: RealField> CapsuleShapeManifoldGenerator<N> {
    /// Creates a new collision detector between a concave shape and another shape.
    pub fn new(flip: bool) -> CapsuleShapeManifoldGenerator<N> {
        CapsuleShapeManifoldGenerator {
            sub_detector: None,
            flip,
        }
    }

    fn do_update(
        &mut self,
        dispatcher: &dyn ContactDispatcher<N>,
        m1: &Isometry<N>,
        g1: &Capsule<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 {
        let segment = g1.segment();
        let mut prediction = prediction.clone();
        let new_linear_prediction = prediction.linear() + g1.radius();
        prediction.set_linear(new_linear_prediction);

        if self.sub_detector.is_none() {
            self.sub_detector = if flip {
                dispatcher.get_contact_algorithm(g2, &segment)
            } else {
                dispatcher.get_contact_algorithm(&segment, g2)
            }
        }

        // Update all collisions
        if flip {
            self.sub_detector.as_mut().unwrap().generate_contacts(
                dispatcher,
                m2,
                g2,
                proc2,
                m1,
                &segment,
                Some(&(proc1, &g1.contact_preprocessor())),
                &prediction,
                manifold,
            )
        } else {
            self.sub_detector.as_mut().unwrap().generate_contacts(
                dispatcher,
                m1,
                &segment,
                Some(&(proc1, &g1.contact_preprocessor())),
                m2,
                g2,
                proc2,
                &prediction,
                manifold,
            )
        }
    }
}

impl<N: RealField> ContactManifoldGenerator<N> for CapsuleShapeManifoldGenerator<N> {
    fn generate_contacts(
        &mut self,
        d: &dyn ContactDispatcher<N>,
        ma: &Isometry<N>,
        a: &dyn Shape<N>,
        proc1: Option<&dyn ContactPreprocessor<N>>,
        mb: &Isometry<N>,
        b: &dyn Shape<N>,
        proc2: Option<&dyn ContactPreprocessor<N>>,
        prediction: &ContactPrediction<N>,
        manifold: &mut ContactManifold<N>,
    ) -> bool {
        if !self.flip {
            if let Some(cs) = a.as_shape::<Capsule<N>>() {
                return self.do_update(d, ma, cs, proc1, mb, b, proc2, prediction, manifold, false);
            }
        } else {
            if let Some(cs) = b.as_shape::<Capsule<N>>() {
                return self.do_update(d, mb, cs, proc2, ma, a, proc1, prediction, manifold, true);
            }
        }

        return false;
    }
}