parry3d/query/contact_manifolds/
contact_manifolds_cuboid_triangle.rs

1#[cfg(feature = "dim2")]
2use crate::math::Vector;
3use crate::math::{Isometry, Real};
4use crate::query::contact_manifolds::{NormalConstraints, NormalConstraintsPair};
5use crate::query::{sat, ContactManifold};
6use crate::shape::PolygonalFeature;
7use crate::shape::{Cuboid, Shape, Triangle};
8
9/// Computes the contact manifold between a cuboid and a triangle represented as `Shape` trait-objects.
10pub fn contact_manifold_cuboid_triangle_shapes<ManifoldData, ContactData>(
11    pos12: &Isometry<Real>,
12    shape1: &dyn Shape,
13    shape2: &dyn Shape,
14    normal_constraints1: Option<&dyn NormalConstraints>,
15    normal_constraints2: Option<&dyn NormalConstraints>,
16    prediction: Real,
17    manifold: &mut ContactManifold<ManifoldData, ContactData>,
18) where
19    ContactData: Default + Copy,
20{
21    if let (Some(cuboid1), Some(triangle2)) = (shape1.as_cuboid(), shape2.as_triangle()) {
22        contact_manifold_cuboid_triangle(
23            pos12,
24            &pos12.inverse(),
25            cuboid1,
26            triangle2,
27            normal_constraints1,
28            normal_constraints2,
29            prediction,
30            manifold,
31            false,
32        );
33    } else if let (Some(triangle1), Some(cuboid2)) = (shape1.as_triangle(), shape2.as_cuboid()) {
34        contact_manifold_cuboid_triangle(
35            &pos12.inverse(),
36            pos12,
37            cuboid2,
38            triangle1,
39            normal_constraints2,
40            normal_constraints1,
41            prediction,
42            manifold,
43            true,
44        );
45    }
46}
47
48/// Computes the contact manifold between a cuboid and a triangle.
49pub fn contact_manifold_cuboid_triangle<'a, ManifoldData, ContactData>(
50    pos12: &Isometry<Real>,
51    pos21: &Isometry<Real>,
52    cuboid1: &'a Cuboid,
53    triangle2: &'a Triangle,
54    normal_constraints1: Option<&dyn NormalConstraints>,
55    normal_constraints2: Option<&dyn NormalConstraints>,
56    prediction: Real,
57    manifold: &mut ContactManifold<ManifoldData, ContactData>,
58    flipped: bool,
59) where
60    ContactData: Default + Copy,
61{
62    if (!flipped && manifold.try_update_contacts(pos12))
63        || (flipped && manifold.try_update_contacts(pos21))
64    {
65        return;
66    }
67
68    /*
69     *
70     * Point-Face cases.
71     *
72     */
73    let sep1 =
74        sat::cuboid_support_map_find_local_separating_normal_oneway(cuboid1, triangle2, pos12);
75    if sep1.0 > prediction {
76        manifold.clear();
77        return;
78    }
79
80    let sep2 = sat::triangle_cuboid_find_local_separating_normal_oneway(triangle2, cuboid1, pos21);
81    if sep2.0 > prediction {
82        manifold.clear();
83        return;
84    }
85
86    /*
87     *
88     * Edge-Edge cases.
89     *
90     */
91    #[cfg(feature = "dim2")]
92    let sep3 = (-Real::MAX, Vector::x()); // This case does not exist in 2D.
93    #[cfg(feature = "dim3")]
94    let sep3 = sat::cuboid_triangle_find_local_separating_edge_twoway(cuboid1, triangle2, pos12);
95    if sep3.0 > prediction {
96        manifold.clear();
97        return;
98    }
99
100    /*
101     *
102     * Select the best combination of features
103     * and get the polygons to clip.
104     *
105     */
106    let mut normal1 = sep1.1;
107    let mut dist = sep1.0;
108
109    if sep2.0 > sep1.0 && sep2.0 > sep3.0 {
110        normal1 = pos12 * -sep2.1;
111        dist = sep2.0;
112    } else if sep3.0 > sep1.0 {
113        normal1 = sep3.1;
114        dist = sep3.0;
115    }
116
117    // Apply any normal constraint to the separating axis.
118    let mut normal2 = pos21 * -normal1;
119
120    if !(normal_constraints1, normal_constraints2).project_local_normals(
121        pos12,
122        &mut normal1,
123        &mut normal2,
124    ) {
125        manifold.clear();
126        return; // The contact got completely discarded by normal correction.
127    }
128
129    let feature1;
130    let feature2;
131
132    #[cfg(feature = "dim2")]
133    {
134        feature1 = cuboid1.support_face(normal1);
135        feature2 = triangle2.support_face(normal2);
136    }
137    #[cfg(feature = "dim3")]
138    {
139        feature1 = cuboid1.support_face(normal1);
140        feature2 = PolygonalFeature::from(*triangle2);
141    }
142
143    // We do this clone to perform contact tracking and transfer impulses.
144    // TODO: find a more efficient way of doing this.
145    let old_manifold_points = manifold.points.clone();
146    manifold.clear();
147
148    PolygonalFeature::contacts(
149        pos12, pos21, &normal1, &normal2, &feature1, &feature2, manifold, flipped,
150    );
151
152    if normal_constraints1.is_some() || normal_constraints2.is_some() {
153        // HACK: some normal correction can lead to very incorrect penetration
154        //       depth, e.g., if the other object extends very far toward that direction.
155        //       This is caused by the locality of the convex/convex check.
156        //       I haven’t found a good mathematically robust approach to account for
157        //       that locally, so for now, we eliminate points that are large divergence
158        //       relative to the unconstrained penetration distance.
159        manifold
160            .points
161            .retain(|pt| dist >= 0.0 || pt.dist >= 0.0 || pt.dist >= dist * 5.0);
162    }
163
164    if flipped {
165        manifold.local_n1 = normal2;
166        manifold.local_n2 = normal1;
167    } else {
168        manifold.local_n1 = normal1;
169        manifold.local_n2 = normal2;
170    }
171
172    // Transfer impulses.
173    manifold.match_contacts(&old_manifold_points);
174}
parry3d/query/contact_manifolds/contact_manifolds_cuboid_triangle.rs

parry3d/query/contact_manifolds/
contact_manifolds_cuboid_triangle.rs
