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
}
}
}