parry3d/query/closest_points/
closest_points_cuboid_triangle.rs

1use crate::math::{Isometry, Real};
2use crate::query::{sat, ClosestPoints, PointQuery};
3use crate::shape::{Cuboid, SupportMap, Triangle};
4
5/// Closest points between a cuboid and a triangle.
6#[inline]
7pub fn closest_points_cuboid_triangle(
8    pos12: &Isometry<Real>,
9    cuboid1: &Cuboid,
10    triangle2: &Triangle,
11    margin: Real,
12) -> ClosestPoints {
13    let pos21 = pos12.inverse();
14
15    let sep1 =
16        sat::cuboid_support_map_find_local_separating_normal_oneway(cuboid1, triangle2, pos12);
17    if sep1.0 > margin {
18        return ClosestPoints::Disjoint;
19    }
20
21    let sep2 = sat::triangle_cuboid_find_local_separating_normal_oneway(triangle2, cuboid1, &pos21);
22    if sep2.0 > margin {
23        return ClosestPoints::Disjoint;
24    }
25
26    #[cfg(feature = "dim2")]
27    let sep3 = (-Real::MAX, crate::math::Vector::<Real>::y()); // This case does not exist in 2D.
28    #[cfg(feature = "dim3")]
29    let sep3 = sat::cuboid_triangle_find_local_separating_edge_twoway(cuboid1, triangle2, pos12);
30    if sep3.0 > margin {
31        return ClosestPoints::Disjoint;
32    }
33
34    if sep1.0 <= 0.0 && sep2.0 <= 0.0 && sep3.0 <= 0.0 {
35        return ClosestPoints::Intersecting;
36    }
37
38    // The best separating axis is face-vertex.
39    if sep1.0 >= sep2.0 && sep1.0 >= sep3.0 {
40        // To compute the closest points, we need to project the support point
41        // from triangle2 on the support-face of triangle1. For simplicity, we just
42        // project the support point from triangle2 on cuboid1 itself (not just the face).
43        let pt2_1 = triangle2.support_point(pos12, &-sep1.1);
44        let proj1 = cuboid1.project_local_point(&pt2_1, true);
45        if na::distance_squared(&proj1.point, &pt2_1) > margin * margin {
46            return ClosestPoints::Disjoint;
47        } else {
48            return ClosestPoints::WithinMargin(proj1.point, pos21 * pt2_1);
49        }
50    }
51
52    // The best separating axis is vertex-face.
53    if sep2.0 >= sep1.0 && sep2.0 >= sep3.0 {
54        // To compute the actual closest points, we need to project the support point
55        // from cuboid1 on the support-face of triangle2. For simplicity, we just
56        // project the support point from cuboid1 on triangle2 itself (not just the face).
57        let pt1_2 = cuboid1.support_point(&pos21, &-sep2.1);
58        let proj2 = triangle2.project_local_point(&pt1_2, true);
59
60        if na::distance_squared(&proj2.point, &pt1_2) > margin * margin {
61            return ClosestPoints::Disjoint;
62        } else {
63            return ClosestPoints::WithinMargin(pos12 * pt1_2, proj2.point);
64        }
65    }
66
67    // The best separating axis is edge-edge.
68    #[cfg(feature = "dim3")]
69    if sep3.0 >= sep2.0 && sep3.0 >= sep1.0 {
70        // To compute the actual distance, we need to compute the closest
71        // points between the two edges that generated the separating axis.
72        let edge1 = cuboid1.local_support_edge_segment(sep3.1);
73        let edge2 = triangle2.local_support_edge_segment(pos21 * -sep3.1);
74        return super::closest_points_segment_segment(pos12, &edge1, &edge2, margin);
75    }
76
77    unreachable!()
78}
79
80/// Closest points between a triangle and a cuboid.
81#[inline]
82pub fn closest_points_triangle_cuboid(
83    pos12: &Isometry<Real>,
84    triangle1: &Triangle,
85    cuboid2: &Cuboid,
86    margin: Real,
87) -> ClosestPoints {
88    closest_points_cuboid_triangle(&pos12.inverse(), cuboid2, triangle1, margin).flipped()
89}
parry3d/query/closest_points/closest_points_cuboid_triangle.rs

parry3d/query/closest_points/
closest_points_cuboid_triangle.rs
