box3d_rust/mesh/
query.rs

1//! Mesh overlap, query, mover collide, and triangle accessor.
2//!
3//! SPDX-FileCopyrightText: 2026 Erin Catto
4//! SPDX-License-Identifier: MIT
5
6use super::types::{
7    Mesh, CONCAVE_EDGE1, CONCAVE_EDGE2, CONCAVE_EDGE3, INVERSE_CONCAVE_EDGE1,
8    INVERSE_CONCAVE_EDGE2, INVERSE_CONCAVE_EDGE3, MESH_STACK_SIZE,
9};
10use crate::constants::linear_slop;
11use crate::distance::{
12    compute_proxy_aabb, make_local_proxy, make_proxy, shape_distance, DistanceInput, ShapeProxy,
13    SimplexCache,
14};
15use crate::geometry::{Capsule, PlaneResult};
16use crate::math_functions::{
17    add, max, min, mul, mul_sv, sub, test_bounds_overlap, test_bounds_triangle_overlap, Aabb,
18    Plane, Transform, Triangle, Vec3, TRANSFORM_IDENTITY,
19};
20
21/// Test overlap between a mesh and a shape proxy. (b3OverlapMesh)
22pub fn overlap_mesh(shape: &Mesh<'_>, shape_transform: Transform, proxy: &ShapeProxy) -> bool {
23    debug_assert!(proxy.count > 0);
24    let mut cache = SimplexCache::default();
25
26    let local_proxy = make_local_proxy(proxy, shape_transform);
27    let aabb = compute_proxy_aabb(&local_proxy);
28
29    let mesh_scale = shape.scale;
30    let inv_scale = Vec3 {
31        x: 1.0 / mesh_scale.x,
32        y: 1.0 / mesh_scale.y,
33        z: 1.0 / mesh_scale.z,
34    };
35    let temp1 = mul(inv_scale, aabb.lower_bound);
36    let temp2 = mul(inv_scale, aabb.upper_bound);
37    let inv_scaled_bounds_min = min(temp1, temp2);
38    let inv_scaled_bounds_max = max(temp1, temp2);
39    let inv_scaled_bounds_center = mul_sv(0.5, add(inv_scaled_bounds_min, inv_scaled_bounds_max));
40    let inv_scaled_bounds_extent = sub(inv_scaled_bounds_max, inv_scaled_bounds_center);
41
42    let mut input = DistanceInput {
43        proxy_a: Default::default(),
44        proxy_b: local_proxy,
45        transform: TRANSFORM_IDENTITY,
46        use_radii: true,
47    };
48
49    let mut stack = [0i32; MESH_STACK_SIZE];
50    let mut count = 0usize;
51    let mut node_index = 0usize;
52
53    let data = shape.data;
54    let triangles = &data.triangles;
55    let vertices = &data.vertices;
56
57    loop {
58        let node = &data.nodes[node_index];
59        if test_bounds_overlap(
60            node.lower_bound,
61            node.upper_bound,
62            inv_scaled_bounds_min,
63            inv_scaled_bounds_max,
64        ) {
65            if node.is_leaf() {
66                let triangle_count = node.triangle_count() as i32;
67                let triangle_offset = node.triangle_offset as i32;
68
69                for index in 0..triangle_count {
70                    let triangle_index = triangle_offset + index;
71                    let triangle = triangles[triangle_index as usize];
72
73                    let vertex1 = vertices[triangle.index1 as usize];
74                    let vertex2 = vertices[triangle.index2 as usize];
75                    let vertex3 = vertices[triangle.index3 as usize];
76
77                    if test_bounds_triangle_overlap(
78                        inv_scaled_bounds_center,
79                        inv_scaled_bounds_extent,
80                        vertex1,
81                        vertex2,
82                        vertex3,
83                    ) {
84                        let triangle_vertices = [
85                            mul(mesh_scale, vertex1),
86                            mul(mesh_scale, vertex2),
87                            mul(mesh_scale, vertex3),
88                        ];
89                        input.proxy_a = make_proxy(&triangle_vertices, 0.0);
90                        cache.count = 0;
91
92                        let output = shape_distance(&input, &mut cache, None);
93                        let tolerance = 0.1 * linear_slop();
94                        if output.distance < tolerance {
95                            return true;
96                        }
97                    }
98                }
99            } else {
100                debug_assert!(count <= MESH_STACK_SIZE - 1);
101                stack[count] = node_index as i32 + node.child_offset() as i32;
102                count += 1;
103                node_index += 1;
104                continue;
105            }
106        }
107
108        if count == 0 {
109            break;
110        }
111        count -= 1;
112        node_index = stack[count] as usize;
113    }
114
115    false
116}
117
118/// Get a scaled mesh triangle with edge flags. (b3GetMeshTriangle)
119pub fn get_mesh_triangle(mesh: &Mesh<'_>, triangle_index: i32) -> Triangle {
120    debug_assert!(0 <= triangle_index && triangle_index < mesh.data.triangle_count);
121
122    let triangles = &mesh.data.triangles;
123    let flags = &mesh.data.flags;
124    let vertices = &mesh.data.vertices;
125
126    let triangle = triangles[triangle_index as usize];
127    let triangle_flags = flags[triangle_index as usize];
128    let scale = mesh.scale;
129
130    let mut result = Triangle {
131        vertices: [Default::default(); 3],
132        i1: triangle.index1,
133        i2: 0,
134        i3: 0,
135        flags: 0,
136    };
137    result.vertices[0] = mul(scale, vertices[triangle.index1 as usize]);
138
139    if scale.x * scale.y * scale.z < 0.0 {
140        result.vertices[1] = mul(scale, vertices[triangle.index3 as usize]);
141        result.vertices[2] = mul(scale, vertices[triangle.index2 as usize]);
142        result.i2 = triangle.index3;
143        result.i3 = triangle.index2;
144
145        result.flags = 0;
146        if (triangle_flags as i32) & INVERSE_CONCAVE_EDGE1 != 0 {
147            result.flags |= CONCAVE_EDGE1;
148        }
149        if (triangle_flags as i32) & INVERSE_CONCAVE_EDGE2 != 0 {
150            result.flags |= CONCAVE_EDGE2;
151        }
152        if (triangle_flags as i32) & INVERSE_CONCAVE_EDGE3 != 0 {
153            result.flags |= CONCAVE_EDGE3;
154        }
155    } else {
156        result.vertices[1] = mul(scale, vertices[triangle.index2 as usize]);
157        result.vertices[2] = mul(scale, vertices[triangle.index3 as usize]);
158        result.i2 = triangle.index2;
159        result.i3 = triangle.index3;
160        result.flags = triangle_flags as i32;
161    }
162
163    result
164}
165
166/// Collide a capsule mover against a mesh, writing contact planes.
167/// (b3CollideMoverAndMesh)
168pub fn collide_mover_and_mesh(
169    planes: &mut [PlaneResult],
170    shape: &Mesh<'_>,
171    mover: &Capsule,
172) -> i32 {
173    let capacity = planes.len() as i32;
174    if capacity == 0 {
175        return 0;
176    }
177
178    let mut distance_input = DistanceInput {
179        proxy_a: Default::default(),
180        proxy_b: make_proxy(&[mover.center1, mover.center2], 0.0),
181        transform: TRANSFORM_IDENTITY,
182        use_radii: false,
183    };
184
185    let mut cache = SimplexCache::default();
186    let radius = mover.radius;
187
188    let r = Vec3 {
189        x: radius,
190        y: radius,
191        z: radius,
192    };
193    let bounds_min = sub(min(mover.center1, mover.center2), r);
194    let bounds_max = add(max(mover.center1, mover.center2), r);
195
196    let mesh_scale = shape.scale;
197    let inv_scale = Vec3 {
198        x: 1.0 / mesh_scale.x,
199        y: 1.0 / mesh_scale.y,
200        z: 1.0 / mesh_scale.z,
201    };
202    let temp1 = mul(inv_scale, bounds_min);
203    let temp2 = mul(inv_scale, bounds_max);
204    let inv_scaled_bounds_min = min(temp1, temp2);
205    let inv_scaled_bounds_max = max(temp1, temp2);
206    let inv_scaled_bounds_center = mul_sv(0.5, add(inv_scaled_bounds_min, inv_scaled_bounds_max));
207    let inv_scaled_bounds_extent = sub(inv_scaled_bounds_max, inv_scaled_bounds_center);
208
209    let mut stack = [0i32; MESH_STACK_SIZE];
210    let mut count = 0usize;
211    let mut node_index = 0usize;
212
213    let data = shape.data;
214    let triangles = &data.triangles;
215    let vertices = &data.vertices;
216
217    let mut plane_count = 0i32;
218    while plane_count < capacity {
219        let node = &data.nodes[node_index];
220        if test_bounds_overlap(
221            node.lower_bound,
222            node.upper_bound,
223            inv_scaled_bounds_min,
224            inv_scaled_bounds_max,
225        ) {
226            if node.is_leaf() {
227                let triangle_count = node.triangle_count() as i32;
228                let triangle_offset = node.triangle_offset as i32;
229
230                for index in 0..triangle_count {
231                    let triangle_index = triangle_offset + index;
232                    let triangle = triangles[triangle_index as usize];
233
234                    let vertex1 = vertices[triangle.index1 as usize];
235                    let vertex2 = vertices[triangle.index2 as usize];
236                    let vertex3 = vertices[triangle.index3 as usize];
237
238                    if test_bounds_triangle_overlap(
239                        inv_scaled_bounds_center,
240                        inv_scaled_bounds_extent,
241                        vertex1,
242                        vertex2,
243                        vertex3,
244                    ) {
245                        let triangle_vertices = [
246                            mul(mesh_scale, vertex1),
247                            mul(mesh_scale, vertex2),
248                            mul(mesh_scale, vertex3),
249                        ];
250                        distance_input.proxy_a = make_proxy(&triangle_vertices, 0.0);
251                        cache.count = 0;
252
253                        let distance_output = shape_distance(&distance_input, &mut cache, None);
254
255                        if distance_output.distance == 0.0 {
256                            // todo SAT
257                        } else if distance_output.distance <= mover.radius {
258                            let plane = Plane {
259                                normal: distance_output.normal,
260                                offset: mover.radius - distance_output.distance,
261                            };
262                            planes[plane_count as usize] = PlaneResult {
263                                plane,
264                                point: distance_output.point_a,
265                            };
266                            plane_count += 1;
267                            if plane_count == capacity {
268                                return plane_count;
269                            }
270                        }
271                    }
272                }
273            } else {
274                debug_assert!(count <= MESH_STACK_SIZE - 1);
275                stack[count] = node_index as i32 + node.child_offset() as i32;
276                count += 1;
277                node_index += 1;
278                continue;
279            }
280        }
281
282        if count == 0 {
283            break;
284        }
285        count -= 1;
286        node_index = stack[count] as usize;
287    }
288
289    plane_count
290}
291
292/// Query mesh triangles overlapping an AABB.
293/// Callback receives (a, b, c, triangle_index) and may return `false` to stop early.
294/// (b3QueryMesh)
295pub fn query_mesh<F>(mesh: &Mesh<'_>, bounds: Aabb, mut fcn: F)
296where
297    F: FnMut(Vec3, Vec3, Vec3, i32) -> bool,
298{
299    let mesh_scale = mesh.scale;
300    // C names this `clockwise` but the predicate is inverted vs ray cast.
301    let clockwise = mesh_scale.x * mesh_scale.y * mesh_scale.z > 0.0;
302
303    let inv_scale = Vec3 {
304        x: 1.0 / mesh_scale.x,
305        y: 1.0 / mesh_scale.y,
306        z: 1.0 / mesh_scale.z,
307    };
308    let temp1 = mul(inv_scale, bounds.lower_bound);
309    let temp2 = mul(inv_scale, bounds.upper_bound);
310    let inv_scaled_bounds_min = min(temp1, temp2);
311    let inv_scaled_bounds_max = max(temp1, temp2);
312    let inv_scaled_bounds_center = mul_sv(0.5, add(inv_scaled_bounds_min, inv_scaled_bounds_max));
313    let inv_scaled_bounds_extent = sub(inv_scaled_bounds_max, inv_scaled_bounds_center);
314
315    let data = mesh.data;
316    let mut stack = [0i32; MESH_STACK_SIZE];
317    let mut count = 0usize;
318    let mut node_index = 0usize;
319
320    let triangles = &data.triangles;
321    let vertices = &data.vertices;
322
323    loop {
324        let node = &data.nodes[node_index];
325        if test_bounds_overlap(
326            node.lower_bound,
327            node.upper_bound,
328            inv_scaled_bounds_min,
329            inv_scaled_bounds_max,
330        ) {
331            if node.is_leaf() {
332                let triangle_count = node.triangle_count() as i32;
333                let triangle_offset = node.triangle_offset as i32;
334
335                for index in 0..triangle_count {
336                    let triangle_index = triangle_offset + index;
337                    let triangle = triangles[triangle_index as usize];
338
339                    let vertex1 = vertices[triangle.index1 as usize];
340                    let vertex2 = vertices[triangle.index2 as usize];
341                    let vertex3 = vertices[triangle.index3 as usize];
342
343                    if test_bounds_triangle_overlap(
344                        inv_scaled_bounds_center,
345                        inv_scaled_bounds_extent,
346                        vertex1,
347                        vertex2,
348                        vertex3,
349                    ) {
350                        let a = mul(mesh_scale, vertex1);
351                        let (b, c) = if clockwise {
352                            (mul(mesh_scale, vertex2), mul(mesh_scale, vertex3))
353                        } else {
354                            (mul(mesh_scale, vertex3), mul(mesh_scale, vertex2))
355                        };
356
357                        if !fcn(a, b, c, triangle_index) {
358                            return;
359                        }
360                    }
361                }
362            } else {
363                debug_assert!(count <= MESH_STACK_SIZE - 1);
364                stack[count] = node_index as i32 + node.child_offset() as i32;
365                count += 1;
366                node_index += 1;
367                continue;
368            }
369        }
370
371        if count == 0 {
372            break;
373        }
374        count -= 1;
375        node_index = stack[count] as usize;
376    }
377}
box3d_rust/mesh/query.rs

box3d_rust/mesh/
query.rs
