parry3d/query/contact_manifolds/
contact_manifold.rs

1use crate::math::{Isometry, Point, Real, Vector};
2use crate::shape::PackedFeatureId;
3#[cfg(feature = "dim3")]
4use alloc::vec::Vec;
5
6#[derive(Copy, Clone, Debug)]
7#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
8#[cfg_attr(
9    feature = "rkyv",
10    derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize),
11    archive(check_bytes)
12)]
13/// A single contact between two shape.
14pub struct TrackedContact<Data> {
15    /// The contact point in the local-space of the first shape.
16    pub local_p1: Point<Real>,
17    /// The contact point in the local-space of the second shape.
18    pub local_p2: Point<Real>,
19    /// The distance between the two contact points.
20    pub dist: Real,
21
22    /// The feature ID of the first shape involved in the contact.
23    pub fid1: PackedFeatureId,
24    /// The feature ID of the second shape involved in the contact.
25    pub fid2: PackedFeatureId,
26    /// User-data associated to this contact.
27    pub data: Data,
28}
29
30impl<Data: Default + Copy> TrackedContact<Data> {
31    /// Creates a new tracked contact.
32    pub fn new(
33        local_p1: Point<Real>,
34        local_p2: Point<Real>,
35        fid1: PackedFeatureId,
36        fid2: PackedFeatureId,
37        dist: Real,
38    ) -> Self {
39        Self {
40            local_p1,
41            local_p2,
42            fid1,
43            fid2,
44            dist,
45            data: Data::default(),
46        }
47    }
48
49    /// Creates a new tracked contact where its input may need to be flipped.
50    pub fn flipped(
51        local_p1: Point<Real>,
52        local_p2: Point<Real>,
53        fid1: PackedFeatureId,
54        fid2: PackedFeatureId,
55        dist: Real,
56        flipped: bool,
57    ) -> Self {
58        if !flipped {
59            Self::new(local_p1, local_p2, fid1, fid2, dist)
60        } else {
61            Self::new(local_p2, local_p1, fid2, fid1, dist)
62        }
63    }
64
65    /// Copy to `self` the geometric information from `contact`.
66    pub fn copy_geometry_from(&mut self, contact: Self) {
67        self.local_p1 = contact.local_p1;
68        self.local_p2 = contact.local_p2;
69        self.fid1 = contact.fid1;
70        self.fid2 = contact.fid2;
71        self.dist = contact.dist;
72    }
73}
74
75#[derive(Clone, Debug, Default)]
76#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
77/// A contact manifold between two shapes.
78///
79/// A contact manifold describes a set of contacts between two shapes. All the contact
80/// part of the same contact manifold share the same contact normal and contact kinematics.
81pub struct ContactManifold<ManifoldData, ContactData> {
82    // NOTE: use a SmallVec instead?
83    // And for 2D use an ArrayVec since there will never be more than 2 contacts anyways.
84    /// The contacts points.
85    #[cfg(feature = "dim2")]
86    pub points: arrayvec::ArrayVec<TrackedContact<ContactData>, 2>,
87    /// The contacts points.
88    #[cfg(feature = "dim3")]
89    pub points: Vec<TrackedContact<ContactData>>,
90    /// The contact normal of all the contacts of this manifold, expressed in the local space of the first shape.
91    pub local_n1: Vector<Real>,
92    /// The contact normal of all the contacts of this manifold, expressed in the local space of the second shape.
93    pub local_n2: Vector<Real>,
94    /// The first subshape involved in this contact manifold.
95    ///
96    /// This is zero if the first shape is not a composite shape.
97    pub subshape1: u32,
98    /// The second subshape involved in this contact manifold.
99    ///
100    /// This is zero if the second shape is not a composite shape.
101    pub subshape2: u32,
102    /// If the first shape involved is a composite shape, this contains the position of its subshape
103    /// involved in this contact.
104    pub subshape_pos1: Option<Isometry<Real>>,
105    /// If the second shape involved is a composite shape, this contains the position of its subshape
106    /// involved in this contact.
107    pub subshape_pos2: Option<Isometry<Real>>,
108    /// Additional tracked data associated to this contact manifold.
109    pub data: ManifoldData,
110}
111
112impl<ManifoldData, ContactData: Default + Copy> ContactManifold<ManifoldData, ContactData> {
113    /// Create a new empty contact-manifold.
114    pub fn new() -> Self
115    where
116        ManifoldData: Default,
117    {
118        Self::default()
119    }
120
121    /// Create a new empty contact-manifold with the given associated data.
122    pub fn with_data(subshape1: u32, subshape2: u32, data: ManifoldData) -> Self {
123        Self {
124            #[cfg(feature = "dim2")]
125            points: arrayvec::ArrayVec::new(),
126            #[cfg(feature = "dim3")]
127            points: Vec::new(),
128            local_n1: Vector::zeros(),
129            local_n2: Vector::zeros(),
130            subshape1,
131            subshape2,
132            subshape_pos1: None,
133            subshape_pos2: None,
134            data,
135        }
136    }
137
138    /// Clones `self` and then remove all contact points from `self`.
139    pub fn take(&mut self) -> Self
140    where
141        ManifoldData: Clone,
142    {
143        #[cfg(feature = "dim2")]
144        let points = self.points.clone();
145        #[cfg(feature = "dim3")]
146        let points = core::mem::take(&mut self.points);
147        self.points.clear();
148
149        ContactManifold {
150            points,
151            local_n1: self.local_n1,
152            local_n2: self.local_n2,
153            subshape1: self.subshape1,
154            subshape2: self.subshape2,
155            subshape_pos1: self.subshape_pos1,
156            subshape_pos2: self.subshape_pos2,
157            data: self.data.clone(),
158        }
159    }
160
161    /*
162    pub(crate) fn single_manifold<'a, 'b>(
163        manifolds: &mut Vec<Self>,
164        data: &dyn Fn() -> ManifoldData,
165    ) -> &'a mut Self {
166        if manifolds.is_empty() {
167            let manifold_data = data();
168            manifolds.push(ContactManifold::with_data((0, 0), manifold_data));
169        }
170
171        &mut manifolds[0]
172    }
173    */
174
175    /// The slice of all the contacts, active or not, on this contact manifold.
176    #[inline]
177    pub fn contacts(&self) -> &[TrackedContact<ContactData>] {
178        &self.points
179    }
180
181    /// Attempts to use spatial coherence to update contacts points.
182    #[inline]
183    pub fn try_update_contacts(&mut self, pos12: &Isometry<Real>) -> bool {
184        // const DOT_THRESHOLD: Real = 0.crate::COS_10_DEGREES;
185        // const DOT_THRESHOLD: Real = crate::utils::COS_5_DEGREES;
186        const DOT_THRESHOLD: Real = crate::utils::COS_1_DEGREES;
187        const DIST_SQ_THRESHOLD: Real = 1.0e-6; // TODO: this should not be hard-coded.
188        self.try_update_contacts_eps(pos12, DOT_THRESHOLD, DIST_SQ_THRESHOLD)
189    }
190
191    /// Attempts to use spatial coherence to update contacts points, using user-defined tolerances.
192    #[inline]
193    pub fn try_update_contacts_eps(
194        &mut self,
195        pos12: &Isometry<Real>,
196        angle_dot_threshold: Real,
197        dist_sq_threshold: Real,
198    ) -> bool {
199        if self.points.is_empty() {
200            return false;
201        }
202
203        let local_n2 = pos12 * self.local_n2;
204
205        if -self.local_n1.dot(&local_n2) < angle_dot_threshold {
206            return false;
207        }
208
209        for pt in &mut self.points {
210            let local_p2 = pos12 * pt.local_p2;
211            let dpt = local_p2 - pt.local_p1;
212            let dist = dpt.dot(&self.local_n1);
213
214            if dist * pt.dist < 0.0 {
215                // We switched between penetrating/non-penetrating.
216                // The may result in other contacts to appear.
217                return false;
218            }
219            let new_local_p1 = local_p2 - self.local_n1 * dist;
220
221            if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_sq_threshold {
222                return false;
223            }
224
225            pt.dist = dist;
226            pt.local_p1 = new_local_p1;
227        }
228
229        true
230    }
231
232    /// Copy data associated to contacts from `old_contacts` to the new contacts in `self`
233    /// based on matching their feature-ids.
234    pub fn match_contacts(&mut self, old_contacts: &[TrackedContact<ContactData>]) {
235        for contact in &mut self.points {
236            for old_contact in old_contacts {
237                if contact.fid1 == old_contact.fid1 && contact.fid2 == old_contact.fid2 {
238                    // Transfer the tracked data.
239                    contact.data = old_contact.data;
240                }
241            }
242        }
243    }
244
245    /// Copy data associated to contacts from `old_contacts` to the new contacts in `self`
246    /// based on matching the contact positions.
247    pub fn match_contacts_using_positions(
248        &mut self,
249        old_contacts: &[TrackedContact<ContactData>],
250        dist_threshold: Real,
251    ) {
252        let sq_threshold = dist_threshold * dist_threshold;
253        for contact in &mut self.points {
254            for old_contact in old_contacts {
255                if na::distance_squared(&contact.local_p1, &old_contact.local_p1) < sq_threshold
256                    && na::distance_squared(&contact.local_p2, &old_contact.local_p2) < sq_threshold
257                {
258                    // Transfer the tracked data.
259                    contact.data = old_contact.data;
260                }
261            }
262        }
263    }
264
265    /// Removes all the contacts from `self`.
266    pub fn clear(&mut self) {
267        self.points.clear();
268    }
269
270    /// Returns the contact with the smallest distance (i.e. the largest penetration depth).
271    pub fn find_deepest_contact(&self) -> Option<&TrackedContact<ContactData>> {
272        let mut deepest = self.points.first()?;
273
274        for pt in &self.points {
275            if pt.dist < deepest.dist {
276                deepest = pt;
277            }
278        }
279
280        Some(deepest)
281    }
282}
parry3d/query/contact_manifolds/contact_manifold.rs

parry3d/query/contact_manifolds/
contact_manifold.rs
