box3d_rust/math_functions/
query.rs

1// AABB helpers and segment/line/point distance queries.
2// Part of the math_functions module.
3
4use super::*;
5
6/// Get the AABB of a point cloud.
7pub fn make_aabb(points: &[Vec3], radius: f32) -> Aabb {
8    debug_assert!(!points.is_empty());
9    let mut a = Aabb {
10        lower_bound: points[0],
11        upper_bound: points[0],
12    };
13    for point in &points[1..] {
14        a.lower_bound = min(a.lower_bound, *point);
15        a.upper_bound = max(a.upper_bound, *point);
16    }
17
18    let r = Vec3 {
19        x: radius,
20        y: radius,
21        z: radius,
22    };
23    a.lower_bound = sub(a.lower_bound, r);
24    a.upper_bound = add(a.upper_bound, r);
25
26    a
27}
28
29/// Does a fully contain b?
30pub fn aabb_contains(a: Aabb, b: Aabb) -> bool {
31    if a.lower_bound.x > b.lower_bound.x || b.upper_bound.x > a.upper_bound.x {
32        return false;
33    }
34    if a.lower_bound.y > b.lower_bound.y || b.upper_bound.y > a.upper_bound.y {
35        return false;
36    }
37    if a.lower_bound.z > b.lower_bound.z || b.upper_bound.z > a.upper_bound.z {
38        return false;
39    }
40
41    true
42}
43
44/// Get the surface area of an axis-aligned bounding box.
45pub fn aabb_area(a: Aabb) -> f32 {
46    let delta = sub(a.upper_bound, a.lower_bound);
47    2.0 * (delta.x * delta.y + delta.y * delta.z + delta.z * delta.x)
48}
49
50/// Get the center of an axis-aligned bounding box.
51pub fn aabb_center(a: Aabb) -> Vec3 {
52    mul_sv(0.5, add(a.upper_bound, a.lower_bound))
53}
54
55/// Get the extents (half-widths) of an axis-aligned bounding box.
56pub fn aabb_extents(a: Aabb) -> Vec3 {
57    mul_sv(0.5, sub(a.upper_bound, a.lower_bound))
58}
59
60/// Get the union of two axis-aligned bounding boxes.
61pub fn aabb_union(a: Aabb, b: Aabb) -> Aabb {
62    Aabb {
63        lower_bound: min(a.lower_bound, b.lower_bound),
64        upper_bound: max(a.upper_bound, b.upper_bound),
65    }
66}
67
68/// Add a point to an AABB. (math_internal.h: b3AABB_AddPoint)
69pub fn aabb_add_point(a: Aabb, point: Vec3) -> Aabb {
70    Aabb {
71        lower_bound: min(a.lower_bound, point),
72        upper_bound: max(a.upper_bound, point),
73    }
74}
75
76/// Add uniform padding to an axis-aligned bounding box.
77pub fn aabb_inflate(a: Aabb, extension: f32) -> Aabb {
78    let radius = Vec3 {
79        x: extension,
80        y: extension,
81        z: extension,
82    };
83
84    Aabb {
85        lower_bound: sub(a.lower_bound, radius),
86        upper_bound: add(a.upper_bound, radius),
87    }
88}
89
90/// Do two axis-aligned boxes overlap?
91pub fn aabb_overlaps(a: Aabb, b: Aabb) -> bool {
92    // No intersection if separated along one axis
93    if a.upper_bound.x < b.lower_bound.x || a.lower_bound.x > b.upper_bound.x {
94        return false;
95    }
96    if a.upper_bound.y < b.lower_bound.y || a.lower_bound.y > b.upper_bound.y {
97        return false;
98    }
99    if a.upper_bound.z < b.lower_bound.z || a.lower_bound.z > b.upper_bound.z {
100        return false;
101    }
102
103    // Overlapping on all axis means bounds are intersecting
104    true
105}
106
107/// Transform an axis-aligned bounding box. This can create a larger box
108/// than if you recomputed the AABB of the original shape with the transform
109/// applied.
110pub fn aabb_transform(transform: Transform, a: Aabb) -> Aabb {
111    let center = transform_point(transform, aabb_center(a));
112    let m = make_matrix_from_quat(transform.q);
113    let extent = mul_mv(abs_matrix3(m), aabb_extents(a));
114    Aabb {
115        lower_bound: sub(center, extent),
116        upper_bound: add(center, extent),
117    }
118}
119
120/// Get the closest point on an axis-aligned bounding box.
121pub fn closest_point_to_aabb(point: Vec3, a: Aabb) -> Vec3 {
122    clamp(point, a.lower_bound, a.upper_bound)
123}
124
125/// Compute the closest point on the segment a-b to the target q.
126pub fn point_to_segment_distance(a: Vec3, b: Vec3, q: Vec3) -> Vec3 {
127    let ab = sub(b, a);
128    let aq = sub(q, a);
129
130    let alpha = dot(ab, aq);
131
132    if alpha <= 0.0 {
133        // q projects outside interval [a, b] on the side of a
134        a
135    } else {
136        let denominator = dot(ab, ab);
137        if alpha > denominator {
138            // q projects outside interval [a, b] on the side of b
139            b
140        } else {
141            // q projects inside interval [a, b]
142            let alpha = alpha / denominator;
143            mul_add(a, alpha, ab)
144        }
145    }
146}
147
148/// Compute the closest points on two infinite lines.
149pub fn line_distance(p1: Vec3, d1: Vec3, p2: Vec3, d2: Vec3) -> SegmentDistanceResult {
150    // Solve A*x = b
151    let a11 = dot(d1, d1);
152    let a12 = -dot(d1, d2);
153    let a21 = dot(d2, d1);
154    let a22 = -dot(d2, d2);
155
156    let w = sub(p1, p2);
157    let b1 = -dot(d1, w);
158    let b2 = -dot(d2, w);
159
160    let det = a11 * a22 - a12 * a21;
161    if det * det < 1000.0 * f32::MIN_POSITIVE {
162        // Lines are parallel - project p2 onto line L1: x1 = p1 + s1 * d1
163        let s1 = dot(sub(p2, p1), d1) / dot(d1, d1);
164        let s2 = 0.0;
165
166        return SegmentDistanceResult {
167            point1: mul_add(p1, s1, d1),
168            fraction1: s1,
169            point2: mul_add(p2, s2, d2),
170            fraction2: s2,
171        };
172    }
173
174    let s1 = (a22 * b1 - a12 * b2) / det;
175    let s2 = (a11 * b2 - a21 * b1) / det;
176
177    SegmentDistanceResult {
178        point1: mul_add(p1, s1, d1),
179        fraction1: s1,
180        point2: mul_add(p2, s2, d2),
181        fraction2: s2,
182    }
183}
184
185/// Compute the closest points on two line segments.
186pub fn segment_distance(p1: Vec3, q1: Vec3, p2: Vec3, q2: Vec3) -> SegmentDistanceResult {
187    let d1 = sub(q1, p1);
188    let d2 = sub(q2, p2);
189    let r = sub(p1, p2);
190
191    let a = dot(d1, d1);
192    let b = dot(d1, d2);
193    let c = dot(d1, r);
194    let e = dot(d2, d2);
195    let f = dot(d2, r);
196
197    // Check if one of the segments degenerates into a point
198    if a < 100.0 * f32::EPSILON && e < 100.0 * f32::EPSILON {
199        // Both segments degenerate into points
200        return SegmentDistanceResult {
201            point1: p1,
202            fraction1: 0.0,
203            point2: p2,
204            fraction2: 0.0,
205        };
206    }
207
208    if a < 100.0 * f32::EPSILON {
209        // First segment degenerates into a point
210        let s2 = clamp_float(f / e, 0.0, 1.0);
211
212        return SegmentDistanceResult {
213            point1: p1,
214            fraction1: 0.0,
215            point2: mul_add(p2, s2, d2),
216            fraction2: s2,
217        };
218    }
219
220    if e < 100.0 * f32::EPSILON {
221        // Second segment degenerates into a point
222        let s1 = clamp_float(-c / a, 0.0, 1.0);
223
224        return SegmentDistanceResult {
225            point1: mul_add(p1, s1, d1),
226            fraction1: s1,
227            point2: p2,
228            fraction2: 0.0,
229        };
230    }
231
232    // Non-degenerate case
233    let denom = a * e - b * b;
234    let mut s1 = if denom > 1000.0 * f32::MIN_POSITIVE {
235        clamp_float((b * f - c * e) / denom, 0.0, 1.0)
236    } else {
237        0.0
238    };
239    let mut s2 = (b * s1 + f) / e;
240
241    // Clamp lambda2 and recompute lambda1 if necessary
242    if s2 < 0.0 {
243        s1 = clamp_float(-c / a, 0.0, 1.0);
244        s2 = 0.0;
245    } else if s2 > 1.0 {
246        s1 = clamp_float((b - c) / a, 0.0, 1.0);
247        s2 = 1.0;
248    }
249
250    SegmentDistanceResult {
251        point1: mul_add(p1, s1, d1),
252        fraction1: s1,
253        point2: mul_add(p2, s2, d2),
254        fraction2: s2,
255    }
256}
box3d_rust/math_functions/query.rs

box3d_rust/math_functions/
query.rs
