parry3d/query/sat/
sat_triangle_segment.rs

1use crate::math::{Isometry, Real, Vector};
2use crate::query::sat;
3use crate::shape::{Segment, SupportMap, Triangle};
4use na::Unit;
5
6/// Finds the best separating normal a triangle and a segment.
7///
8/// Only the normals of `triangle1` are tested.
9pub fn triangle_segment_find_local_separating_normal_oneway(
10    triangle1: &Triangle,
11    segment2: &Segment,
12    pos12: &Isometry<Real>,
13) -> (Real, Vector<Real>) {
14    if let Some(dir) = triangle1.normal() {
15        let p2a = segment2.support_point_toward(pos12, &-dir);
16        let p2b = segment2.support_point_toward(pos12, &dir);
17        let sep_a = (p2a - triangle1.a).dot(&dir);
18        let sep_b = -(p2b - triangle1.a).dot(&dir);
19
20        if sep_a >= sep_b {
21            (sep_a, *dir)
22        } else {
23            (sep_b, -*dir)
24        }
25    } else {
26        (-Real::MAX, Vector::zeros())
27    }
28}
29
30/// Finds the best separating edge between a segment and a triangle.
31///
32/// All combinations of edges from the segment and the triangle are taken into
33/// account.
34pub fn segment_triangle_find_local_separating_edge_twoway(
35    segment1: &Segment,
36    triangle2: &Triangle,
37    pos12: &Isometry<Real>,
38) -> (Real, Vector<Real>) {
39    let x2 = pos12 * (triangle2.b - triangle2.a);
40    let y2 = pos12 * (triangle2.c - triangle2.b);
41    let z2 = pos12 * (triangle2.a - triangle2.c);
42    let dir1 = segment1.scaled_direction();
43
44    let crosses1 = [dir1.cross(&x2), dir1.cross(&y2), dir1.cross(&z2)];
45    let axes1 = [
46        crosses1[0],
47        crosses1[1],
48        crosses1[2],
49        -crosses1[0],
50        -crosses1[1],
51        -crosses1[2],
52    ];
53    let mut max_separation = -Real::MAX;
54    let mut sep_dir = axes1[0];
55
56    for axis1 in &axes1 {
57        if let Some(axis1) = Unit::try_new(*axis1, 0.0) {
58            let sep =
59                sat::support_map_support_map_compute_separation(segment1, triangle2, pos12, &axis1);
60
61            if sep > max_separation {
62                max_separation = sep;
63                sep_dir = *axis1;
64            }
65        }
66    }
67
68    (max_separation, sep_dir)
69}
parry3d/query/sat/sat_triangle_segment.rs

parry3d/query/sat/
sat_triangle_segment.rs
