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

use crate::math::{Isometry, Real, Vector};

// NOTE: the 'static requirement is only needed for the following impl to work:
//       impl<'a> TypedSimdCompositeShape for dyn SimdCompositeShape
//       We can probably work something out if that becomes too restrictive in
//       the future.
/// Constraints of contact normals, generally for internal edges resolution.
///
/// This trait used for applying constraints on normal direction for contact manifolds calculation.
/// Non-convex shapes will generally simplify collision-detection as a collection of simpler
/// convex-based collision-detection problems. However, that partial convex formulation allows
/// some contact normals that are theoretically impossible (in a convex analysis sense). The normal
/// constraints aims to correct/remove invalid normals, avoiding some artifacts in physics
/// simulations. In particular, this addresses the well-known "internal edges" problem on triangle
/// meshes and heightfields.
pub trait NormalConstraints: 'static {
    /// Corrects in-place or discards the specified normal (assumed to be unit-sized) based on the
    /// constraints encoded by `Self`.
    ///
    /// If this method returns `false` then the contacts associated to that normal should be
    /// considered invalid and be ignored by the collision-detection pipeline.
    fn project_local_normal_mut(&self, normal: &mut Vector<Real>) -> bool;
    /// Corrects or discards the specified normal (assumed to be unit-sized) based on the constraints
    /// encoded by `Self`.
    ///
    /// If this method returns `None` then the contacts associated to that normal should be
    /// considered invalid and be ignored by the collision-detection pipeline.
    fn project_local_normal(&self, mut normal: Vector<Real>) -> Option<Vector<Real>> {
        self.project_local_normal_mut(&mut normal).then_some(normal)
    }

    // NOTE: despite this not taking an UnitVector, the normal is
    //       assumed to be unit-sized.
    /// Applies normal correction to the unit vectors `normal1` and `normal2` based on the
    /// assumption that `normal1` is in the same coordinates space as `Self`.
    ///
    /// The `normal2` will be modified to be equal to `-normal1` expressed in the local coordinate
    /// space of the second shape.
    ///
    /// If this method returns `false` then the contacts associated to that normal should be
    /// considered invalid and be ignored by the collision-detection pipeline.
    fn project_local_normal1(
        &self,
        pos12: &Isometry<Real>,
        normal1: &mut Vector<Real>,
        normal2: &mut Vector<Real>,
    ) -> bool {
        if !self.project_local_normal_mut(normal1) {
            return false;
        }

        *normal2 = pos12.inverse_transform_vector(&-*normal1);

        true
    }

    /// Applies normal correction to the unit vectors `normal1` and `normal2` based on the
    /// assumption that `normal2` is in the same coordinates space as `Self`.
    ///
    /// The `normal1` will be modified to be equal to `-normal2` expressed in the local coordinate
    /// space of the first shape.
    ///
    /// If this method returns `false` then the contacts associated to that normal should be
    /// considered invalid and be ignored by the collision-detection pipeline.
    fn project_local_normal2(
        &self,
        pos12: &Isometry<Real>,
        normal1: &mut Vector<Real>,
        normal2: &mut Vector<Real>,
    ) -> bool {
        if !self.project_local_normal_mut(normal2) {
            return false;
        }

        *normal1 = pos12 * (-*normal2);

        true
    }
}

impl NormalConstraints for () {
    fn project_local_normal_mut(&self, _: &mut Vector<Real>) -> bool {
        true
    }
}

/// A pair of [`NormalConstraints`].
pub trait NormalConstraintsPair {
    /// Applies the normal constraints to `normal1` and `normal2`.
    ///
    /// This trait is mostly used internally to combine two [`NormalConstraints`] conveniently.
    fn project_local_normals(
        &self,
        pos12: &Isometry<Real>,
        normal1: &mut Vector<Real>,
        normal2: &mut Vector<Real>,
    ) -> bool;
}

// We generally use Option<&dyn NormalConstraints> instead of the naked
// trait-object in our codebase so providing this impl is convenient.
impl NormalConstraintsPair
    for (
        Option<&dyn NormalConstraints>,
        Option<&dyn NormalConstraints>,
    )
{
    fn project_local_normals(
        &self,
        pos12: &Isometry<Real>,
        normal1: &mut Vector<Real>,
        normal2: &mut Vector<Real>,
    ) -> bool {
        if let Some(proj) = self.0 {
            if !proj.project_local_normal1(pos12, normal1, normal2) {
                return false;
            }
        }

        if let Some(proj) = self.1 {
            proj.project_local_normal2(pos12, normal1, normal2)
        } else {
            true
        }
    }
}