parry3d/query/contact_manifolds/
contact_manifolds_cuboid_cuboid.rs

1#[cfg(feature = "dim2")]
2use crate::math::Vector;
3use crate::math::{Isometry, Real};
4use crate::query::{sat, ContactManifold};
5use crate::shape::{Cuboid, PolygonalFeature, Shape};
6
7/// Computes the contact manifold between two cuboids represented as `Shape` trait-objects.
8pub fn contact_manifold_cuboid_cuboid_shapes<ManifoldData, ContactData: Default + Copy>(
9    pos12: &Isometry<Real>,
10    g1: &dyn Shape,
11    g2: &dyn Shape,
12    prediction: Real,
13    manifold: &mut ContactManifold<ManifoldData, ContactData>,
14) {
15    if let (Some(cuboid1), Some(cuboid2)) = (g1.as_cuboid(), g2.as_cuboid()) {
16        contact_manifold_cuboid_cuboid(pos12, cuboid1, cuboid2, prediction, manifold);
17    }
18}
19
20/// Computes the contact manifold between two cuboids.
21pub fn contact_manifold_cuboid_cuboid<'a, ManifoldData, ContactData: Default + Copy>(
22    pos12: &Isometry<Real>,
23    cuboid1: &'a Cuboid,
24    cuboid2: &'a Cuboid,
25    prediction: Real,
26    manifold: &mut ContactManifold<ManifoldData, ContactData>,
27) {
28    if manifold.try_update_contacts(pos12) {
29        return;
30    }
31
32    let pos21 = &pos12.inverse();
33
34    /*
35     *
36     * Point-Face
37     *
38     */
39    let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cuboid1, cuboid2, pos12);
40    if sep1.0 > prediction {
41        manifold.clear();
42        return;
43    }
44
45    let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cuboid2, cuboid1, pos21);
46    if sep2.0 > prediction {
47        manifold.clear();
48        return;
49    }
50
51    /*
52     *
53     * Edge-Edge cases
54     *
55     */
56    #[cfg(feature = "dim2")]
57    let sep3 = (-Real::MAX, Vector::x()); // This case does not exist in 2D.
58    #[cfg(feature = "dim3")]
59    let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cuboid1, cuboid2, pos12);
60    if sep3.0 > prediction {
61        manifold.clear();
62        return;
63    }
64
65    /*
66     *
67     * Select the best combination of features
68     * and get the polygons to clip.
69     *
70     */
71    let mut best_sep = sep1;
72
73    if sep2.0 > sep1.0 && sep2.0 > sep3.0 {
74        best_sep = (sep2.0, pos12 * -sep2.1);
75    } else if sep3.0 > sep1.0 {
76        best_sep = sep3;
77    }
78
79    // We do this clone to perform contact tracking and transfer impulses.
80    // TODO: find a more efficient way of doing this.
81    let old_manifold_points = manifold.points.clone();
82    manifold.clear();
83
84    let local_n2 = pos21 * -best_sep.1;
85
86    // Now the reference feature is from `cuboid1` and the best separation is `best_sep`.
87    // Everything must be expressed in the local-space of `cuboid1` for contact clipping.
88    let feature1 = cuboid1.support_feature(best_sep.1);
89    let feature2 = cuboid2.support_feature(local_n2);
90
91    PolygonalFeature::contacts(
92        pos12,
93        pos21,
94        &best_sep.1,
95        &local_n2,
96        &feature1,
97        &feature2,
98        manifold,
99        false,
100    );
101
102    manifold.local_n1 = best_sep.1;
103    manifold.local_n2 = local_n2;
104
105    // Transfer impulses.
106    manifold.match_contacts(&old_manifold_points);
107}
parry3d/query/contact_manifolds/contact_manifolds_cuboid_cuboid.rs

parry3d/query/contact_manifolds/
contact_manifolds_cuboid_cuboid.rs
