parry3d/query/point/
point_aabb.rs

1use crate::bounding_volume::Aabb;
2use crate::math::{Real, Vector, DIM};
3use crate::num::{Bounded, Zero};
4use crate::query::{PointProjection, PointQuery};
5use crate::shape::FeatureId;
6
7impl Aabb {
8    fn do_project_local_point(&self, pt: Vector, solid: bool) -> (bool, Vector, Vector) {
9        let mins_pt = self.mins - pt;
10        let pt_maxs = pt - self.maxs;
11        let shift = mins_pt.max(Vector::ZERO) - pt_maxs.max(Vector::ZERO);
12
13        let inside = shift == Vector::ZERO;
14
15        if !inside {
16            (false, pt + shift, shift)
17        } else if solid {
18            (true, pt, shift)
19        } else {
20            let _max: Real = Bounded::max_value();
21            let mut best = -_max;
22            let mut is_mins = false;
23            let mut best_id = 0;
24
25            for i in 0..DIM {
26                let mins_pt_i = mins_pt[i];
27                let pt_maxs_i = pt_maxs[i];
28
29                if mins_pt_i < pt_maxs_i {
30                    if pt_maxs[i] > best {
31                        best_id = i;
32                        is_mins = false;
33                        best = pt_maxs_i
34                    }
35                } else if mins_pt_i > best {
36                    best_id = i;
37                    is_mins = true;
38                    best = mins_pt_i
39                }
40            }
41
42            let mut shift: Vector = Vector::ZERO;
43
44            if is_mins {
45                shift[best_id] = best;
46            } else {
47                shift[best_id] = -best;
48            }
49
50            (inside, pt + shift, shift)
51        }
52    }
53}
54
55impl PointQuery for Aabb {
56    #[inline]
57    fn project_local_point(&self, pt: Vector, solid: bool) -> PointProjection {
58        let (inside, ls_pt, _) = self.do_project_local_point(pt, solid);
59        PointProjection::new(inside, ls_pt)
60    }
61
62    #[allow(unused_assignments)] // For last_zero_shift which is used only in 3D.
63    #[allow(unused_variables)] // For last_zero_shift which is used only in 3D.
64    #[inline]
65    fn project_local_point_and_get_feature(&self, pt: Vector) -> (PointProjection, FeatureId) {
66        let (inside, ls_pt, shift) = self.do_project_local_point(pt, false);
67        let proj = PointProjection::new(inside, ls_pt);
68        let mut nzero_shifts = 0;
69        let mut last_zero_shift = 0;
70        let mut last_not_zero_shift = 0;
71
72        for i in 0..DIM {
73            if shift[i].is_zero() {
74                nzero_shifts += 1;
75                last_zero_shift = i;
76            } else {
77                last_not_zero_shift = i;
78            }
79        }
80
81        if nzero_shifts == DIM {
82            for i in 0..DIM {
83                if ls_pt[i] > self.maxs[i] - crate::math::DEFAULT_EPSILON {
84                    return (proj, FeatureId::Face(i as u32));
85                }
86                if ls_pt[i] <= self.mins[i] + crate::math::DEFAULT_EPSILON {
87                    return (proj, FeatureId::Face((i + DIM) as u32));
88                }
89            }
90
91            (proj, FeatureId::Unknown)
92        } else if nzero_shifts == DIM - 1 {
93            // On a 3D face.
94            if ls_pt[last_not_zero_shift] < self.center()[last_not_zero_shift] {
95                (proj, FeatureId::Face((last_not_zero_shift + DIM) as u32))
96            } else {
97                (proj, FeatureId::Face(last_not_zero_shift as u32))
98            }
99        } else {
100            // On a vertex or edge.
101            let mut id = 0;
102            let center = self.center();
103
104            for i in 0..DIM {
105                if ls_pt[i] < center[i] {
106                    id |= 1 << i;
107                }
108            }
109
110            #[cfg(feature = "dim3")]
111            {
112                if nzero_shifts == 0 {
113                    (proj, FeatureId::Vertex(id))
114                } else {
115                    (proj, FeatureId::Edge((id << 2) | (last_zero_shift as u32)))
116                }
117            }
118
119            #[cfg(feature = "dim2")]
120            {
121                (proj, FeatureId::Vertex(id))
122            }
123        }
124    }
125
126    #[inline]
127    fn distance_to_local_point(&self, pt: Vector, solid: bool) -> Real {
128        let mins_pt = self.mins - pt;
129        let pt_maxs = pt - self.maxs;
130        let shift = mins_pt.max(pt_maxs).max(Vector::ZERO);
131
132        if solid || shift != Vector::ZERO {
133            shift.length()
134        } else {
135            // TODO: optimize that.
136            -pt.distance(self.project_local_point(pt, solid).point)
137        }
138    }
139}
parry3d/query/point/point_aabb.rs

parry3d/query/point/
point_aabb.rs
