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 (dir, norm) = normalized_dir.unwrap();
56            // No penetration.
57            normal1 = dir;
58            dist = norm;
59        }
60
61        if dist > prediction {
62            return None;
63        }
64
65        return Some(Contact::new(
66            proj1.point,
67            pos12.inverse_transform_point(&pt2_1),
68            normal1,
69            pos12.inverse_transform_unit_vector(&-normal1),
70            dist,
71        ));
72    }
73
74    // The best separating axis is vertex-face.
75    if sep2.0 >= sep1.0 && sep2.0 >= sep3.0 {
76        // To compute the actual closest points, we need to project the support point
77        // from cuboid1 on the support-face of cuboid2. For simplicity, we just
78        // project the support point from cuboid1 on cuboid2 itself (not just the face).
79        let pt1_2 = cuboid1.support_point(&pos21, &-sep2.1);
80        let proj2 = cuboid2.project_local_point(&pt1_2, false);
81
82        let separation = (pt1_2 - proj2.point).dot(&sep2.1);
83        let normalized_dir = Unit::try_new_and_get(pt1_2 - proj2.point, Real::default_epsilon());
84        let normal2;
85        let dist;
86
87        // NOTE: we had to recompute the normal because we can't use
88        // the separation vector for the case where we have a vertex-vertex contact.
89        if separation < 0.0 || normalized_dir.is_none() {
90            // Penetration or contact lying on the boundary exactly.
91            normal2 = Unit::new_unchecked(sep2.1);
92            dist = separation;
93        } else {
94            // No penetration.
95            let (dir, norm) = normalized_dir.unwrap();
96            normal2 = dir;
97            dist = norm;
98        }
99
100        if dist > prediction {
101            return None;
102        }
103
104        return Some(Contact::new(
105            pos12.transform_point(&pt1_2),
106            proj2.point,
107            pos12 * -normal2,
108            normal2,
109            dist,
110        ));
111    }
112
113    // The best separating axis is edge-edge.
114    #[cfg(feature = "dim3")]
115    if sep3.0 >= sep2.0 && sep3.0 >= sep1.0 {
116        use crate::query::{details, ClosestPoints};
117        // To compute the actual distance, we need to compute the closest
118        // points between the two edges that generated the separating axis.
119        let edge1 = cuboid1.local_support_edge_segment(sep3.1);
120        let edge2 = cuboid2.local_support_edge_segment(pos21 * -sep3.1);
121
122        match details::closest_points_segment_segment(pos12, &edge1, &edge2, prediction) {
123            ClosestPoints::Disjoint => return None,
124            ClosestPoints::WithinMargin(a, b) => {
125                let normal1 = Unit::new_unchecked(sep3.1);
126                let normal2 = pos12.inverse_transform_unit_vector(&-normal1);
127                return Some(Contact::new(a, b, normal1, normal2, sep3.0));
128            }
129            ClosestPoints::Intersecting => unreachable!(),
130        }
131    }
132
133    unreachable!()
134}
parry3d/query/contact/contact_cuboid_cuboid.rs

parry3d/query/contact/
contact_cuboid_cuboid.rs
