parry3d/query/closest_points/
closest_points_cuboid_cuboid.rs

1use crate::math::{Isometry, 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: &Isometry<Real>,
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::<Real>::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        // To compute the closest points, we need to project the support point
42        // from cuboid2 on the support-face of cuboid1. For simplicity, we just
43        // project the support point from cuboid2 on cuboid1 itself (not just the face).
44        let pt2_1 = cuboid2.support_point(pos12, &-sep1.1);
45        let proj1 = cuboid1.project_local_point(&pt2_1, true);
46        if na::distance_squared(&proj1.point, &pt2_1) > margin * margin {
47            return ClosestPoints::Disjoint;
48        } else {
49            return ClosestPoints::WithinMargin(proj1.point, pos21 * pt2_1);
50        }
51    }
52
53    // The best separating axis is vertex-face.
54    if sep2.0 >= sep1.0 && sep2.0 >= sep3.0 {
55        // println!("BB: {:?}", sep2);
56
57        // To compute the actual closest points, we need to project the support point
58        // from cuboid1 on the support-face of cuboid2. For simplicity, we just
59        // project the support point from cuboid1 on cuboid2 itself (not just the face).
60        let pt1_2 = cuboid1.support_point(&pos21, &-sep2.1);
61        let proj2 = cuboid2.project_local_point(&pt1_2, true);
62
63        if na::distance_squared(&proj2.point, &pt1_2) > margin * margin {
64            return ClosestPoints::Disjoint;
65        } else {
66            return ClosestPoints::WithinMargin(pos12 * pt1_2, proj2.point);
67        }
68    }
69
70    // The best separating axis is edge-edge.
71    #[cfg(feature = "dim3")]
72    if sep3.0 >= sep2.0 && sep3.0 >= sep1.0 {
73        // println!("AA: {:?}, BB: {:?}, CC: {:?}", sep1, sep2, sep3);
74
75        // To compute the actual distance, we need to compute the closest
76        // points between the two edges that generated the separating axis.
77        let edge1 = cuboid1.local_support_edge_segment(sep3.1);
78        let edge2 = cuboid2.local_support_edge_segment(pos21 * -sep3.1);
79        return super::closest_points_segment_segment(pos12, &edge1, &edge2, margin);
80    }
81
82    unreachable!()
83}
parry3d/query/closest_points/closest_points_cuboid_cuboid.rs

parry3d/query/closest_points/
closest_points_cuboid_cuboid.rs
