oxiphysics_collision/query/
querycapsule_traits.rs

1// # QueryCapsule - Trait Implementations
2//
3// This module contains trait implementations for `QueryCapsule`.
4//
5// ## Implemented Traits
6//
7// - `ShapeQuery`
8//
9// 🤖 Generated with [SplitRS](https://github.com/cool-japan/splitrs)
10
11use super::functions::*;
12use super::types::{PointQueryResult, QueryCapsule, QuerySphere, RayCastResult};
13
14impl ShapeQuery for QueryCapsule {
15    fn ray_cast(
16        &self,
17        ray_origin: [f64; 3],
18        ray_dir: [f64; 3],
19        max_toi: f64,
20    ) -> Option<RayCastResult> {
21        let ab = sub(self.p1, self.p0);
22        let ao = sub(ray_origin, self.p0);
23        let r = self.radius;
24        let d = ray_dir;
25        let ab_len = norm(ab);
26        if ab_len < 1e-15 {
27            let sphere = QuerySphere {
28                center: self.p0,
29                radius: r,
30            };
31            return sphere.ray_cast(ray_origin, ray_dir, max_toi);
32        }
33        let axis = scale(ab, 1.0 / ab_len);
34        let d_par = dot(d, axis);
35        let d_perp = sub(d, scale(axis, d_par));
36        let ao_par = dot(ao, axis);
37        let ao_perp = sub(ao, scale(axis, ao_par));
38        let a = dot(d_perp, d_perp);
39        let b = dot(ao_perp, d_perp);
40        let c_coef = dot(ao_perp, ao_perp) - r * r;
41        let mut best_toi = f64::INFINITY;
42        let mut best_point = [0.0; 3];
43        let mut best_normal = [0.0; 3];
44        if a > 1e-30 {
45            let disc = b * b - a * c_coef;
46            if disc >= 0.0 {
47                let sqrt_disc = disc.sqrt();
48                for &sign in &[-1.0_f64, 1.0] {
49                    let t = (-b + sign * sqrt_disc) / a;
50                    if t < 0.0 || t > max_toi {
51                        continue;
52                    }
53                    let hit = add(ray_origin, scale(d, t));
54                    let hit_par = dot(sub(hit, self.p0), axis);
55                    if hit_par < 0.0 || hit_par > ab_len {
56                        continue;
57                    }
58                    if t < best_toi {
59                        best_toi = t;
60                        best_point = hit;
61                        let proj = add(self.p0, scale(axis, hit_par));
62                        best_normal = normalize(sub(hit, proj));
63                    }
64                }
65            }
66        }
67        for &cap_center in &[self.p0, self.p1] {
68            let sphere = QuerySphere {
69                center: cap_center,
70                radius: r,
71            };
72            if let Some(res) = sphere.ray_cast(ray_origin, d, max_toi) {
73                let hit_par = dot(sub(res.point, self.p0), axis);
74                let valid = if cap_center == self.p0 {
75                    hit_par <= 0.0
76                } else {
77                    hit_par >= ab_len
78                };
79                if valid && res.toi < best_toi {
80                    best_toi = res.toi;
81                    best_point = res.point;
82                    best_normal = res.normal;
83                }
84            }
85        }
86        if best_toi.is_finite() && best_toi <= max_toi {
87            Some(RayCastResult {
88                toi: best_toi,
89                normal: best_normal,
90                point: best_point,
91            })
92        } else {
93            None
94        }
95    }
96    fn point_query(&self, point: [f64; 3]) -> PointQueryResult {
97        let (seg_pt, _t) = self.closest_point_on_segment(point);
98        let diff = sub(point, seg_pt);
99        let dist_from_axis = norm(diff);
100        let signed_dist = dist_from_axis - self.radius;
101        let inside = signed_dist < 0.0;
102        let closest = if dist_from_axis < 1e-15 {
103            add(seg_pt, [self.radius, 0.0, 0.0])
104        } else {
105            add(seg_pt, scale(normalize(diff), self.radius))
106        };
107        PointQueryResult {
108            point: closest,
109            distance: signed_dist,
110            inside,
111        }
112    }
113    fn aabb(&self) -> ([f64; 3], [f64; 3]) {
114        let r = self.radius;
115        let lo = [
116            self.p0[0].min(self.p1[0]) - r,
117            self.p0[1].min(self.p1[1]) - r,
118            self.p0[2].min(self.p1[2]) - r,
119        ];
120        let hi = [
121            self.p0[0].max(self.p1[0]) + r,
122            self.p0[1].max(self.p1[1]) + r,
123            self.p0[2].max(self.p1[2]) + r,
124        ];
125        (lo, hi)
126    }
127}
oxiphysics_collision/query/querycapsule_traits.rs

oxiphysics_collision/query/
querycapsule_traits.rs
