oxiphysics_geometry/compound/
functions.rs

1//! Auto-generated module
2//!
3//! 🤖 Generated with [SplitRS](https://github.com/cool-japan/splitrs)
4
5use super::types::ChildShapeKind;
6
7/// Compute the 3×3 inertia tensor of a sphere about its own center.
8pub fn sphere_inertia(mass: f64, r: f64) -> [[f64; 3]; 3] {
9    let i = 2.0 / 5.0 * mass * r * r;
10    [[i, 0.0, 0.0], [0.0, i, 0.0], [0.0, 0.0, i]]
11}
12/// Compute the 3×3 inertia tensor of an axis-aligned box about its own center.
13///
14/// `hx`, `hy`, `hz` are the half-extents.
15pub fn box_inertia(mass: f64, hx: f64, hy: f64, hz: f64) -> [[f64; 3]; 3] {
16    let i_xx = mass / 3.0 * (hy * hy + hz * hz);
17    let i_yy = mass / 3.0 * (hx * hx + hz * hz);
18    let i_zz = mass / 3.0 * (hx * hx + hy * hy);
19    [[i_xx, 0.0, 0.0], [0.0, i_yy, 0.0], [0.0, 0.0, i_zz]]
20}
21/// Ray-sphere intersection. Returns (toi, normal).
22pub(super) fn ray_sphere(
23    origin: [f64; 3],
24    dir: [f64; 3],
25    radius: f64,
26    max_toi: f64,
27) -> Option<(f64, [f64; 3])> {
28    let a = dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2];
29    let b = 2.0 * (origin[0] * dir[0] + origin[1] * dir[1] + origin[2] * dir[2]);
30    let c = origin[0] * origin[0] + origin[1] * origin[1] + origin[2] * origin[2] - radius * radius;
31    let disc = b * b - 4.0 * a * c;
32    if disc < 0.0 {
33        return None;
34    }
35    let sqrt_disc = disc.sqrt();
36    let t1 = (-b - sqrt_disc) / (2.0 * a);
37    let t2 = (-b + sqrt_disc) / (2.0 * a);
38    let t = if t1 >= 0.0 { t1 } else { t2 };
39    if t < 0.0 || t > max_toi {
40        return None;
41    }
42    let p = [
43        origin[0] + dir[0] * t,
44        origin[1] + dir[1] * t,
45        origin[2] + dir[2] * t,
46    ];
47    let len = (p[0] * p[0] + p[1] * p[1] + p[2] * p[2]).sqrt();
48    let n = if len > 1e-12 {
49        [p[0] / len, p[1] / len, p[2] / len]
50    } else {
51        [0.0, 1.0, 0.0]
52    };
53    Some((t, n))
54}
55/// Ray-box intersection (axis-aligned, centered at origin). Returns (toi, normal).
56pub(super) fn ray_box(
57    origin: [f64; 3],
58    dir: [f64; 3],
59    half_extents: [f64; 3],
60    max_toi: f64,
61) -> Option<(f64, [f64; 3])> {
62    let mut tmin = f64::NEG_INFINITY;
63    let mut tmax = f64::INFINITY;
64    let mut normal = [0.0; 3];
65    for i in 0..3 {
66        if dir[i].abs() < 1e-12 {
67            if origin[i] < -half_extents[i] || origin[i] > half_extents[i] {
68                return None;
69            }
70        } else {
71            let t1 = (-half_extents[i] - origin[i]) / dir[i];
72            let t2 = (half_extents[i] - origin[i]) / dir[i];
73            let (ta, tb, sign) = if t1 < t2 {
74                (t1, t2, -1.0)
75            } else {
76                (t2, t1, 1.0)
77            };
78            if ta > tmin {
79                tmin = ta;
80                normal = [0.0; 3];
81                normal[i] = sign;
82            }
83            tmax = tmax.min(tb);
84            if tmin > tmax {
85                return None;
86            }
87        }
88    }
89    if tmin < 0.0 {
90        tmin = 0.0;
91    }
92    if tmin > max_toi {
93        return None;
94    }
95    Some((tmin, normal))
96}
97/// Ray-capsule intersection (Y-axis aligned, centered at origin).
98pub(super) fn ray_capsule(
99    origin: [f64; 3],
100    dir: [f64; 3],
101    radius: f64,
102    half_height: f64,
103    max_toi: f64,
104) -> Option<(f64, [f64; 3])> {
105    let mut best: Option<(f64, [f64; 3])> = None;
106    let top_o = [origin[0], origin[1] - half_height, origin[2]];
107    if let Some((t, n)) = ray_sphere(top_o, dir, radius, max_toi) {
108        let hit_y = origin[1] + dir[1] * t;
109        if hit_y >= half_height && best.as_ref().is_none_or(|(bt, _)| t < *bt) {
110            best = Some((t, n));
111        }
112    }
113    let bot_o = [origin[0], origin[1] + half_height, origin[2]];
114    if let Some((t, n)) = ray_sphere(bot_o, dir, radius, max_toi) {
115        let hit_y = origin[1] + dir[1] * t;
116        if hit_y <= -half_height && best.as_ref().is_none_or(|(bt, _)| t < *bt) {
117            best = Some((t, n));
118        }
119    }
120    let a = dir[0] * dir[0] + dir[2] * dir[2];
121    let b = 2.0 * (origin[0] * dir[0] + origin[2] * dir[2]);
122    let c = origin[0] * origin[0] + origin[2] * origin[2] - radius * radius;
123    let disc = b * b - 4.0 * a * c;
124    if a > 1e-12 && disc >= 0.0 {
125        let sqrt_disc = disc.sqrt();
126        for &t in &[(-b - sqrt_disc) / (2.0 * a), (-b + sqrt_disc) / (2.0 * a)] {
127            if t >= 0.0 && t <= max_toi {
128                let hit_y = origin[1] + dir[1] * t;
129                if hit_y.abs() <= half_height {
130                    let hx = origin[0] + dir[0] * t;
131                    let hz = origin[2] + dir[2] * t;
132                    let len = (hx * hx + hz * hz).sqrt();
133                    let n = if len > 1e-12 {
134                        [hx / len, 0.0, hz / len]
135                    } else {
136                        [1.0, 0.0, 0.0]
137                    };
138                    if best.as_ref().is_none_or(|(bt, _)| t < *bt) {
139                        best = Some((t, n));
140                    }
141                }
142            }
143        }
144    }
145    best
146}
147#[cfg(test)]
148mod tests {
149    use super::*;
150    use crate::Compound;
151    use crate::Shape;
152    use crate::box_shape::BoxShape;
153    use crate::compound::ChildShapeKind;
154
155    use crate::compound::CompoundShape;
156
157    use crate::sphere::Sphere;
158    use oxiphysics_core::Real;
159    use oxiphysics_core::Transform;
160    use oxiphysics_core::math::Vec3;
161    use std::f64::consts::PI;
162    use std::sync::Arc;
163    #[test]
164    fn test_compound_volume() {
165        let s1: Arc<dyn Shape> = Arc::new(Sphere::new(1.0));
166        let s2: Arc<dyn Shape> = Arc::new(Sphere::new(1.0));
167        let compound = Compound::new(vec![
168            (Transform::default(), s1.clone()),
169            (
170                Transform::from_position(Vec3::new(5.0, 0.0, 0.0)),
171                s2.clone(),
172            ),
173        ]);
174        assert!((compound.volume() - 2.0 * s1.volume()).abs() < 1e-10);
175    }
176    /// Compound of two unit spheres → volume = 2 * (4/3)π
177    #[test]
178    fn test_compound_two_spheres_volume() {
179        let s1: Arc<dyn Shape> = Arc::new(Sphere::new(1.0));
180        let s2: Arc<dyn Shape> = Arc::new(Sphere::new(1.0));
181        let compound = Compound::new(vec![
182            (Transform::default(), s1),
183            (Transform::from_position(Vec3::new(4.0, 0.0, 0.0)), s2),
184        ]);
185        let expected = 2.0 * (4.0 / 3.0) * PI;
186        assert!(
187            (compound.volume() - expected).abs() < 1e-6,
188            "volume={} expected={}",
189            compound.volume(),
190            expected
191        );
192    }
193    /// Two equal spheres on x-axis at ±d → I_yy = 2*(I_sphere + m_child*d²)
194    #[test]
195    fn test_compound_inertia_parallel_axis() {
196        let r = 1.0_f64;
197        let d = 3.0_f64;
198        let total_mass = 10.0_f64;
199        let s1: Arc<dyn Shape> = Arc::new(Sphere::new(r));
200        let s2: Arc<dyn Shape> = Arc::new(Sphere::new(r));
201        let compound = Compound::new(vec![
202            (Transform::from_position(Vec3::new(d, 0.0, 0.0)), s1),
203            (Transform::from_position(Vec3::new(-d, 0.0, 0.0)), s2),
204        ]);
205        let m_child = total_mass / 2.0;
206        let i_sphere = 2.0 / 5.0 * m_child * r * r;
207        let expected_iyy = 2.0 * (i_sphere + m_child * d * d);
208        let inertia = compound.inertia_tensor(total_mass);
209        let iyy = inertia[(1, 1)];
210        assert!(
211            (iyy - expected_iyy).abs() < 1e-2,
212            "I_yy={} expected={}",
213            iyy,
214            expected_iyy
215        );
216    }
217    /// Compound with box at origin and sphere at (5,0,0); ray from (10,0,0) toward -x → hits sphere first (TOI≈4)
218    #[test]
219    fn test_compound_raycast_hits_child() {
220        let box_shape: Arc<dyn Shape> = Arc::new(BoxShape::new(Vec3::new(0.5, 0.5, 0.5)));
221        let sphere: Arc<dyn Shape> = Arc::new(Sphere::new(1.0));
222        let compound = Compound::new(vec![
223            (Transform::default(), box_shape),
224            (Transform::from_position(Vec3::new(5.0, 0.0, 0.0)), sphere),
225        ]);
226        let origin = Vec3::new(10.0, 0.0, 0.0);
227        let direction = Vec3::new(-1.0, 0.0, 0.0);
228        let hit = compound.ray_cast(&origin, &direction, 100.0);
229        assert!(hit.is_some(), "ray should hit the compound shape");
230        let hit = hit.unwrap();
231        assert!((hit.toi - 4.0).abs() < 1e-2, "toi={} expected≈4.0", hit.toi);
232    }
233    /// Compound of N shapes → total mass = sum of child masses
234    #[test]
235    fn test_compound_mass_properties_additive() {
236        let density = 500.0_f64;
237        let shapes: Vec<Arc<dyn Shape>> = vec![
238            Arc::new(Sphere::new(1.0)),
239            Arc::new(Sphere::new(0.5)),
240            Arc::new(BoxShape::new(Vec3::new(1.0, 1.0, 1.0))),
241        ];
242        let offsets = [
243            Vec3::new(0.0, 0.0, 0.0),
244            Vec3::new(3.0, 0.0, 0.0),
245            Vec3::new(-3.0, 0.0, 0.0),
246        ];
247        let expected_total_mass: Real = shapes.iter().map(|s| density * s.volume()).sum();
248        let children: Vec<(Transform, Arc<dyn Shape>)> = offsets
249            .iter()
250            .zip(shapes.iter())
251            .map(|(pos, s)| (Transform::from_position(*pos), s.clone()))
252            .collect();
253        let compound = Compound::new(children);
254        let props = compound.mass_properties(density);
255        assert!(
256            (props.mass - expected_total_mass).abs() < 1e-6,
257            "compound mass={} expected={}",
258            props.mass,
259            expected_total_mass
260        );
261    }
262    #[test]
263    fn test_compound_shape_single_sphere_volume() {
264        let mut cs = CompoundShape::new();
265        cs.add_sphere([0.0, 0.0, 0.0], 2.0);
266        let expected = (4.0 / 3.0) * PI * 8.0;
267        assert!(
268            (cs.total_volume() - expected).abs() < 1e-6,
269            "vol={} expected={}",
270            cs.total_volume(),
271            expected
272        );
273    }
274    #[test]
275    fn test_compound_shape_box_volume() {
276        let mut cs = CompoundShape::new();
277        cs.add_box([0.0, 0.0, 0.0], [1.0, 2.0, 3.0]);
278        assert!(
279            (cs.total_volume() - 48.0).abs() < 1e-10,
280            "vol={}",
281            cs.total_volume()
282        );
283    }
284    #[test]
285    fn test_compound_shape_aabb_single_sphere() {
286        let mut cs = CompoundShape::new();
287        cs.add_sphere([1.0, 2.0, 3.0], 0.5);
288        let (min, max) = cs.aabb();
289        assert!((min[0] - 0.5).abs() < 1e-10);
290        assert!((min[1] - 1.5).abs() < 1e-10);
291        assert!((min[2] - 2.5).abs() < 1e-10);
292        assert!((max[0] - 1.5).abs() < 1e-10);
293        assert!((max[1] - 2.5).abs() < 1e-10);
294        assert!((max[2] - 3.5).abs() < 1e-10);
295    }
296    #[test]
297    fn test_compound_shape_aabb_multiple() {
298        let mut cs = CompoundShape::new();
299        cs.add_sphere([-5.0, 0.0, 0.0], 1.0);
300        cs.add_sphere([5.0, 0.0, 0.0], 1.0);
301        let (min, max) = cs.aabb();
302        assert!((min[0] - (-6.0)).abs() < 1e-10);
303        assert!((max[0] - 6.0).abs() < 1e-10);
304    }
305    #[test]
306    fn test_compound_shape_com_symmetric() {
307        let mut cs = CompoundShape::new();
308        cs.add_sphere([-3.0, 0.0, 0.0], 1.0);
309        cs.add_sphere([3.0, 0.0, 0.0], 1.0);
310        let com = cs.center_of_mass();
311        assert!((com[0]).abs() < 1e-10, "com_x={}", com[0]);
312        assert!((com[1]).abs() < 1e-10, "com_y={}", com[1]);
313        assert!((com[2]).abs() < 1e-10, "com_z={}", com[2]);
314    }
315    #[test]
316    fn test_compound_shape_com_weighted() {
317        let mut cs = CompoundShape::new();
318        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
319        cs.add_sphere([10.0, 0.0, 0.0], 1.0);
320        let com = cs.center_of_mass();
321        assert!(
322            (com[0] - 5.0).abs() < 1e-10,
323            "com_x={} expected 5.0",
324            com[0]
325        );
326    }
327    #[test]
328    fn test_compound_shape_ray_through_sphere() {
329        let mut cs = CompoundShape::new();
330        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
331        let origin = [-5.0, 0.0, 0.0];
332        let dir = [1.0, 0.0, 0.0];
333        let hit = cs.ray_cast(origin, dir, 100.0);
334        assert!(hit.is_some(), "should hit sphere");
335        let (toi, normal, idx) = hit.unwrap();
336        assert_eq!(idx, 0);
337        assert!((toi - 4.0).abs() < 1e-6, "toi={} expected 4.0", toi);
338        assert!((normal[0] - (-1.0)).abs() < 1e-6, "normal_x={}", normal[0]);
339    }
340    #[test]
341    fn test_compound_shape_ray_misses() {
342        let mut cs = CompoundShape::new();
343        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
344        let origin = [-5.0, 0.0, 0.0];
345        let dir = [-1.0, 0.0, 0.0];
346        let hit = cs.ray_cast(origin, dir, 100.0);
347        assert!(hit.is_none(), "should not hit sphere from behind");
348    }
349    #[test]
350    fn test_compound_shape_ray_closest_child() {
351        let mut cs = CompoundShape::new();
352        cs.add_sphere([5.0, 0.0, 0.0], 1.0);
353        cs.add_sphere([10.0, 0.0, 0.0], 1.0);
354        let origin = [0.0, 0.0, 0.0];
355        let dir = [1.0, 0.0, 0.0];
356        let hit = cs.ray_cast(origin, dir, 100.0);
357        assert!(hit.is_some());
358        let (_, _, idx) = hit.unwrap();
359        assert_eq!(idx, 0, "should hit closer sphere first");
360    }
361    #[test]
362    fn test_compound_shape_contains_point_sphere() {
363        let mut cs = CompoundShape::new();
364        cs.add_sphere([0.0, 0.0, 0.0], 2.0);
365        assert!(cs.contains_point([0.0, 0.0, 0.0]));
366        assert!(cs.contains_point([1.0, 0.0, 0.0]));
367        assert!(!cs.contains_point([3.0, 0.0, 0.0]));
368    }
369    #[test]
370    fn test_compound_shape_contains_point_box() {
371        let mut cs = CompoundShape::new();
372        cs.add_box([0.0, 0.0, 0.0], [1.0, 1.0, 1.0]);
373        assert!(cs.contains_point([0.5, 0.5, 0.5]));
374        assert!(!cs.contains_point([1.5, 0.0, 0.0]));
375    }
376    #[test]
377    fn test_compound_shape_child_count() {
378        let mut cs = CompoundShape::new();
379        assert_eq!(cs.child_count(), 0);
380        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
381        cs.add_box([1.0, 0.0, 0.0], [0.5, 0.5, 0.5]);
382        cs.add_capsule([2.0, 0.0, 0.0], 0.5, 1.0);
383        assert_eq!(cs.child_count(), 3);
384    }
385    #[test]
386    fn test_compound_shape_capsule_volume() {
387        let mut cs = CompoundShape::new();
388        cs.add_capsule([0.0, 0.0, 0.0], 1.0, 2.0);
389        let sphere_vol = (4.0 / 3.0) * PI;
390        let cyl_vol = PI * 4.0;
391        let expected = sphere_vol + cyl_vol;
392        assert!(
393            (cs.total_volume() - expected).abs() < 1e-6,
394            "vol={} expected={}",
395            cs.total_volume(),
396            expected
397        );
398    }
399    #[test]
400    fn test_compound_shape_ray_cast_box() {
401        let mut cs = CompoundShape::new();
402        cs.add_box([0.0, 0.0, 0.0], [1.0, 1.0, 1.0]);
403        let origin = [-5.0, 0.0, 0.0];
404        let dir = [1.0, 0.0, 0.0];
405        let hit = cs.ray_cast(origin, dir, 100.0);
406        assert!(hit.is_some());
407        let (toi, normal, _) = hit.unwrap();
408        assert!((toi - 4.0).abs() < 1e-6, "toi={} expected 4.0", toi);
409        assert!((normal[0] - (-1.0)).abs() < 1e-6);
410    }
411    #[test]
412    fn test_compound_shape_inertia_sphere() {
413        let mut cs = CompoundShape::new();
414        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
415        let density = 1.0;
416        let inertia = cs.inertia_tensor(density);
417        let vol = cs.total_volume();
418        let mass = density * vol;
419        let expected_i = 2.0 / 5.0 * mass;
420        assert!(
421            (inertia[0][0] - expected_i).abs() < 1e-6,
422            "I_xx={}",
423            inertia[0][0]
424        );
425        assert!(
426            (inertia[1][1] - expected_i).abs() < 1e-6,
427            "I_yy={}",
428            inertia[1][1]
429        );
430        assert!(
431            (inertia[2][2] - expected_i).abs() < 1e-6,
432            "I_zz={}",
433            inertia[2][2]
434        );
435    }
436    #[test]
437    fn test_compound_shape_inertia_off_diagonal_zero_for_symmetric() {
438        let mut cs = CompoundShape::new();
439        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
440        let inertia = cs.inertia_tensor(1.0);
441        assert!(inertia[0][1].abs() < 1e-10);
442        assert!(inertia[0][2].abs() < 1e-10);
443        assert!(inertia[1][2].abs() < 1e-10);
444    }
445    #[test]
446    fn test_bounding_sphere_single_sphere() {
447        let mut cs = CompoundShape::new();
448        cs.add_sphere([0.0, 0.0, 0.0], 2.0);
449        let (center, radius) = cs.bounding_sphere();
450        assert!((center[0]).abs() < 1e-10);
451        assert!(radius >= 2.0);
452    }
453    #[test]
454    fn test_bounding_sphere_two_spheres() {
455        let mut cs = CompoundShape::new();
456        cs.add_sphere([-3.0, 0.0, 0.0], 1.0);
457        cs.add_sphere([3.0, 0.0, 0.0], 1.0);
458        let (_, radius) = cs.bounding_sphere();
459        assert!(radius >= 4.0, "radius={}", radius);
460    }
461    #[test]
462    fn test_merge_with_adds_children() {
463        let mut cs1 = CompoundShape::new();
464        cs1.add_sphere([0.0, 0.0, 0.0], 1.0);
465        let mut cs2 = CompoundShape::new();
466        cs2.add_box([2.0, 0.0, 0.0], [0.5, 0.5, 0.5]);
467        let merged = cs1.merge_with(&cs2);
468        assert_eq!(merged.child_count(), 2);
469    }
470    #[test]
471    fn test_scale_doubles_radius() {
472        let mut cs = CompoundShape::new();
473        cs.add_sphere([1.0, 0.0, 0.0], 1.0);
474        cs.scale(2.0);
475        match cs.children[0].shape_kind {
476            ChildShapeKind::Sphere { radius } => assert!((radius - 2.0).abs() < 1e-10),
477            _ => panic!("expected sphere"),
478        }
479        assert!((cs.children[0].center[0] - 2.0).abs() < 1e-10);
480    }
481    #[test]
482    fn test_translate_shifts_centers() {
483        let mut cs = CompoundShape::new();
484        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
485        cs.translate([1.0, 2.0, 3.0]);
486        assert!((cs.children[0].center[0] - 1.0).abs() < 1e-10);
487        assert!((cs.children[0].center[1] - 2.0).abs() < 1e-10);
488        assert!((cs.children[0].center[2] - 3.0).abs() < 1e-10);
489    }
490    #[test]
491    fn test_overlaps_sphere_hit() {
492        let mut cs = CompoundShape::new();
493        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
494        assert!(cs.overlaps_sphere([0.5, 0.0, 0.0], 0.1));
495    }
496    #[test]
497    fn test_overlaps_sphere_miss() {
498        let mut cs = CompoundShape::new();
499        cs.add_sphere([0.0, 0.0, 0.0], 0.5);
500        assert!(!cs.overlaps_sphere([5.0, 0.0, 0.0], 0.1));
501    }
502    #[test]
503    fn test_ray_cast_all_returns_both() {
504        let mut cs = CompoundShape::new();
505        cs.add_sphere([2.0, 0.0, 0.0], 0.5);
506        cs.add_sphere([5.0, 0.0, 0.0], 0.5);
507        let hits = cs.ray_cast_all([0.0, 0.0, 0.0], [1.0, 0.0, 0.0], 100.0);
508        assert_eq!(hits.len(), 2, "should hit both spheres");
509        assert!(hits[0].0 < hits[1].0, "first hit should be closer");
510    }
511    #[test]
512    fn test_merged_aabb_contains_children() {
513        let mut cs = CompoundShape::new();
514        cs.add_sphere([-5.0, 0.0, 0.0], 1.0);
515        cs.add_sphere([5.0, 0.0, 0.0], 1.0);
516        let (mn, mx) = cs.merged_aabb();
517        assert!(mn[0] <= -6.0 + 1e-9, "min_x={}", mn[0]);
518        assert!(mx[0] >= 6.0 - 1e-9, "max_x={}", mx[0]);
519    }
520    #[test]
521    fn test_compound_aabb_struct_all_aabbs() {
522        let mut cs = CompoundShape::new();
523        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
524        cs.add_box([3.0, 0.0, 0.0], [0.5, 0.5, 0.5]);
525        let all: Vec<_> = cs
526            .children
527            .iter()
528            .map(CompoundShape::child_aabb_public)
529            .collect();
530        assert_eq!(all.len(), 2);
531        assert!((all[0].0[0] - (-1.0)).abs() < 1e-10);
532        assert!((all[1].0[0] - 2.5).abs() < 1e-10);
533    }
534    #[test]
535    fn test_raycast_returns_t_and_index() {
536        let mut cs = CompoundShape::new();
537        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
538        let result = cs.raycast([-5.0, 0.0, 0.0], [1.0, 0.0, 0.0], 100.0);
539        assert!(result.is_some());
540        let (t, idx) = result.unwrap();
541        assert_eq!(idx, 0);
542        assert!((t - 4.0).abs() < 1e-6, "t={}", t);
543    }
544    #[test]
545    fn test_volume_sum() {
546        let mut cs = CompoundShape::new();
547        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
548        cs.add_sphere([5.0, 0.0, 0.0], 2.0);
549        let v1 = (4.0 / 3.0) * PI;
550        let v2 = (4.0 / 3.0) * PI * 8.0;
551        assert!(
552            (cs.volume() - (v1 + v2)).abs() < 1e-6,
553            "vol={}",
554            cs.volume()
555        );
556    }
557    #[test]
558    fn test_center_of_mass_weighted_average() {
559        let mut cs = CompoundShape::new();
560        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
561        cs.add_sphere([10.0, 0.0, 0.0], 1.0);
562        let com = cs.center_of_mass_weighted(&[1.0, 1.0]);
563        assert!((com[0] - 5.0).abs() < 1e-10, "com_x={}", com[0]);
564        let com2 = cs.center_of_mass_weighted(&[1.0, 3.0]);
565        assert!((com2[0] - 7.5).abs() < 1e-10, "com2_x={}", com2[0]);
566    }
567    #[test]
568    fn test_inertia_tensor_from_masses_symmetric() {
569        let mut cs = CompoundShape::new();
570        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
571        cs.add_sphere([4.0, 0.0, 0.0], 1.0);
572        let masses = [1.0, 1.0];
573        let i = cs.inertia_tensor_from_masses(&masses);
574        assert!(
575            (i[0][1] - i[1][0]).abs() < 1e-10,
576            "not symmetric [0][1] vs [1][0]"
577        );
578        assert!(
579            (i[0][2] - i[2][0]).abs() < 1e-10,
580            "not symmetric [0][2] vs [2][0]"
581        );
582        assert!(
583            (i[1][2] - i[2][1]).abs() < 1e-10,
584            "not symmetric [1][2] vs [2][1]"
585        );
586    }
587    #[test]
588    fn test_closest_point_sphere() {
589        let mut cs = CompoundShape::new();
590        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
591        let (cp, idx) = cs.closest_point([3.0, 0.0, 0.0]);
592        assert_eq!(idx, 0);
593        assert!((cp[0] - 1.0).abs() < 1e-9, "cp_x={}", cp[0]);
594        assert!(cp[1].abs() < 1e-9);
595        assert!(cp[2].abs() < 1e-9);
596    }
597    #[test]
598    fn test_closest_point_selects_nearest_child() {
599        let mut cs = CompoundShape::new();
600        cs.add_sphere([-10.0, 0.0, 0.0], 1.0);
601        cs.add_sphere([2.0, 0.0, 0.0], 1.0);
602        let (_cp, idx) = cs.closest_point([4.0, 0.0, 0.0]);
603        assert_eq!(idx, 1, "should select nearer child");
604    }
605    #[test]
606    fn test_sphere_inertia_helper() {
607        let i = sphere_inertia(5.0, 2.0);
608        for (k, row) in i.iter().enumerate() {
609            assert!((row[k] - 8.0).abs() < 1e-10, "I[{k}][{k}]={}", row[k]);
610        }
611        assert!(i[0][1].abs() < 1e-15);
612    }
613    #[test]
614    fn test_box_inertia_helper() {
615        let i = box_inertia(3.0, 1.0, 2.0, 3.0);
616        assert!((i[0][0] - 13.0).abs() < 1e-10, "I_xx={}", i[0][0]);
617        assert!(i[0][1].abs() < 1e-15);
618    }
619}
620/// Check if a point in local space is inside a `ChildShapeKind`.
621pub(super) fn child_kind_contains(kind: &ChildShapeKind, p: [f64; 3]) -> bool {
622    match kind {
623        ChildShapeKind::Sphere { radius } => {
624            p[0] * p[0] + p[1] * p[1] + p[2] * p[2] <= radius * radius
625        }
626        ChildShapeKind::Box { half_extents } => {
627            p[0].abs() <= half_extents[0]
628                && p[1].abs() <= half_extents[1]
629                && p[2].abs() <= half_extents[2]
630        }
631        ChildShapeKind::Capsule {
632            radius,
633            half_height,
634        } => {
635            let clamped_y = p[1].clamp(-half_height, *half_height);
636            let ry = p[1] - clamped_y;
637            p[0] * p[0] + ry * ry + p[2] * p[2] <= radius * radius
638        }
639    }
640}
641/// Ray cast against a `ChildShapeKind` at the origin.
642pub(super) fn ray_cast_kind(
643    kind: &ChildShapeKind,
644    origin: [f64; 3],
645    dir: [f64; 3],
646    max_toi: f64,
647) -> Option<(f64, [f64; 3])> {
648    match kind {
649        ChildShapeKind::Sphere { radius } => ray_sphere(origin, dir, *radius, max_toi),
650        ChildShapeKind::Box { half_extents } => ray_box(origin, dir, *half_extents, max_toi),
651        ChildShapeKind::Capsule {
652            radius,
653            half_height,
654        } => ray_capsule(origin, dir, *radius, *half_height, max_toi),
655    }
656}
657#[cfg(test)]
658mod tests_extended {
659
660    use crate::compound::ChildShapeKind;
661
662    use crate::compound::CompoundShapeEx;
663    use crate::compound::LocalTransform;
664    use crate::compound::child_kind_contains;
665
666    use std::f64::consts::PI;
667
668    #[test]
669    fn test_local_to_world_identity() {
670        let t = LocalTransform::identity();
671        let p = [1.0, 2.0, 3.0];
672        let w = t.local_to_world(p);
673        assert!((w[0] - 1.0).abs() < 1e-12);
674        assert!((w[1] - 2.0).abs() < 1e-12);
675        assert!((w[2] - 3.0).abs() < 1e-12);
676    }
677    #[test]
678    fn test_world_to_local_identity() {
679        let t = LocalTransform::identity();
680        let p = [5.0, -3.0, 7.0];
681        let l = t.world_to_local(p);
682        assert!((l[0] - 5.0).abs() < 1e-12);
683        assert!((l[1] - (-3.0)).abs() < 1e-12);
684        assert!((l[2] - 7.0).abs() < 1e-12);
685    }
686    #[test]
687    fn test_local_to_world_translation() {
688        let t = LocalTransform::from_translation([10.0, 20.0, 30.0]);
689        let p = [1.0, 0.0, 0.0];
690        let w = t.local_to_world(p);
691        assert!((w[0] - 11.0).abs() < 1e-12);
692        assert!((w[1] - 20.0).abs() < 1e-12);
693        assert!((w[2] - 30.0).abs() < 1e-12);
694    }
695    #[test]
696    fn test_world_to_local_translation_roundtrip() {
697        let t = LocalTransform::from_translation([3.0, -1.0, 5.0]);
698        let world_p = [7.0, 4.0, 8.0];
699        let local_p = t.world_to_local(world_p);
700        let back = t.local_to_world(local_p);
701        for i in 0..3 {
702            assert!(
703                (back[i] - world_p[i]).abs() < 1e-10,
704                "axis {i}: {} != {}",
705                back[i],
706                world_p[i]
707            );
708        }
709    }
710    #[test]
711    fn test_local_to_world_rotation_90_deg_y() {
712        let t = LocalTransform {
713            translation: [0.0; 3],
714            rot: [[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]],
715        };
716        let p = [1.0, 0.0, 0.0];
717        let w = t.local_to_world(p);
718        let len = (w[0] * w[0] + w[1] * w[1] + w[2] * w[2]).sqrt();
719        assert!((len - 1.0).abs() < 1e-10, "rotation should preserve length");
720    }
721    #[test]
722    fn test_compound_ex_aabb_single_sphere_identity() {
723        let mut cs = CompoundShapeEx::new();
724        cs.add_sphere(LocalTransform::identity(), 2.0);
725        let (mn, mx) = cs.aabb();
726        assert!((mn[0] - (-2.0)).abs() < 1e-10, "min_x={}", mn[0]);
727        assert!((mx[0] - 2.0).abs() < 1e-10, "max_x={}", mx[0]);
728    }
729    #[test]
730    fn test_compound_ex_aabb_translated_sphere() {
731        let mut cs = CompoundShapeEx::new();
732        cs.add_sphere(LocalTransform::from_translation([5.0, 0.0, 0.0]), 1.0);
733        let (mn, mx) = cs.aabb();
734        assert!((mn[0] - 4.0).abs() < 1e-10, "min_x={}", mn[0]);
735        assert!((mx[0] - 6.0).abs() < 1e-10, "max_x={}", mx[0]);
736    }
737    #[test]
738    fn test_compound_ex_contains_point_sphere() {
739        let mut cs = CompoundShapeEx::new();
740        cs.add_sphere(LocalTransform::from_translation([3.0, 0.0, 0.0]), 1.5);
741        assert!(
742            cs.contains_point([3.0, 0.0, 0.0]),
743            "center should be inside"
744        );
745        assert!(!cs.contains_point([0.0, 0.0, 0.0]), "origin is outside");
746    }
747    #[test]
748    fn test_compound_ex_contains_point_box() {
749        let mut cs = CompoundShapeEx::new();
750        cs.add_box(LocalTransform::identity(), [1.0, 2.0, 3.0]);
751        assert!(cs.contains_point([0.5, 1.0, 2.0]));
752        assert!(!cs.contains_point([1.5, 0.0, 0.0]));
753    }
754    #[test]
755    fn test_compound_ex_ray_cast_sphere() {
756        let mut cs = CompoundShapeEx::new();
757        cs.add_sphere(LocalTransform::from_translation([5.0, 0.0, 0.0]), 1.0);
758        let hit = cs.ray_cast([0.0, 0.0, 0.0], [1.0, 0.0, 0.0]);
759        assert!(hit.is_some(), "should hit sphere");
760        let (t, _) = hit.unwrap();
761        assert!((t - 4.0).abs() < 1e-6, "t={t} expected 4.0");
762    }
763    #[test]
764    fn test_compound_ex_volume_sphere() {
765        let mut cs = CompoundShapeEx::new();
766        cs.add_sphere(LocalTransform::identity(), 1.0);
767        let expected = (4.0 / 3.0) * PI;
768        assert!((cs.volume() - expected).abs() < 1e-9, "vol={}", cs.volume());
769    }
770    #[test]
771    fn test_compound_ex_inertia_tensor_sphere_at_origin() {
772        let mut cs = CompoundShapeEx::new();
773        cs.add_sphere(LocalTransform::identity(), 1.0);
774        let density = 1.0;
775        let i = cs.inertia_tensor(density);
776        let vol = (4.0 / 3.0) * PI;
777        let mass = density * vol;
778        let expected = 2.0 / 5.0 * mass;
779        assert!((i[0][0] - expected).abs() < 1e-9, "I_xx={}", i[0][0]);
780        assert!((i[1][1] - expected).abs() < 1e-9, "I_yy={}", i[1][1]);
781        assert!((i[2][2] - expected).abs() < 1e-9, "I_zz={}", i[2][2]);
782    }
783    #[test]
784    fn test_compound_ex_inertia_tensor_parallel_axis() {
785        let d = 3.0_f64;
786        let density = 1.0;
787        let mut cs = CompoundShapeEx::new();
788        cs.add_sphere(LocalTransform::from_translation([d, 0.0, 0.0]), 1.0);
789        let i = cs.inertia_tensor(density);
790        let vol = (4.0 / 3.0) * PI;
791        let mass = density * vol;
792        let expected_iyy = 2.0 / 5.0 * mass + mass * d * d;
793        assert!(
794            (i[1][1] - expected_iyy).abs() < 1e-6,
795            "I_yy={} expected={}",
796            i[1][1],
797            expected_iyy
798        );
799    }
800    #[test]
801    fn test_child_kind_contains_sphere() {
802        let k = ChildShapeKind::Sphere { radius: 2.0 };
803        assert!(child_kind_contains(&k, [0.0, 0.0, 0.0]));
804        assert!(child_kind_contains(&k, [1.9, 0.0, 0.0]));
805        assert!(!child_kind_contains(&k, [2.1, 0.0, 0.0]));
806    }
807    #[test]
808    fn test_child_kind_contains_box() {
809        let k = ChildShapeKind::Box {
810            half_extents: [1.0, 2.0, 3.0],
811        };
812        assert!(child_kind_contains(&k, [0.5, 1.5, 2.5]));
813        assert!(!child_kind_contains(&k, [1.5, 0.0, 0.0]));
814    }
815    #[test]
816    fn test_child_kind_contains_capsule() {
817        let k = ChildShapeKind::Capsule {
818            radius: 1.0,
819            half_height: 2.0,
820        };
821        assert!(child_kind_contains(&k, [0.5, 1.0, 0.5]));
822        assert!(!child_kind_contains(&k, [2.0, 0.0, 0.0]));
823    }
824}
825#[cfg(test)]
826mod tests_extended2 {
827
828    use crate::compound::ChildShapeKind;
829    use crate::compound::CompoundChild;
830    use crate::compound::CompoundShape;
831
832    use std::f64::consts::PI;
833
834    #[test]
835    fn test_remove_child_decreases_count() {
836        let mut cs = CompoundShape::new();
837        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
838        cs.add_sphere([5.0, 0.0, 0.0], 1.0);
839        assert_eq!(cs.child_count(), 2);
840        cs.remove_child(0);
841        assert_eq!(cs.child_count(), 1);
842    }
843    #[test]
844    fn test_swap_remove_child() {
845        let mut cs = CompoundShape::new();
846        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
847        cs.add_sphere([5.0, 0.0, 0.0], 2.0);
848        cs.add_sphere([10.0, 0.0, 0.0], 3.0);
849        cs.swap_remove_child(0);
850        assert_eq!(cs.child_count(), 2);
851    }
852    #[test]
853    fn test_replace_with_sphere_changes_kind() {
854        let mut cs = CompoundShape::new();
855        cs.add_box([0.0, 0.0, 0.0], [1.0, 1.0, 1.0]);
856        cs.replace_with_sphere(0, [1.0, 2.0, 3.0], 0.5);
857        match cs.children[0].shape_kind {
858            ChildShapeKind::Sphere { radius } => {
859                assert!((radius - 0.5).abs() < 1e-10, "radius should be 0.5");
860            }
861            _ => panic!("expected Sphere after replace"),
862        }
863        assert!((cs.children[0].center[0] - 1.0).abs() < 1e-10);
864    }
865    #[test]
866    fn test_replace_with_box_changes_kind() {
867        let mut cs = CompoundShape::new();
868        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
869        cs.replace_with_box(0, [2.0, 0.0, 0.0], [0.5, 0.5, 0.5]);
870        match cs.children[0].shape_kind {
871            ChildShapeKind::Box { half_extents } => {
872                assert!((half_extents[0] - 0.5).abs() < 1e-10);
873            }
874            _ => panic!("expected Box after replace"),
875        }
876    }
877    #[test]
878    fn test_is_empty_and_clear() {
879        let mut cs = CompoundShape::new();
880        assert!(cs.is_empty(), "newly created should be empty");
881        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
882        assert!(!cs.is_empty());
883        cs.clear();
884        assert!(cs.is_empty(), "should be empty after clear");
885    }
886    #[test]
887    fn test_closest_point_with_dist2_sphere() {
888        let mut cs = CompoundShape::new();
889        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
890        let (cp, d2, idx) = cs.closest_point_with_dist2([3.0, 0.0, 0.0]);
891        assert_eq!(idx, 0);
892        assert!((cp[0] - 1.0).abs() < 1e-9);
893        assert!((d2 - 4.0).abs() < 1e-9, "dist2 should be 4, got {d2}");
894    }
895    #[test]
896    fn test_broad_phase_pairs_overlap() {
897        let mut cs1 = CompoundShape::new();
898        cs1.add_sphere([0.0, 0.0, 0.0], 2.0);
899        let mut cs2 = CompoundShape::new();
900        cs2.add_sphere([1.0, 0.0, 0.0], 2.0);
901        let pairs = cs1.broad_phase_pairs(&cs2);
902        assert!(
903            !pairs.is_empty(),
904            "overlapping spheres should give broad-phase pair"
905        );
906    }
907    #[test]
908    fn test_broad_phase_pairs_no_overlap() {
909        let mut cs1 = CompoundShape::new();
910        cs1.add_sphere([0.0, 0.0, 0.0], 0.5);
911        let mut cs2 = CompoundShape::new();
912        cs2.add_sphere([100.0, 0.0, 0.0], 0.5);
913        let pairs = cs1.broad_phase_pairs(&cs2);
914        assert!(
915            pairs.is_empty(),
916            "distant spheres should not produce broad-phase pairs"
917        );
918    }
919    #[test]
920    fn test_overlaps_compound_hit() {
921        let mut cs1 = CompoundShape::new();
922        cs1.add_sphere([0.0, 0.0, 0.0], 1.5);
923        let mut cs2 = CompoundShape::new();
924        cs2.add_sphere([1.0, 0.0, 0.0], 1.5);
925        assert!(cs1.overlaps_compound(&cs2));
926    }
927    #[test]
928    fn test_overlaps_compound_miss() {
929        let mut cs1 = CompoundShape::new();
930        cs1.add_sphere([0.0, 0.0, 0.0], 0.5);
931        let mut cs2 = CompoundShape::new();
932        cs2.add_sphere([50.0, 0.0, 0.0], 0.5);
933        assert!(!cs1.overlaps_compound(&cs2));
934    }
935    #[test]
936    fn test_centroid_with_densities_equal() {
937        let mut cs = CompoundShape::new();
938        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
939        cs.add_sphere([10.0, 0.0, 0.0], 1.0);
940        let c = cs.centroid_with_densities(&[1.0, 1.0]);
941        assert!((c[0] - 5.0).abs() < 1e-9, "centroid_x={}", c[0]);
942    }
943    #[test]
944    fn test_centroid_with_densities_unequal() {
945        let mut cs = CompoundShape::new();
946        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
947        cs.add_sphere([10.0, 0.0, 0.0], 1.0);
948        let c = cs.centroid_with_densities(&[1.0, 9.0]);
949        assert!(
950            c[0] > 5.0,
951            "centroid should be above 5 with heavier right child"
952        );
953    }
954    #[test]
955    fn test_penetration_depth_sphere_penetrates() {
956        let mut cs = CompoundShape::new();
957        cs.add_sphere([0.0, 0.0, 0.0], 2.0);
958        let result = cs.penetration_depth_sphere([0.5, 0.0, 0.0], 2.0);
959        assert!(result.is_some(), "should detect penetration");
960        let (depth, idx) = result.unwrap();
961        assert_eq!(idx, 0);
962        assert!(
963            depth < 0.0,
964            "depth should be negative for penetration, got {depth}"
965        );
966    }
967    #[test]
968    fn test_penetration_depth_sphere_no_penetration() {
969        let mut cs = CompoundShape::new();
970        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
971        let result = cs.penetration_depth_sphere([10.0, 0.0, 0.0], 0.5);
972        assert!(result.is_none(), "no penetration for distant sphere");
973    }
974    #[test]
975    fn test_child_masses_sum() {
976        let mut cs = CompoundShape::new();
977        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
978        cs.add_sphere([5.0, 0.0, 0.0], 2.0);
979        let density = 3.0;
980        let masses = cs.child_masses(density);
981        let expected: f64 = cs.total_mass(density);
982        let actual: f64 = masses.iter().sum();
983        assert!((actual - expected).abs() < 1e-9, "mass sum mismatch");
984    }
985    #[test]
986    fn test_total_mass() {
987        let mut cs = CompoundShape::new();
988        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
989        let density = 2.0;
990        let expected = density * (4.0 / 3.0) * PI;
991        let m = cs.total_mass(density);
992        assert!(
993            (m - expected).abs() < 1e-9,
994            "total_mass={m} expected={expected}"
995        );
996    }
997    #[test]
998    fn test_child_aabb_public() {
999        let child = CompoundChild {
1000            center: [1.0, 2.0, 3.0],
1001            shape_kind: ChildShapeKind::Sphere { radius: 1.0 },
1002        };
1003        let (mn, mx) = CompoundShape::child_aabb_public(&child);
1004        assert!((mn[0] - 0.0).abs() < 1e-10);
1005        assert!((mx[0] - 2.0).abs() < 1e-10);
1006    }
1007    #[test]
1008    fn test_expanded_aabb_increases_size() {
1009        let mut cs = CompoundShape::new();
1010        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
1011        let (mn0, mx0) = cs.aabb();
1012        let (mn1, mx1) = cs.expanded_aabb(0.5);
1013        for i in 0..3 {
1014            assert!(mn1[i] < mn0[i], "expanded min should be smaller");
1015            assert!(mx1[i] > mx0[i], "expanded max should be larger");
1016        }
1017    }
1018    #[test]
1019    fn test_sphere_overlaps_aabb_inside() {
1020        let mut cs = CompoundShape::new();
1021        cs.add_sphere([0.0, 0.0, 0.0], 2.0);
1022        assert!(cs.sphere_overlaps_aabb([0.0, 0.0, 0.0], 0.1));
1023    }
1024    #[test]
1025    fn test_sphere_overlaps_aabb_outside() {
1026        let mut cs = CompoundShape::new();
1027        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
1028        assert!(!cs.sphere_overlaps_aabb([100.0, 0.0, 0.0], 0.5));
1029    }
1030}
1031#[cfg(test)]
1032mod tests_extra {
1033
1034    use crate::compound::ChildShapeKind;
1035
1036    use crate::compound::CompoundShape;
1037    use crate::compound::CompoundShapeEx;
1038    use crate::compound::LocalTransform;
1039
1040    use std::f64::consts::PI;
1041
1042    #[test]
1043    fn test_aabb_union_three_spheres() {
1044        let mut cs = CompoundShape::new();
1045        cs.add_sphere([-5.0, 0.0, 0.0], 1.0);
1046        cs.add_sphere([0.0, 5.0, 0.0], 1.0);
1047        cs.add_sphere([0.0, 0.0, 5.0], 1.0);
1048        let (mn, mx) = cs.aabb();
1049        assert!(mn[0] <= -6.0 + 1e-9);
1050        assert!(mx[1] >= 6.0 - 1e-9);
1051        assert!(mx[2] >= 6.0 - 1e-9);
1052    }
1053    #[test]
1054    fn test_aabb_union_empty_compound() {
1055        let cs = CompoundShape::new();
1056        let (mn, mx) = cs.aabb();
1057        for i in 0..3 {
1058            assert!((mn[i]).abs() < 1e-12);
1059            assert!((mx[i]).abs() < 1e-12);
1060        }
1061    }
1062    #[test]
1063    fn test_aabb_tight_for_single_box() {
1064        let mut cs = CompoundShape::new();
1065        cs.add_box([3.0, 1.0, -2.0], [2.0, 1.0, 0.5]);
1066        let (mn, mx) = cs.aabb();
1067        assert!((mn[0] - 1.0).abs() < 1e-9, "min_x={}", mn[0]);
1068        assert!((mx[0] - 5.0).abs() < 1e-9, "max_x={}", mx[0]);
1069        assert!((mn[1] - 0.0).abs() < 1e-9, "min_y={}", mn[1]);
1070        assert!((mx[1] - 2.0).abs() < 1e-9, "max_y={}", mx[1]);
1071    }
1072    #[test]
1073    fn test_aabb_capsule() {
1074        let mut cs = CompoundShape::new();
1075        cs.add_capsule([0.0, 0.0, 0.0], 1.0, 3.0);
1076        let (mn, mx) = cs.aabb();
1077        assert!((mn[0] - (-1.0)).abs() < 1e-9);
1078        assert!(
1079            (mx[1] - 4.0).abs() < 1e-9,
1080            "max_y for capsule with hh=3, r=1 should be 4, got {}",
1081            mx[1]
1082        );
1083    }
1084    #[test]
1085    fn test_raycast_selects_first_among_three_children() {
1086        let mut cs = CompoundShape::new();
1087        cs.add_sphere([2.0, 0.0, 0.0], 0.5);
1088        cs.add_sphere([5.0, 0.0, 0.0], 0.5);
1089        cs.add_sphere([8.0, 0.0, 0.0], 0.5);
1090        let result = cs.raycast([0.0, 0.0, 0.0], [1.0, 0.0, 0.0], 100.0);
1091        assert!(result.is_some());
1092        let (_, idx) = result.unwrap();
1093        assert_eq!(idx, 0, "should hit first (closest) sphere, got idx={idx}");
1094    }
1095    #[test]
1096    fn test_raycast_all_sorts_by_toi() {
1097        let mut cs = CompoundShape::new();
1098        cs.add_sphere([3.0, 0.0, 0.0], 0.5);
1099        cs.add_sphere([7.0, 0.0, 0.0], 0.5);
1100        let hits = cs.ray_cast_all([0.0, 0.0, 0.0], [1.0, 0.0, 0.0], 100.0);
1101        assert_eq!(hits.len(), 2, "should hit both spheres");
1102        assert!(hits[0].0 < hits[1].0, "hits should be sorted by toi");
1103    }
1104    #[test]
1105    fn test_ray_cast_none_when_all_behind_origin() {
1106        let mut cs = CompoundShape::new();
1107        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
1108        let result = cs.ray_cast([5.0, 0.0, 0.0], [1.0, 0.0, 0.0], 100.0);
1109        assert!(result.is_none(), "should not hit sphere behind ray origin");
1110    }
1111    #[test]
1112    fn test_ray_cast_box_all_six_faces() {
1113        let mut cs = CompoundShape::new();
1114        cs.add_box([0.0, 0.0, 0.0], [1.0, 1.0, 1.0]);
1115        let h = cs.ray_cast([5.0, 0.0, 0.0], [-1.0, 0.0, 0.0], 100.0);
1116        assert!(h.is_some(), "+X face miss");
1117        let h = cs.ray_cast([-5.0, 0.0, 0.0], [1.0, 0.0, 0.0], 100.0);
1118        assert!(h.is_some(), "-X face miss");
1119        let h = cs.ray_cast([0.0, 5.0, 0.0], [0.0, -1.0, 0.0], 100.0);
1120        assert!(h.is_some(), "+Y face miss");
1121        let h = cs.ray_cast([0.0, 0.0, -5.0], [0.0, 0.0, 1.0], 100.0);
1122        assert!(h.is_some(), "-Z face miss");
1123    }
1124    #[test]
1125    fn test_total_volume_scales_with_sphere_radius() {
1126        let mut cs1 = CompoundShape::new();
1127        cs1.add_sphere([0.0, 0.0, 0.0], 1.0);
1128        let mut cs2 = CompoundShape::new();
1129        cs2.add_sphere([0.0, 0.0, 0.0], 2.0);
1130        let ratio = cs2.total_volume() / cs1.total_volume();
1131        assert!(
1132            (ratio - 8.0).abs() < 1e-6,
1133            "volume ratio should be 8, got {ratio}"
1134        );
1135    }
1136    #[test]
1137    fn test_total_volume_box_scales_with_half_extents() {
1138        let mut cs = CompoundShape::new();
1139        cs.add_box([0.0, 0.0, 0.0], [2.0, 3.0, 4.0]);
1140        let expected = 8.0 * 2.0 * 3.0 * 4.0;
1141        assert!(
1142            (cs.total_volume() - expected).abs() < 1e-9,
1143            "box volume={}, expected={expected}",
1144            cs.total_volume()
1145        );
1146    }
1147    #[test]
1148    fn test_total_mass_proportional_to_density() {
1149        let mut cs = CompoundShape::new();
1150        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
1151        let m1 = cs.total_mass(1.0);
1152        let m2 = cs.total_mass(3.0);
1153        assert!(
1154            (m2 / m1 - 3.0).abs() < 1e-9,
1155            "mass should scale with density"
1156        );
1157    }
1158    #[test]
1159    fn test_child_masses_length_matches_children() {
1160        let mut cs = CompoundShape::new();
1161        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
1162        cs.add_box([5.0, 0.0, 0.0], [1.0, 1.0, 1.0]);
1163        cs.add_capsule([10.0, 0.0, 0.0], 0.5, 1.0);
1164        let masses = cs.child_masses(2.0);
1165        assert_eq!(masses.len(), 3);
1166        for &m in &masses {
1167            assert!(m > 0.0, "all child masses should be positive");
1168        }
1169    }
1170    #[test]
1171    fn test_centroid_with_densities_single_child() {
1172        let mut cs = CompoundShape::new();
1173        cs.add_sphere([3.0, 4.0, 5.0], 1.0);
1174        let c = cs.centroid_with_densities(&[1.0]);
1175        assert!((c[0] - 3.0).abs() < 1e-9, "cx={}", c[0]);
1176        assert!((c[1] - 4.0).abs() < 1e-9, "cy={}", c[1]);
1177        assert!((c[2] - 5.0).abs() < 1e-9, "cz={}", c[2]);
1178    }
1179    #[test]
1180    fn test_centroid_uses_default_density_for_missing_entries() {
1181        let mut cs = CompoundShape::new();
1182        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
1183        cs.add_sphere([10.0, 0.0, 0.0], 1.0);
1184        let c = cs.centroid_with_densities(&[1.0]);
1185        assert!((c[0] - 5.0).abs() < 1e-9, "cx={}", c[0]);
1186    }
1187    #[test]
1188    fn test_inertia_tensor_single_sphere_at_origin_all_diagonal() {
1189        let mut cs = CompoundShape::new();
1190        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
1191        let i = cs.inertia_tensor(1.0);
1192        assert!(i[0][1].abs() < 1e-10);
1193        assert!(i[0][2].abs() < 1e-10);
1194        assert!(i[1][2].abs() < 1e-10);
1195    }
1196    #[test]
1197    fn test_inertia_tensor_from_masses_single_box() {
1198        let mut cs = CompoundShape::new();
1199        cs.add_box([0.0, 0.0, 0.0], [1.0, 2.0, 3.0]);
1200        let i = cs.inertia_tensor_from_masses(&[10.0]);
1201        let expected_xx = 10.0 / 3.0 * (4.0 + 9.0);
1202        assert!(
1203            (i[0][0] - expected_xx).abs() < 1e-6,
1204            "I_xx={}, expected={expected_xx}",
1205            i[0][0]
1206        );
1207    }
1208    #[test]
1209    fn test_inertia_tensor_two_equal_spheres_parallel_axis() {
1210        let d = 2.0_f64;
1211        let mut cs = CompoundShape::new();
1212        cs.add_sphere([-d, 0.0, 0.0], 1.0);
1213        cs.add_sphere([d, 0.0, 0.0], 1.0);
1214        let density = 1.0;
1215        let i = cs.inertia_tensor(density);
1216        let vol_sphere = (4.0 / 3.0) * PI;
1217        let mass_each = density * vol_sphere;
1218        let i_sphere_y = 2.0 / 5.0 * mass_each;
1219        let expected_iyy = 2.0 * (i_sphere_y + mass_each * d * d);
1220        assert!(
1221            (i[1][1] - expected_iyy).abs() < 1e-6,
1222            "I_yy={}, expected={expected_iyy}",
1223            i[1][1]
1224        );
1225    }
1226    #[test]
1227    fn test_contains_point_capsule_inside_cylinder() {
1228        let mut cs = CompoundShape::new();
1229        cs.add_capsule([0.0, 0.0, 0.0], 1.0, 2.0);
1230        assert!(
1231            cs.contains_point([0.5, 1.0, 0.0]),
1232            "should be inside capsule cylinder"
1233        );
1234    }
1235    #[test]
1236    fn test_contains_point_capsule_inside_hemisphere() {
1237        let mut cs = CompoundShape::new();
1238        cs.add_capsule([0.0, 0.0, 0.0], 1.0, 2.0);
1239        assert!(
1240            cs.contains_point([0.0, 2.5, 0.0]),
1241            "should be inside top hemisphere"
1242        );
1243    }
1244    #[test]
1245    fn test_contains_point_capsule_outside() {
1246        let mut cs = CompoundShape::new();
1247        cs.add_capsule([0.0, 0.0, 0.0], 1.0, 2.0);
1248        assert!(
1249            !cs.contains_point([0.0, 5.0, 0.0]),
1250            "should be outside capsule"
1251        );
1252    }
1253    #[test]
1254    fn test_contains_point_multiple_shapes_uses_union() {
1255        let mut cs = CompoundShape::new();
1256        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
1257        cs.add_sphere([10.0, 0.0, 0.0], 1.0);
1258        assert!(
1259            cs.contains_point([0.5, 0.0, 0.0]),
1260            "should be inside first sphere"
1261        );
1262        assert!(
1263            cs.contains_point([10.5, 0.0, 0.0]),
1264            "should be inside second sphere"
1265        );
1266        assert!(
1267            !cs.contains_point([5.0, 0.0, 0.0]),
1268            "should be outside both spheres"
1269        );
1270    }
1271    #[test]
1272    fn test_closest_point_on_box_surface() {
1273        let mut cs = CompoundShape::new();
1274        cs.add_box([0.0, 0.0, 0.0], [1.0, 1.0, 1.0]);
1275        let (cp, _idx) = cs.closest_point([5.0, 0.0, 0.0]);
1276        assert!(
1277            (cp[0] - 1.0).abs() < 1e-9,
1278            "closest x should be at surface 1.0, got {}",
1279            cp[0]
1280        );
1281    }
1282    #[test]
1283    fn test_closest_point_on_capsule_cylinder_part() {
1284        let mut cs = CompoundShape::new();
1285        cs.add_capsule([0.0, 0.0, 0.0], 1.0, 2.0);
1286        let (cp, _idx) = cs.closest_point([5.0, 0.0, 0.0]);
1287        assert!(
1288            (cp[0] - 1.0).abs() < 1e-9,
1289            "closest x on capsule should be 1.0, got {}",
1290            cp[0]
1291        );
1292    }
1293    #[test]
1294    fn test_merge_preserves_all_shapes() {
1295        let mut cs1 = CompoundShape::new();
1296        cs1.add_sphere([0.0, 0.0, 0.0], 1.0);
1297        cs1.add_box([2.0, 0.0, 0.0], [0.5, 0.5, 0.5]);
1298        let mut cs2 = CompoundShape::new();
1299        cs2.add_capsule([4.0, 0.0, 0.0], 0.5, 1.0);
1300        let merged = cs1.merge_with(&cs2);
1301        assert_eq!(merged.child_count(), 3);
1302        let vol = merged.total_volume();
1303        assert!(vol > cs1.total_volume(), "merged volume should be larger");
1304    }
1305    #[test]
1306    fn test_clear_then_add_works() {
1307        let mut cs = CompoundShape::new();
1308        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
1309        cs.add_sphere([5.0, 0.0, 0.0], 2.0);
1310        cs.clear();
1311        assert!(cs.is_empty());
1312        cs.add_sphere([1.0, 0.0, 0.0], 0.5);
1313        assert_eq!(cs.child_count(), 1);
1314    }
1315    #[test]
1316    fn test_scale_capsule() {
1317        let mut cs = CompoundShape::new();
1318        cs.add_capsule([1.0, 0.0, 0.0], 1.0, 2.0);
1319        cs.scale(3.0);
1320        match cs.children[0].shape_kind {
1321            ChildShapeKind::Capsule {
1322                radius,
1323                half_height,
1324            } => {
1325                assert!((radius - 3.0).abs() < 1e-10);
1326                assert!((half_height - 6.0).abs() < 1e-10);
1327            }
1328            _ => panic!("expected Capsule"),
1329        }
1330        assert!((cs.children[0].center[0] - 3.0).abs() < 1e-10);
1331    }
1332    #[test]
1333    fn test_translate_multiple_children() {
1334        let mut cs = CompoundShape::new();
1335        cs.add_sphere([1.0, 2.0, 3.0], 1.0);
1336        cs.add_box([4.0, 5.0, 6.0], [1.0, 1.0, 1.0]);
1337        cs.translate([10.0, 0.0, -5.0]);
1338        assert!((cs.children[0].center[0] - 11.0).abs() < 1e-10);
1339        assert!((cs.children[0].center[2] - (-2.0)).abs() < 1e-10);
1340        assert!((cs.children[1].center[0] - 14.0).abs() < 1e-10);
1341    }
1342    #[test]
1343    fn test_compound_ex_volume_two_shapes() {
1344        let mut cs = CompoundShapeEx::new();
1345        cs.add_sphere(LocalTransform::identity(), 1.0);
1346        cs.add_box(
1347            LocalTransform::from_translation([5.0, 0.0, 0.0]),
1348            [1.0, 1.0, 1.0],
1349        );
1350        let v_sphere = (4.0 / 3.0) * PI;
1351        let v_box = 8.0;
1352        assert!(
1353            (cs.volume() - (v_sphere + v_box)).abs() < 1e-9,
1354            "total volume mismatch"
1355        );
1356    }
1357    #[test]
1358    fn test_compound_ex_ray_cast_misses() {
1359        let mut cs = CompoundShapeEx::new();
1360        cs.add_sphere(LocalTransform::from_translation([0.0, 10.0, 0.0]), 1.0);
1361        let hit = cs.ray_cast([0.0, 0.0, 0.0], [1.0, 0.0, 0.0]);
1362        assert!(hit.is_none(), "ray along X should not hit sphere at y=10");
1363    }
1364    #[test]
1365    fn test_compound_ex_contains_point_capsule() {
1366        let mut cs = CompoundShapeEx::new();
1367        cs.add_capsule(LocalTransform::from_translation([0.0, 5.0, 0.0]), 1.0, 3.0);
1368        assert!(
1369            cs.contains_point([0.0, 5.0, 0.0]),
1370            "center of capsule should be inside"
1371        );
1372        assert!(
1373            !cs.contains_point([0.0, 0.0, 0.0]),
1374            "origin should be outside"
1375        );
1376    }
1377    #[test]
1378    fn test_local_transform_direction_roundtrip() {
1379        let t = LocalTransform {
1380            translation: [3.0, -1.0, 2.0],
1381            rot: [[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]],
1382        };
1383        let v = [1.0, 0.0, 0.0];
1384        let world = t.local_to_world_dir(v);
1385        let back = t.world_to_local_dir(world);
1386        for i in 0..3 {
1387            assert!(
1388                (back[i] - v[i]).abs() < 1e-10,
1389                "dir roundtrip failed at index {i}"
1390            );
1391        }
1392    }
1393    #[test]
1394    fn test_overlaps_sphere_at_boundary() {
1395        let mut cs = CompoundShape::new();
1396        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
1397        assert!(
1398            cs.overlaps_sphere([1.5, 0.0, 0.0], 1.0),
1399            "overlapping spheres should be detected"
1400        );
1401    }
1402    #[test]
1403    fn test_sphere_overlaps_aabb_boundary() {
1404        let mut cs = CompoundShape::new();
1405        cs.add_box([0.0, 0.0, 0.0], [1.0, 1.0, 1.0]);
1406        assert!(cs.sphere_overlaps_aabb([2.5, 0.0, 0.0], 1.5));
1407    }
1408    #[test]
1409    fn test_penetration_depth_box_penetrates() {
1410        let mut cs = CompoundShape::new();
1411        cs.add_box([0.0, 0.0, 0.0], [2.0, 2.0, 2.0]);
1412        let result = cs.penetration_depth_sphere([1.0, 0.0, 0.0], 2.0);
1413        assert!(result.is_some(), "sphere inside box should penetrate");
1414        let (depth, _idx) = result.unwrap();
1415        assert!(depth < 0.0, "penetration depth should be negative");
1416    }
1417    #[test]
1418    fn test_closest_point_with_dist2_multiple_children() {
1419        let mut cs = CompoundShape::new();
1420        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
1421        cs.add_sphere([10.0, 0.0, 0.0], 1.0);
1422        let (_cp, _d2, idx) = cs.closest_point_with_dist2([3.0, 0.0, 0.0]);
1423        assert_eq!(idx, 0, "child 0 is closer");
1424    }
1425    #[test]
1426    fn test_inertia_tensor_is_symmetric_three_mixed_shapes() {
1427        let mut cs = CompoundShape::new();
1428        cs.add_sphere([1.0, 0.0, 0.0], 1.0);
1429        cs.add_box([-1.0, 2.0, 0.0], [0.5, 1.0, 0.5]);
1430        cs.add_capsule([0.0, -3.0, 1.0], 0.8, 1.5);
1431        let i = cs.inertia_tensor(2.0);
1432        assert!((i[0][1] - i[1][0]).abs() < 1e-9, "[0][1] vs [1][0]");
1433        assert!((i[0][2] - i[2][0]).abs() < 1e-9, "[0][2] vs [2][0]");
1434        assert!((i[1][2] - i[2][1]).abs() < 1e-9, "[1][2] vs [2][1]");
1435    }
1436    #[test]
1437    fn test_inertia_tensor_diagonal_positive() {
1438        let mut cs = CompoundShape::new();
1439        cs.add_sphere([0.0, 0.0, 0.0], 1.0);
1440        cs.add_box([5.0, 0.0, 0.0], [1.0, 1.0, 1.0]);
1441        let i = cs.inertia_tensor(1.0);
1442        assert!(i[0][0] > 0.0, "I_xx should be positive");
1443        assert!(i[1][1] > 0.0, "I_yy should be positive");
1444        assert!(i[2][2] > 0.0, "I_zz should be positive");
1445    }
1446}
oxiphysics_geometry/compound/functions.rs

oxiphysics_geometry/compound/
functions.rs
