parry3d/query/sat/
sat_cuboid_triangle.rs

1#[cfg(feature = "dim3")]
2use crate::approx::AbsDiffEq;
3use crate::math::{Isometry, Real, Vector};
4#[cfg(feature = "dim3")]
5use crate::query::sat;
6#[cfg(feature = "dim2")]
7use crate::query::sat::support_map_support_map_compute_separation;
8use crate::shape::{Cuboid, SupportMap, Triangle};
9
10/// Finds the best separating edge between a cuboid and a triangle.
11///
12/// All combinations of edges from the cuboid and the triangle are taken into
13/// account.
14#[cfg(feature = "dim3")]
15#[inline(always)]
16pub fn cuboid_triangle_find_local_separating_edge_twoway(
17    cube1: &Cuboid,
18    triangle2: &Triangle,
19    pos12: &Isometry<Real>,
20) -> (Real, Vector<Real>) {
21    // NOTE: everything in this method will be expressed
22    // in the local-space of the first triangle. So we
23    // don't bother adding 2_1 suffixes (e.g. `a2_1`) to everything in
24    // order to keep the code more readable.
25    let a = pos12 * triangle2.a;
26    let b = pos12 * triangle2.b;
27    let c = pos12 * triangle2.c;
28
29    let ab = b - a;
30    let bc = c - b;
31    let ca = a - c;
32
33    // We have 3 * 3 = 9 axes to test.
34    let axes = [
35        // Vector::{x, y ,z}().cross(ab)
36        Vector::new(0.0, -ab.z, ab.y),
37        Vector::new(ab.z, 0.0, -ab.x),
38        Vector::new(-ab.y, ab.x, 0.0),
39        // Vector::{x, y ,z}().cross(bc)
40        Vector::new(0.0, -bc.z, bc.y),
41        Vector::new(bc.z, 0.0, -bc.x),
42        Vector::new(-bc.y, bc.x, 0.0),
43        // Vector::{x, y ,z}().cross(ca)
44        Vector::new(0.0, -ca.z, ca.y),
45        Vector::new(ca.z, 0.0, -ca.x),
46        Vector::new(-ca.y, ca.x, 0.0),
47    ];
48
49    let tri_dots = [
50        (axes[0].dot(&a.coords), axes[0].dot(&c.coords)),
51        (axes[1].dot(&a.coords), axes[1].dot(&c.coords)),
52        (axes[2].dot(&a.coords), axes[2].dot(&c.coords)),
53        (axes[3].dot(&a.coords), axes[3].dot(&c.coords)),
54        (axes[4].dot(&a.coords), axes[4].dot(&c.coords)),
55        (axes[5].dot(&a.coords), axes[5].dot(&c.coords)),
56        (axes[6].dot(&a.coords), axes[6].dot(&b.coords)),
57        (axes[7].dot(&a.coords), axes[7].dot(&b.coords)),
58        (axes[8].dot(&a.coords), axes[8].dot(&b.coords)),
59    ];
60
61    let mut best_sep = -Real::MAX;
62    let mut best_axis = axes[0];
63
64    for (i, axis) in axes.iter().enumerate() {
65        let axis_norm_squared = axis.norm_squared();
66
67        if axis_norm_squared > Real::default_epsilon() {
68            let axis_norm = na::ComplexField::sqrt(axis_norm_squared);
69
70            // NOTE: for both axis and -axis, the dot1 will have the same
71            // value because of the cuboid's symmetry.
72            let local_pt1 = cube1.local_support_point(axis);
73            let dot1 = local_pt1.coords.dot(axis) / axis_norm;
74
75            let (dot2_min, dot2_max) = crate::utils::sort2(tri_dots[i].0, tri_dots[i].1);
76
77            let separation_a = dot2_min / axis_norm - dot1; // separation on axis
78            let separation_b = -dot2_max / axis_norm - dot1; // separation on -axis
79
80            if separation_a > best_sep {
81                best_sep = separation_a;
82                best_axis = *axis / axis_norm;
83            }
84
85            if separation_b > best_sep {
86                best_sep = separation_b;
87                best_axis = -*axis / axis_norm;
88            }
89        }
90    }
91
92    (best_sep, best_axis)
93}
94
95/// Finds the best separating normal between a triangle and a convex shape implementing the `SupportMap` trait.
96///
97/// Only the normals of `triangle1` are tested.
98#[cfg(feature = "dim2")]
99pub fn triangle_support_map_find_local_separating_normal_oneway(
100    triangle1: &Triangle,
101    shape2: &impl SupportMap,
102    pos12: &Isometry<Real>,
103) -> (Real, Vector<Real>) {
104    let mut best_sep = -Real::MAX;
105    let mut best_normal = Vector::zeros();
106
107    for edge in &triangle1.edges() {
108        if let Some(normal) = edge.normal() {
109            let sep = support_map_support_map_compute_separation(triangle1, shape2, pos12, &normal);
110
111            if sep > best_sep {
112                best_sep = sep;
113                best_normal = *normal;
114            }
115        }
116    }
117
118    (best_sep, best_normal)
119}
120
121/// Finds the best separating normal between a triangle and a cuboid.
122///
123/// Only the normals of `triangle1` are tested.
124#[cfg(feature = "dim2")]
125pub fn triangle_cuboid_find_local_separating_normal_oneway(
126    triangle1: &Triangle,
127    shape2: &Cuboid,
128    pos12: &Isometry<Real>,
129) -> (Real, Vector<Real>) {
130    triangle_support_map_find_local_separating_normal_oneway(triangle1, shape2, pos12)
131}
132
133/// Finds the best separating normal a triangle and a cuboid.
134///
135/// Only the normals of `triangle1` are tested.
136#[cfg(feature = "dim3")]
137#[inline(always)]
138pub fn triangle_cuboid_find_local_separating_normal_oneway(
139    triangle1: &Triangle,
140    shape2: &Cuboid,
141    pos12: &Isometry<Real>,
142) -> (Real, Vector<Real>) {
143    sat::point_cuboid_find_local_separating_normal_oneway(
144        triangle1.a,
145        triangle1.normal(),
146        shape2,
147        pos12,
148    )
149}
parry3d/query/sat/sat_cuboid_triangle.rs

parry3d/query/sat/
sat_cuboid_triangle.rs
