parry2d/query/contact/
contact_cuboid_cuboid.rs

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

parry2d/query/contact/
contact_cuboid_cuboid.rs
