box3d_rust/compound/
cast.rs

1//! Compound AABB, overlap, ray cast, and shape cast.
2//!
3//! SPDX-FileCopyrightText: 2025 Erin Catto
4//! SPDX-License-Identifier: MIT
5
6use super::types::{get_compound_child, ChildGeometry, CompoundData, MAX_COMPOUND_MESH_MATERIALS};
7use crate::constants::MAX_SHAPE_CAST_POINTS;
8use crate::distance::{CastOutput, ShapeProxy, Sweep};
9use crate::dynamic_tree::BoxCastInput;
10use crate::geometry::{
11    overlap_capsule, overlap_sphere, ray_cast_capsule, ray_cast_sphere, shape_cast_capsule,
12    shape_cast_sphere, RayCastInput, ShapeCastInput,
13};
14use crate::hull::{overlap_hull, ray_cast_hull, shape_cast_hull};
15use crate::math_functions::{
16    aabb_transform, add, inv_rotate_vector, inv_transform_point, invert_transform, make_aabb,
17    make_matrix_from_quat, max, min, min_int, mul_mv, mul_transforms, rotate_vector, sub,
18    transform_point, Aabb, Transform, Vec3, VEC3_ZERO,
19};
20use crate::mesh::{overlap_mesh, ray_cast_mesh, shape_cast_mesh};
21
22/// Compute the AABB of a compound. (b3ComputeCompoundAABB)
23pub fn compute_compound_aabb(shape: &CompoundData, transform: Transform) -> Aabb {
24    debug_assert!(shape.node_offset > 0);
25    let aabb = shape.tree.root_bounds();
26    aabb_transform(transform, aabb)
27}
28
29/// Test overlap between a compound and a shape proxy. (b3OverlapCompound)
30pub fn overlap_compound(
31    shape: &CompoundData,
32    shape_transform: Transform,
33    proxy: &ShapeProxy,
34) -> bool {
35    let mut overlap = false;
36
37    let mut aabb = Aabb {
38        lower_bound: proxy.points[0],
39        upper_bound: proxy.points[0],
40    };
41    for i in 1..proxy.count as usize {
42        aabb.lower_bound = min(aabb.lower_bound, proxy.points[i]);
43        aabb.upper_bound = max(aabb.upper_bound, proxy.points[i]);
44    }
45
46    let r = Vec3 {
47        x: proxy.radius,
48        y: proxy.radius,
49        z: proxy.radius,
50    };
51    aabb.lower_bound = sub(aabb.lower_bound, r);
52    aabb.upper_bound = add(aabb.upper_bound, r);
53
54    shape
55        .tree
56        .query(aabb, !0u64, false, |_proxy_id, user_data| {
57            let child_index = user_data as i32;
58            let child = get_compound_child(shape, child_index);
59            let transform = mul_transforms(shape_transform, child.transform);
60
61            let child_overlap = match child.geometry {
62                ChildGeometry::Capsule(ref capsule) => overlap_capsule(capsule, transform, proxy),
63                ChildGeometry::Hull(hull) => overlap_hull(hull, transform, proxy),
64                ChildGeometry::Mesh(ref mesh) => overlap_mesh(mesh, transform, proxy),
65                ChildGeometry::Sphere(ref sphere) => overlap_sphere(sphere, transform, proxy),
66            };
67
68            if child_overlap {
69                overlap = true;
70                false
71            } else {
72                true
73            }
74        });
75
76    overlap
77}
78
79/// Ray cast versus a compound. (b3RayCastCompound)
80pub fn ray_cast_compound(shape: &CompoundData, input: &RayCastInput) -> CastOutput {
81    let mut result = CastOutput::default();
82
83    shape
84        .tree
85        .ray_cast(input, !0u64, false, |ray_input, _proxy_id, user_data| {
86            let child_index = user_data as i32;
87            let child = get_compound_child(shape, child_index);
88
89            let local_input = RayCastInput {
90                origin: inv_transform_point(child.transform, ray_input.origin),
91                translation: inv_rotate_vector(child.transform.q, ray_input.translation),
92                max_fraction: ray_input.max_fraction,
93            };
94
95            let mut output = match child.geometry {
96                ChildGeometry::Capsule(ref capsule) => {
97                    let mut o = ray_cast_capsule(capsule, &local_input);
98                    o.material_index = child.material_indices[0];
99                    o
100                }
101                ChildGeometry::Hull(hull) => {
102                    let mut o = ray_cast_hull(hull, &local_input);
103                    o.material_index = child.material_indices[0];
104                    o
105                }
106                ChildGeometry::Mesh(ref mesh) => {
107                    let mut o = ray_cast_mesh(mesh, &local_input);
108                    debug_assert!(0 <= o.material_index);
109                    let child_material_index =
110                        min_int(o.material_index, MAX_COMPOUND_MESH_MATERIALS as i32 - 1);
111                    o.material_index = child.material_indices[child_material_index as usize];
112                    o
113                }
114                ChildGeometry::Sphere(ref sphere) => {
115                    let mut o = ray_cast_sphere(sphere, &local_input);
116                    o.material_index = child.material_indices[0];
117                    o
118                }
119            };
120
121            if output.hit {
122                output.point = transform_point(child.transform, output.point);
123                output.normal = rotate_vector(child.transform.q, output.normal);
124                output.child_index = child_index;
125                result = output;
126                return output.fraction;
127            }
128
129            ray_input.max_fraction
130        });
131
132    result
133}
134
135/// Shape cast versus a compound. (b3ShapeCastCompound)
136pub fn shape_cast_compound(shape: &CompoundData, input: &ShapeCastInput) -> CastOutput {
137    let mut result = CastOutput::default();
138
139    if input.proxy.count == 0 {
140        return result;
141    }
142
143    let box_ = make_aabb(
144        &input.proxy.points[..input.proxy.count as usize],
145        input.proxy.radius,
146    );
147    let tree_input = BoxCastInput {
148        box_,
149        translation: input.translation,
150        max_fraction: input.max_fraction,
151    };
152
153    shape.tree.box_cast(
154        &tree_input,
155        !0u64,
156        false,
157        |box_input, _proxy_id, user_data| {
158            let child_index = user_data as i32;
159            let child = get_compound_child(shape, child_index);
160
161            let mut local_input = *input;
162            local_input.max_fraction = box_input.max_fraction;
163
164            let mut local_points = [Vec3::default(); MAX_SHAPE_CAST_POINTS];
165            local_input.proxy.count = min_int(input.proxy.count, MAX_SHAPE_CAST_POINTS as i32);
166
167            let inv_transform = invert_transform(child.transform);
168            let r = make_matrix_from_quat(inv_transform.q);
169
170            for i in 0..local_input.proxy.count as usize {
171                local_points[i] = add(mul_mv(r, input.proxy.points[i]), inv_transform.p);
172            }
173            local_input.proxy.points = local_points;
174            local_input.translation = mul_mv(r, input.translation);
175
176            let mut output = match child.geometry {
177                ChildGeometry::Capsule(ref capsule) => {
178                    let mut o = shape_cast_capsule(capsule, &local_input);
179                    o.material_index = child.material_indices[0];
180                    o
181                }
182                ChildGeometry::Hull(hull) => {
183                    let mut o = shape_cast_hull(hull, &local_input);
184                    o.material_index = child.material_indices[0];
185                    o
186                }
187                ChildGeometry::Mesh(ref mesh) => {
188                    let mut o = shape_cast_mesh(mesh, &local_input);
189                    debug_assert!(0 <= o.material_index);
190                    let child_material_index =
191                        min_int(o.material_index, MAX_COMPOUND_MESH_MATERIALS as i32 - 1);
192                    o.material_index = child.material_indices[child_material_index as usize];
193                    o
194                }
195                ChildGeometry::Sphere(ref sphere) => {
196                    let mut o = shape_cast_sphere(sphere, &local_input);
197                    o.material_index = child.material_indices[0];
198                    o
199                }
200            };
201
202            if output.hit {
203                output.point = transform_point(child.transform, output.point);
204                output.normal = rotate_vector(child.transform.q, output.normal);
205                output.child_index = child_index;
206                result = output;
207                return output.fraction;
208            }
209
210            box_input.max_fraction
211        },
212    );
213
214    result
215}
216
217/// Static compound-child sweep for TOI (child fixed in compound frame).
218/// `xf = compoundTransform * childTransform`. (b3MakeCompoundChildSweep)
219pub fn make_compound_child_sweep(
220    compound_transform: Transform,
221    child_transform: Transform,
222) -> Sweep {
223    let xf = mul_transforms(compound_transform, child_transform);
224    Sweep {
225        local_center: VEC3_ZERO,
226        c1: xf.p,
227        c2: xf.p,
228        q1: xf.q,
229        q2: xf.q,
230    }
231}
box3d_rust/compound/cast.rs

box3d_rust/compound/
cast.rs
