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
128
129
130
131
132
133
134
135
136
137
138
139
140

//! Support mapping based Capsule shape.

use na::{self, RealField, Unit};

use crate::math::{Isometry, Point, Vector};
use crate::query::{Contact, ContactKinematic, ContactPreprocessor};
use crate::shape::{FeatureId, Segment, SupportMap};

/// SupportMap description of a capsule shape with its principal axis aligned with the `y` axis.
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
#[derive(PartialEq, Debug, Clone)]
pub struct Capsule<N> {
    half_height: N,
    radius: N,
}

impl<N: RealField> Capsule<N> {
    /// Creates a new capsule.
    ///
    /// # Arguments:
    /// * `half_height` - the half length of the capsule along the `y` axis.
    /// * `radius` - radius of the rounded part of the capsule.
    pub fn new(half_height: N, radius: N) -> Capsule<N> {
        assert!(half_height.is_positive() && radius.is_positive());

        Capsule {
            half_height,
            radius,
        }
    }

    /// The capsule half length along its local `y` axis.
    #[inline]
    pub fn half_height(&self) -> N {
        self.half_height
    }

    /// The capsule height along its local `y` axis.
    #[inline]
    pub fn height(&self) -> N {
        self.half_height * na::convert(2.0)
    }

    /// The radius of the capsule's rounded part.
    #[inline]
    pub fn radius(&self) -> N {
        self.radius
    }

    /// The segment that, once dilated by `self.radius` yields this capsule.
    #[inline]
    pub fn segment(&self) -> Segment<N> {
        let mut a = Point::origin();
        let mut b = Point::origin();
        a.y = -self.half_height;
        b.y = self.half_height;

        Segment::new(a, b)
    }

    /// The contact preprocessor to be used for contact determination with this capsule.
    #[inline]
    pub fn contact_preprocessor(&self) -> impl ContactPreprocessor<N> {
        CapsuleContactPreprocessor {
            radius: self.radius,
        }
    }
}

impl<N: RealField> SupportMap<N> for Capsule<N> {
    #[inline]
    fn support_point(&self, m: &Isometry<N>, dir: &Vector<N>) -> Point<N> {
        self.support_point_toward(m, &Unit::new_normalize(*dir))
    }

    #[inline]
    fn support_point_toward(&self, m: &Isometry<N>, dir: &Unit<Vector<N>>) -> Point<N> {
        let local_dir = m.inverse_transform_vector(dir);

        let mut res: Vector<N> = na::zero();

        if local_dir[1].is_negative() {
            res[1] = -self.half_height()
        } else {
            res[1] = self.half_height()
        }

        m * Point::from(res + local_dir * self.radius())
    }
}

struct CapsuleContactPreprocessor<N: RealField> {
    radius: N,
}

impl<N: RealField> CapsuleContactPreprocessor<N> {
    //    pub fn new(radius: N) -> Self {
    //        CapsuleContactPreprocessor {
    //            radius
    //        }
    //    }
}

impl<N: RealField> ContactPreprocessor<N> for CapsuleContactPreprocessor<N> {
    fn process_contact(
        &self,
        c: &mut Contact<N>,
        kinematic: &mut ContactKinematic<N>,
        is_first: bool,
    ) -> bool {
        // Fix the feature ID.
        let feature = if is_first {
            kinematic.feature1()
        } else {
            kinematic.feature2()
        };

        let actual_feature = match feature {
            FeatureId::Vertex(i) => FeatureId::Face(i),
            #[cfg(feature = "dim3")]
            FeatureId::Edge(_) => FeatureId::Face(2),
            FeatureId::Face(i) => FeatureId::Face(2 + i),
            FeatureId::Unknown => return false,
        };

        if is_first {
            kinematic.set_feature1(actual_feature);
            kinematic.set_dilation1(self.radius);
            c.world1 += *c.normal * self.radius;
            c.depth += self.radius;
        } else {
            kinematic.set_feature2(actual_feature);
            kinematic.set_dilation2(self.radius);
            c.world2 -= *c.normal * self.radius;
            c.depth += self.radius;
        }

        true
    }
}