parry3d/query/contact/
contact_cuboid_cuboid.rs

1use crate::math::{Isometry, Real};
2use crate::query::{sat, Contact, PointQuery};
3use crate::shape::{Cuboid, SupportMap};
4use approx::AbsDiffEq;
5use na::Unit;
6
7/// Contact between two cuboids.
8#[inline]
9pub fn contact_cuboid_cuboid(
10    pos12: &Isometry<Real>,
11    cuboid1: &Cuboid,
12    cuboid2: &Cuboid,
13    prediction: Real,
14) -> Option<Contact> {
15    let pos21 = pos12.inverse();
16
17    let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cuboid1, cuboid2, pos12);
18    if sep1.0 > prediction {
19        return None;
20    }
21
22    let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cuboid2, cuboid1, &pos21);
23    if sep2.0 > prediction {
24        return None;
25    }
26
27    #[cfg(feature = "dim2")]
28    let sep3 = (-Real::MAX, crate::math::Vector::<Real>::y()); // This case does not exist in 2D.
29    #[cfg(feature = "dim3")]
30    let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cuboid1, cuboid2, pos12);
31    if sep3.0 > prediction {
32        return None;
33    }
34
35    // The best separating axis is face-vertex.
36    if sep1.0 >= sep2.0 && sep1.0 >= sep3.0 {
37        // To compute the closest points, we need to project the support point
38        // from cuboid2 on the support-face of cuboid1. For simplicity, we just
39        // project the support point from cuboid2 on cuboid1 itself (not just the face).
40        let pt2_1 = cuboid2.support_point(pos12, &-sep1.1);
41        let proj1 = cuboid1.project_local_point(&pt2_1, false);
42
43        let separation = (pt2_1 - proj1.point).dot(&sep1.1);
44        let normalized_dir = Unit::try_new_and_get(pt2_1 - proj1.point, Real::default_epsilon());
45        let normal1;
46        let dist;
47
48        // NOTE: we had to recompute the normal because we can't use
49        // the separation vector for the case where we have a vertex-vertex contact.
50        if separation < 0.0 || normalized_dir.is_none() {
51            // Penetration or contact lying on the boundary exactly.
52            normal1 = Unit::new_unchecked(sep1.1);
53            dist = separation;
54        } else {
55            let Some((dir, norm)) = normalized_dir else {
56                unreachable!()
57            };
58            // No penetration.
59            normal1 = dir;
60            dist = norm;
61        }
62
63        if dist > prediction {
64            return None;
65        }
66
67        return Some(Contact::new(
68            proj1.point,
69            pos12.inverse_transform_point(&pt2_1),
70            normal1,
71            pos12.inverse_transform_unit_vector(&-normal1),
72            dist,
73        ));
74    }
75
76    // The best separating axis is vertex-face.
77    if sep2.0 >= sep1.0 && sep2.0 >= sep3.0 {
78        // To compute the actual closest points, we need to project the support point
79        // from cuboid1 on the support-face of cuboid2. For simplicity, we just
80        // project the support point from cuboid1 on cuboid2 itself (not just the face).
81        let pt1_2 = cuboid1.support_point(&pos21, &-sep2.1);
82        let proj2 = cuboid2.project_local_point(&pt1_2, false);
83
84        let separation = (pt1_2 - proj2.point).dot(&sep2.1);
85        let normalized_dir = Unit::try_new_and_get(pt1_2 - proj2.point, Real::default_epsilon());
86        let normal2;
87        let dist;
88
89        // NOTE: we had to recompute the normal because we can't use
90        // the separation vector for the case where we have a vertex-vertex contact.
91        if separation < 0.0 || normalized_dir.is_none() {
92            // Penetration or contact lying on the boundary exactly.
93            normal2 = Unit::new_unchecked(sep2.1);
94            dist = separation;
95        } else {
96            // No penetration.
97            let Some((dir, norm)) = normalized_dir else {
98                unreachable!()
99            };
100            normal2 = dir;
101            dist = norm;
102        }
103
104        if dist > prediction {
105            return None;
106        }
107
108        return Some(Contact::new(
109            pos12.transform_point(&pt1_2),
110            proj2.point,
111            pos12 * -normal2,
112            normal2,
113            dist,
114        ));
115    }
116
117    // The best separating axis is edge-edge.
118    #[cfg(feature = "dim3")]
119    if sep3.0 >= sep2.0 && sep3.0 >= sep1.0 {
120        use crate::query::{details, ClosestPoints};
121        // To compute the actual distance, we need to compute the closest
122        // points between the two edges that generated the separating axis.
123        let edge1 = cuboid1.local_support_edge_segment(sep3.1);
124        let edge2 = cuboid2.local_support_edge_segment(pos21 * -sep3.1);
125
126        match details::closest_points_segment_segment(pos12, &edge1, &edge2, prediction) {
127            ClosestPoints::Disjoint => return None,
128            ClosestPoints::WithinMargin(a, b) => {
129                let normal1 = Unit::new_unchecked(sep3.1);
130                let normal2 = pos12.inverse_transform_unit_vector(&-normal1);
131                return Some(Contact::new(a, b, normal1, normal2, sep3.0));
132            }
133            ClosestPoints::Intersecting => unreachable!(),
134        }
135    }
136
137    unreachable!()
138}
parry3d/query/contact/contact_cuboid_cuboid.rs

parry3d/query/contact/
contact_cuboid_cuboid.rs
