parry3d/query/point/
point_aabb.rs

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

parry3d/query/point/
point_aabb.rs
