parry2d/query/closest_points/
closest_points_cuboid_cuboid.rs

1use crate::math::{Pose, Real};
2use crate::query::{sat, ClosestPoints, PointQuery};
3use crate::shape::{Cuboid, SupportMap};
4
5/// Closest points between two cuboids.
6#[inline]
7pub fn closest_points_cuboid_cuboid(
8    pos12: &Pose,
9    cuboid1: &Cuboid,
10    cuboid2: &Cuboid,
11    margin: Real,
12) -> ClosestPoints {
13    let pos21 = pos12.inverse();
14
15    let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cuboid1, cuboid2, pos12);
16    if sep1.0 > margin {
17        return ClosestPoints::Disjoint;
18    }
19
20    let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cuboid2, cuboid1, &pos21);
21    if sep2.0 > margin {
22        return ClosestPoints::Disjoint;
23    }
24
25    #[cfg(feature = "dim2")]
26    let sep3 = (-Real::MAX, crate::math::Vector::Y); // This case does not exist in 2D.
27    #[cfg(feature = "dim3")]
28    let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cuboid1, cuboid2, pos12);
29    if sep3.0 > margin {
30        return ClosestPoints::Disjoint;
31    }
32
33    if sep1.0 <= 0.0 && sep2.0 <= 0.0 && sep3.0 <= 0.0 {
34        return ClosestPoints::Intersecting;
35    }
36
37    // The best separating axis is face-vertex.
38    if sep1.0 >= sep2.0 && sep1.0 >= sep3.0 {
39        // println!("AA: {:?}", sep1);
40
41        let pt2_1 = cuboid2.support_point(pos12, -sep1.1);
42        // Use double-projection to converge to the closest points even in ambiguous cases
43        // where both cuboids have some parallel features.
44        let proj1 = cuboid1.project_local_point(pt2_1, true);
45        let proj2 = cuboid2.project_local_point(pos21 * proj1.point, true);
46        let pt2_1 = pos12 * proj2.point;
47        if (proj1.point - pt2_1).length_squared() > margin * margin {
48            return ClosestPoints::Disjoint;
49        } else {
50            return ClosestPoints::WithinMargin(proj1.point, proj2.point);
51        }
52    }
53
54    // The best separating axis is vertex-face.
55    if sep2.0 >= sep1.0 && sep2.0 >= sep3.0 {
56        // println!("BB: {:?}", sep2);
57
58        // Use double-projection to converge to the closest points even in ambiguous cases
59        // where both cuboids have some parallel features.
60        let pt1_2 = cuboid1.support_point(&pos21, -sep2.1);
61        let proj2 = cuboid2.project_local_point(pt1_2, true);
62        let proj1 = cuboid1.project_local_point(pos12 * proj2.point, true);
63        let pt1_2 = pos21 * proj1.point;
64
65        if (proj2.point - pt1_2).length_squared() > margin * margin {
66            return ClosestPoints::Disjoint;
67        } else {
68            return ClosestPoints::WithinMargin(proj1.point, proj2.point);
69        }
70    }
71
72    // The best separating axis is edge-edge.
73    #[cfg(feature = "dim3")]
74    if sep3.0 >= sep2.0 && sep3.0 >= sep1.0 {
75        // println!("AA: {:?}, BB: {:?}, CC: {:?}", sep1, sep2, sep3);
76
77        // To compute the actual distance, we need to compute the closest
78        // points between the two edges that generated the separating axis.
79        let edge1 = cuboid1.local_support_edge_segment(sep3.1);
80        let edge2 = cuboid2.local_support_edge_segment(pos21.rotation * -sep3.1);
81        let (_, loc2) = super::closest_points_segment_segment_with_locations(pos12, &edge1, &edge2);
82
83        // Use double-projection to converge to the closest points even in ambiguous cases
84        // where both cuboids have some parallel features.
85        let pt2_1 = pos12 * edge2.point_at(&loc2);
86        let proj1 = cuboid1.project_local_point(pt2_1, true);
87        let proj2 = cuboid2.project_local_point(pos21 * proj1.point, true);
88        let pt2_1 = pos12 * proj2.point;
89        if (proj1.point - pt2_1).length_squared() > margin * margin {
90            return ClosestPoints::Disjoint;
91        } else {
92            return ClosestPoints::WithinMargin(proj1.point, proj2.point);
93        }
94    }
95
96    unreachable!()
97}
parry2d/query/closest_points/closest_points_cuboid_cuboid.rs

parry2d/query/closest_points/
closest_points_cuboid_cuboid.rs
