parry3d/query/contact_manifolds/
normals_constraint.rs

1use crate::math::{Pose, 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) -> 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) -> Option<Vector> {
29        self.project_local_normal_mut(&mut normal).then_some(normal)
30    }
31
32    // NOTE: the normal is assumed to be unit-sized.
33    /// Applies normal correction to the unit vectors `normal1` and `normal2` based on the
34    /// assumption that `normal1` is in the same coordinates space as `Self`.
35    ///
36    /// The `normal2` will be modified to be equal to `-normal1` expressed in the local coordinate
37    /// space of the second shape.
38    ///
39    /// If this method returns `false` then the contacts associated to that normal should be
40    /// considered invalid and be ignored by the collision-detection pipeline.
41    fn project_local_normal1(
42        &self,
43        pos12: &Pose,
44        normal1: &mut Vector,
45        normal2: &mut Vector,
46    ) -> bool {
47        if !self.project_local_normal_mut(normal1) {
48            return false;
49        }
50
51        *normal2 = pos12.rotation.inverse() * -*normal1;
52
53        true
54    }
55
56    /// Applies normal correction to the unit vectors `normal1` and `normal2` based on the
57    /// assumption that `normal2` is in the same coordinates space as `Self`.
58    ///
59    /// The `normal1` will be modified to be equal to `-normal2` expressed in the local coordinate
60    /// space of the first shape.
61    ///
62    /// If this method returns `false` then the contacts associated to that normal should be
63    /// considered invalid and be ignored by the collision-detection pipeline.
64    fn project_local_normal2(
65        &self,
66        pos12: &Pose,
67        normal1: &mut Vector,
68        normal2: &mut Vector,
69    ) -> bool {
70        if !self.project_local_normal_mut(normal2) {
71            return false;
72        }
73
74        *normal1 = pos12.rotation * (-*normal2);
75
76        true
77    }
78}
79
80impl NormalConstraints for () {
81    fn project_local_normal_mut(&self, _: &mut Vector) -> bool {
82        true
83    }
84}
85
86/// A pair of [`NormalConstraints`].
87pub trait NormalConstraintsPair {
88    /// Applies the normal constraints to `normal1` and `normal2`.
89    ///
90    /// This trait is mostly used internally to combine two [`NormalConstraints`] conveniently.
91    fn project_local_normals(
92        &self,
93        pos12: &Pose,
94        normal1: &mut Vector,
95        normal2: &mut Vector,
96    ) -> bool;
97}
98
99// We generally use Option<&dyn NormalConstraints> instead of the naked
100// trait-object in our codebase so providing this impl is convenient.
101impl NormalConstraintsPair
102    for (
103        Option<&dyn NormalConstraints>,
104        Option<&dyn NormalConstraints>,
105    )
106{
107    fn project_local_normals(
108        &self,
109        pos12: &Pose,
110        normal1: &mut Vector,
111        normal2: &mut Vector,
112    ) -> bool {
113        if let Some(proj) = self.0 {
114            if !proj.project_local_normal1(pos12, normal1, normal2) {
115                return false;
116            }
117        }
118
119        if let Some(proj) = self.1 {
120            proj.project_local_normal2(pos12, normal1, normal2)
121        } else {
122            true
123        }
124    }
125}
parry3d/query/contact_manifolds/normals_constraint.rs

parry3d/query/contact_manifolds/
normals_constraint.rs
