parry3d/query/contact_manifolds/
normals_constraint.rs

1use crate::math::{Isometry, Real, Vector};
2
3// NOTE: the 'static requirement is only needed for the following impl to work:
4//       impl<'a> TypedCompositeShape for dyn CompositeShape
5//       We can probably work something out if that becomes too restrictive in
6//       the future.
7/// Constraints of contact normals, generally for internal edges resolution.
8///
9/// This trait used for applying constraints on normal direction for contact manifolds calculation.
10/// Non-convex shapes will generally simplify collision-detection as a collection of simpler
11/// convex-based collision-detection problems. However, that partial convex formulation allows
12/// some contact normals that are theoretically impossible (in a convex analysis sense). The normal
13/// constraints aims to correct/remove invalid normals, avoiding some artifacts in physics
14/// simulations. In particular, this addresses the well-known "internal edges" problem on triangle
15/// meshes and heightfields.
16pub trait NormalConstraints: 'static {
17    /// Corrects in-place or discards the specified normal (assumed to be unit-sized) based on the
18    /// constraints encoded by `Self`.
19    ///
20    /// If this method returns `false` then the contacts associated to that normal should be
21    /// considered invalid and be ignored by the collision-detection pipeline.
22    fn project_local_normal_mut(&self, normal: &mut Vector<Real>) -> bool;
23    /// Corrects or discards the specified normal (assumed to be unit-sized) based on the constraints
24    /// encoded by `Self`.
25    ///
26    /// If this method returns `None` then the contacts associated to that normal should be
27    /// considered invalid and be ignored by the collision-detection pipeline.
28    fn project_local_normal(&self, mut normal: Vector<Real>) -> Option<Vector<Real>> {
29        self.project_local_normal_mut(&mut normal).then_some(normal)
30    }
31
32    // NOTE: despite this not taking an UnitVector, the normal is
33    //       assumed to be unit-sized.
34    /// Applies normal correction to the unit vectors `normal1` and `normal2` based on the
35    /// assumption that `normal1` is in the same coordinates space as `Self`.
36    ///
37    /// The `normal2` will be modified to be equal to `-normal1` expressed in the local coordinate
38    /// space of the second shape.
39    ///
40    /// If this method returns `false` then the contacts associated to that normal should be
41    /// considered invalid and be ignored by the collision-detection pipeline.
42    fn project_local_normal1(
43        &self,
44        pos12: &Isometry<Real>,
45        normal1: &mut Vector<Real>,
46        normal2: &mut Vector<Real>,
47    ) -> bool {
48        if !self.project_local_normal_mut(normal1) {
49            return false;
50        }
51
52        *normal2 = pos12.inverse_transform_vector(&-*normal1);
53
54        true
55    }
56
57    /// Applies normal correction to the unit vectors `normal1` and `normal2` based on the
58    /// assumption that `normal2` is in the same coordinates space as `Self`.
59    ///
60    /// The `normal1` will be modified to be equal to `-normal2` expressed in the local coordinate
61    /// space of the first shape.
62    ///
63    /// If this method returns `false` then the contacts associated to that normal should be
64    /// considered invalid and be ignored by the collision-detection pipeline.
65    fn project_local_normal2(
66        &self,
67        pos12: &Isometry<Real>,
68        normal1: &mut Vector<Real>,
69        normal2: &mut Vector<Real>,
70    ) -> bool {
71        if !self.project_local_normal_mut(normal2) {
72            return false;
73        }
74
75        *normal1 = pos12 * (-*normal2);
76
77        true
78    }
79}
80
81impl NormalConstraints for () {
82    fn project_local_normal_mut(&self, _: &mut Vector<Real>) -> bool {
83        true
84    }
85}
86
87/// A pair of [`NormalConstraints`].
88pub trait NormalConstraintsPair {
89    /// Applies the normal constraints to `normal1` and `normal2`.
90    ///
91    /// This trait is mostly used internally to combine two [`NormalConstraints`] conveniently.
92    fn project_local_normals(
93        &self,
94        pos12: &Isometry<Real>,
95        normal1: &mut Vector<Real>,
96        normal2: &mut Vector<Real>,
97    ) -> bool;
98}
99
100// We generally use Option<&dyn NormalConstraints> instead of the naked
101// trait-object in our codebase so providing this impl is convenient.
102impl NormalConstraintsPair
103    for (
104        Option<&dyn NormalConstraints>,
105        Option<&dyn NormalConstraints>,
106    )
107{
108    fn project_local_normals(
109        &self,
110        pos12: &Isometry<Real>,
111        normal1: &mut Vector<Real>,
112        normal2: &mut Vector<Real>,
113    ) -> bool {
114        if let Some(proj) = self.0 {
115            if !proj.project_local_normal1(pos12, normal1, normal2) {
116                return false;
117            }
118        }
119
120        if let Some(proj) = self.1 {
121            proj.project_local_normal2(pos12, normal1, normal2)
122        } else {
123            true
124        }
125    }
126}
parry3d/query/contact_manifolds/normals_constraint.rs

parry3d/query/contact_manifolds/
normals_constraint.rs
