From 31e03ea91e69ab30fdb0395cb13bc01174924b02 Mon Sep 17 00:00:00 2001 From: Joonatan Saarhelo Date: Mon, 7 Sep 2026 19:40:06 +0200 Subject: [PATCH 1/6] first version and solid tests --- src/query/ray/mod.rs | 1 + src/query/ray/ray_capsule.rs | 348 +++++++++++++++++++++++++++++++ src/query/ray/ray_support_map.rs | 19 +- 3 files changed, 350 insertions(+), 18 deletions(-) create mode 100644 src/query/ray/ray_capsule.rs diff --git a/src/query/ray/mod.rs b/src/query/ray/mod.rs index 543dcae7..feccf6c9 100644 --- a/src/query/ray/mod.rs +++ b/src/query/ray/mod.rs @@ -16,6 +16,7 @@ pub mod ray; mod ray_aabb; mod ray_ball; mod ray_bounding_sphere; +mod ray_capsule; #[cfg(feature = "alloc")] mod ray_composite_shape; mod ray_cuboid; diff --git a/src/query/ray/ray_capsule.rs b/src/query/ray/ray_capsule.rs new file mode 100644 index 00000000..f4fabd62 --- /dev/null +++ b/src/query/ray/ray_capsule.rs @@ -0,0 +1,348 @@ +use crate::math::Real; +use crate::query::{Ray, RayCast, RayIntersection}; +use crate::shape::{Capsule, FeatureId, Segment}; + +use num::Zero; + +impl RayCast for Capsule { + #[inline] + fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { + ray_toi_with_capsule(&self.segment, self.radius, ray, solid) + .1 + .filter(|time_of_impact| *time_of_impact <= max_time_of_impact) + } + + #[inline] + fn cast_local_ray_and_get_normal( + &self, + ray: &Ray, + max_time_of_impact: Real, + solid: bool, + ) -> Option { + ray_toi_and_normal_with_capsule(&self.segment, self.radius, ray, solid) + .filter(|inter| inter.time_of_impact <= max_time_of_impact) + } +} + +/// Computes the time of impact of a ray on a capsule. +/// Returns true if the ray started inside the capsule and the time of impact. +/// +/// Adapted from Inigo Quilez (https://iquilezles.org/articles/intersectors/), +/// extended for unnormalized directions, and an explicit axis-parallel special case +/// (the original depends on GLSL zero division behaviour). +/// The cap quadratics are built from the body's scalars +/// ("extend the quadratic", cf. PhysX's `Gu::intersectRayCapsule`). +#[inline] +fn ray_toi_with_capsule( + segment: &Segment, + radius: Real, + ray: &Ray, + solid: bool, +) -> (bool, Option) { + let r = radius; + let o = ray.origin; + let d = ray.dir; + let ba = segment.b - segment.a; + let oa = o - segment.a; + let l2 = ba.length_squared(); + let dd = d.length_squared(); + let bard = ba.dot(d); + let baoa = ba.dot(oa); + let rdoa = d.dot(oa); + let oaoa = oa.length_squared(); + let a = l2 * dd - bard * bard; + let b = l2 * rdoa - baoa * bard; + let c = l2 * oaoa - baoa * baoa - r * r * l2; + let h = b * b - a * c; + let axis_coord = |t: Real| baoa + t * bard; + + // The sphere of radius `r` around the cap center (segment.a or segment.b) + // as a quadratic in `t`, scaled by |d|^2 and built from the body's + // scalars. `root` = -1.0 is the entry, +1.0 the exit. + let cap_toi = |b_end: bool, root: Real| -> Option { + let b2 = if b_end { rdoa - bard } else { rdoa }; + let c2 = if b_end { + oaoa - 2.0 * baoa + l2 - r * r + } else { + oaoa - r * r + }; + let h2 = b2 * b2 - dd * c2; + (h2 >= 0.0) + .then(|| { + let t = (-b2 + root * h2.sqrt()) / dd; + (t >= 0.0).then_some(t) + }) + .flatten() + }; + + // Inside the capsule (division-free; the band test is scaled by l2). + let inside = oaoa <= r * r + || oaoa - 2.0 * baoa + l2 <= r * r + || (baoa > 0.0 && baoa < l2 && (oaoa - r * r) * l2 <= baoa * baoa); + + // A degenerate (zero-length) ray: contact iff the origin is inside. + if dd.is_zero() { + return (inside, inside.then_some(0.0)); + } + + if inside { + if solid { + // Contact at the origin. + return (true, Some(0.0)); + } + // Hollow: the exit, i.e. the latest boundary crossing. + let mut best: Option = None; + if a > 0.0 && h >= 0.0 { + let t = (-b + h.sqrt()) / a; + let y = axis_coord(t); + if y > 0.0 && y < l2 { + best = Some(t); + } + } + for b_end in [false, true] { + if let Some(t) = cap_toi(b_end, 1.0) { + let y = axis_coord(t); + let valid = (b_end && y >= l2) || (!b_end && y <= 0.0); + if valid && best.is_none_or(|x| t > x) { + best = Some(t); + } + } + } + return (true, best); + } + + // Outside: the first contact. + if a > 0.0 { + if h >= 0.0 { + let t = (-b - h.sqrt()) / a; + let y = axis_coord(t); + if y > 0.0 && y < l2 && t >= 0.0 { + return (false, Some(t)); + } + // The cap on the side the (possibly phantom) root points to. + return (false, cap_toi(y > 0.0, -1.0)); + } + // The closest approach to the axis stays beyond r, and so do the caps. + return (false, None); + } + // Ray parallel to the axis: only the cap on the current side is reachable. + if baoa <= 0.0 { + (false, cap_toi(false, -1.0)) + } else if baoa >= l2 { + (false, cap_toi(true, -1.0)) + } else { + (false, None) + } +} + +/// Computes the time of impact and contact normal of a ray on a capsule. +fn ray_toi_and_normal_with_capsule( + segment: &Segment, + radius: Real, + ray: &Ray, + solid: bool, +) -> Option { + let (inside, inter) = ray_toi_with_capsule(segment, radius, ray, solid); + + inter.map(|t| { + let o = ray.origin; + let d = ray.dir; + let ba = segment.b - segment.a; + let l2 = ba.length_squared(); + + let n = if d.length_squared().is_zero() { + // Degenerate zero-length ray: toward the closest axis point. + let s = if l2 > 0.0 { + (ba.dot(o - segment.a) / l2).clamp(0.0, 1.0) + } else { + 0.0 + }; + (segment.a + ba * s - o).normalize() + } else if solid && t.is_zero() { + // Contact at the origin: normal opposing the ray. + (-d).normalize() + } else { + let p = o + d * t; + let y = ba.dot(p - segment.a); + let normal = if y > 0.0 && y < l2 { + (p - segment.a - ba * (y / l2)).normalize() + } else if y <= 0.0 { + (p - segment.a).normalize() + } else { + (p - segment.b).normalize() + }; + if inside { + // Hollow: exit, inward normal. + -normal + } else { + normal + } + }; + + RayIntersection::new(t, n, FeatureId::Face(0)) + }) +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::math::Vector; + use crate::query::point::point_query::PointQuery; + use oorandom::Rand32; + + #[test] + fn exact_cases() { + let c = Capsule::new(v2(0.0, 0.5), v2(0.0, 1.5), 0.5); + // Hit straight down the axis on the top cap, unnormalized direction. + expect_hit(&c, v2(0.0, 5.0), v2(0.0, -0.2), true, 15.0, v2(0.0, 1.0)); + // Oblique hit on the cylinder. + expect_hit(&c, v2(5.0, 1.0), v2(-0.3, 0.0), true, 15.0, v2(1.0, 0.0)); + // Tangential hit at the tip of the top cap. + expect_hit(&c, v2(5.0, 2.0), v2(-1.0, 0.0), true, 5.0, v2(0.0, 1.0)); + // Hit on the bottom cap from below, parallel to the axis but offset + // from it. + expect_hit( + &c, + v2(0.1, -4.0), + v2(0.0, 0.2), + true, + 20.0505, + v2(0.2, -0.9798), + ); + // Lateral miss. + assert!(c + .cast_local_ray(&Ray::new(v2(10.0, 5.0), v2(0.0, 0.1)), 50.0, true) + .is_none()); + // Inside, solid: contact at the origin, normal opposing the ray. + expect_hit(&c, v2(0.0, 1.0), v2(0.0, 1.0), true, 0.0, v2(0.0, -1.0)); + // Inside, hollow: the exit, inward normal. + expect_hit(&c, v2(0.0, 1.0), v2(0.0, 1.0), false, 1.0, v2(0.0, -1.0)); + // Degenerate zero-length ray, inside / outside. + expect_hit(&c, v2(0.1, 1.0), v2(0.0, 0.0), true, 0.0, v2(-1.0, 0.0)); + assert!(c + .cast_local_ray(&Ray::new(v2(0.1, 3.0), v2(0.0, 0.0)), 50.0, true) + .is_none()); + // max_toi filtering (the top-cap hit above is at t = 15). + assert!(c + .cast_local_ray(&Ray::new(v2(0.0, 5.0), v2(0.0, -0.2)), 14.9, true) + .is_none()); + assert!(c + .cast_local_ray(&Ray::new(v2(0.0, 5.0), v2(0.0, -0.2)), 15.1, true) + .is_some()); + assert!(c + .cast_local_ray_and_get_normal(&Ray::new(v2(0.0, 5.0), v2(0.0, -0.2)), 14.9, true) + .is_none()); + } + + fn v2(x: Real, y: Real) -> Vector { + Vector::new( + x, + y, + #[cfg(feature = "dim3")] + 0.0, + ) + } + + fn expect_hit(c: &Capsule, o: Vector, d: Vector, solid: bool, et: Real, en: Vector) { + let i = c + .cast_local_ray_and_get_normal(&Ray::new(o, d), 50.0, solid) + .unwrap_or_else(|| panic!("expected hit (o={:?}, d={:?})", o, d)); + assert!( + (i.time_of_impact - et).abs() < 1e-4, + "t: got {}, want {}", + i.time_of_impact, + et + ); + assert!( + (i.normal - en).length() < 1e-3, + "n: got {:?}, want {:?}", + i.normal, + en + ); + } + + #[test] + fn fuzz_capsule_ray_casts() { + let epsilon = 0.003; + let mut rng = Rand32::new(42); + + for _ in 0..100_000 { + let (a, b) = (rnd_vec(&mut rng, 10.0), rnd_vec(&mut rng, 10.0)); + let r = 0.5 + 5.0 * rnd(&mut rng); + let capsule = Capsule::new(a, b, r); + + // a random point inside the capsule + let inside = { + let mut w = rnd_vec(&mut rng, r); + while w.length_squared() >= r * r { + w = rnd_vec(&mut rng, r); + } + a + (b - a) * rnd(&mut rng) + w + }; + + // cast random ray toward the inside point + let far_enough = r + a.distance(b); + let mut offset = Vector::ZERO; + while offset.length_squared() < far_enough * far_enough { + offset = rnd_vec(&mut rng, far_enough * 2.0); + } + + let o = (a + b) * 0.5 + offset; + let d = (inside - o) * (0.1 + 0.9 * rnd(&mut rng)); + let i = capsule + .cast_local_ray_and_get_normal(&Ray::new(o, d), 1000.0, true) + .expect("a ray aimed at an interior point must hit"); + + let hit = o + d * i.time_of_impact; + assert!( + capsule.contains_local_point(hit - i.normal * epsilon), + "nudging inward along the normal should go inside the capsule" + ); + assert!( + !capsule.contains_local_point(hit + i.normal * epsilon), + "nudging outward along the normal should go outside the capsule" + ); + + #[cfg(feature = "dim2")] + let tangent = Vector::new(-i.normal.y, i.normal.x); + #[cfg(feature = "dim3")] + let tangent = { + let mut tangent = Vector::ZERO; + while tangent.length_squared() < 1e-8 { + tangent = rnd_vec(&mut rng, 1.0).cross(i.normal); + } + tangent + }; + let origin = hit + i.normal * (epsilon + rnd(&mut rng)) - rnd(&mut rng) * tangent; + assert!( + capsule + .cast_local_ray(&Ray::new(origin, tangent), 1000.0, true) + .is_none(), + "tangent outside the capsule should miss" + ); + } + } + + fn rnd(rng: &mut Rand32) -> Real { + #[cfg(feature = "f32")] + { + rng.rand_float() + } + #[cfg(feature = "f64")] + { + rng.rand_float() as Real + } + } + + fn rnd_vec(rng: &mut Rand32, scale: Real) -> Vector { + let mut component = || (rnd(rng) - 0.5) * 2.0 * scale; + #[cfg(feature = "dim2")] + { + Vector::new(component(), component()) + } + #[cfg(feature = "dim3")] + { + Vector::new(component(), component(), component()) + } + } +} diff --git a/src/query/ray/ray_support_map.rs b/src/query/ray/ray_support_map.rs index 64a89c1a..033320f5 100644 --- a/src/query/ray/ray_support_map.rs +++ b/src/query/ray/ray_support_map.rs @@ -7,9 +7,9 @@ use crate::query::{Ray, RayCast, RayIntersection}; use crate::shape::ConvexPolygon; #[cfg(all(feature = "alloc", feature = "dim3"))] use crate::shape::ConvexPolyhedron; -use crate::shape::{Capsule, FeatureId, Segment, SupportMap}; #[cfg(feature = "dim3")] use crate::shape::{Cone, Cylinder}; +use crate::shape::{FeatureId, Segment, SupportMap}; use num::Zero; @@ -104,23 +104,6 @@ impl RayCast for Cone { } } -impl RayCast for Capsule { - fn cast_local_ray_and_get_normal( - &self, - ray: &Ray, - max_time_of_impact: Real, - solid: bool, - ) -> Option { - local_ray_intersection_with_support_map_with_params( - self, - &mut VoronoiSimplex::new(), - ray, - max_time_of_impact, - solid, - ) - } -} - #[cfg(feature = "dim3")] #[cfg(feature = "alloc")] impl RayCast for ConvexPolyhedron { From 5b1a7cf9432d8c633afa811654a850eebc563d6b Mon Sep 17 00:00:00 2001 From: Joonatan Saarhelo Date: Mon, 7 Sep 2026 22:43:56 +0200 Subject: [PATCH 2/6] deslopify normal computation --- src/query/ray/ray_capsule.rs | 49 +++++++++++------------------------- 1 file changed, 15 insertions(+), 34 deletions(-) diff --git a/src/query/ray/ray_capsule.rs b/src/query/ray/ray_capsule.rs index f4fabd62..a023c402 100644 --- a/src/query/ray/ray_capsule.rs +++ b/src/query/ray/ray_capsule.rs @@ -145,41 +145,22 @@ fn ray_toi_and_normal_with_capsule( let (inside, inter) = ray_toi_with_capsule(segment, radius, ray, solid); inter.map(|t| { - let o = ray.origin; - let d = ray.dir; - let ba = segment.b - segment.a; - let l2 = ba.length_squared(); + let p = ray.origin + ray.dir * t; + let a_to_p = p - segment.a; + let seg = segment.b - segment.a; + let seg_squared = seg.length_squared(); - let n = if d.length_squared().is_zero() { - // Degenerate zero-length ray: toward the closest axis point. - let s = if l2 > 0.0 { - (ba.dot(o - segment.a) / l2).clamp(0.0, 1.0) - } else { - 0.0 - }; - (segment.a + ba * s - o).normalize() - } else if solid && t.is_zero() { - // Contact at the origin: normal opposing the ray. - (-d).normalize() + // the projection of the point onto the capsule's axis times the segment's length + let proj_times_seg = a_to_p.dot(seg); + + let normal = if proj_times_seg <= 0.0 { + (a_to_p).normalize() + } else if proj_times_seg >= seg_squared { + (p - segment.b).normalize() } else { - let p = o + d * t; - let y = ba.dot(p - segment.a); - let normal = if y > 0.0 && y < l2 { - (p - segment.a - ba * (y / l2)).normalize() - } else if y <= 0.0 { - (p - segment.a).normalize() - } else { - (p - segment.b).normalize() - }; - if inside { - // Hollow: exit, inward normal. - -normal - } else { - normal - } + (a_to_p - (proj_times_seg / seg_squared) * seg).normalize() }; - - RayIntersection::new(t, n, FeatureId::Face(0)) + RayIntersection::new(t, if inside { -normal } else { normal }, FeatureId::Face(0)) }) } @@ -213,8 +194,8 @@ mod tests { assert!(c .cast_local_ray(&Ray::new(v2(10.0, 5.0), v2(0.0, 0.1)), 50.0, true) .is_none()); - // Inside, solid: contact at the origin, normal opposing the ray. - expect_hit(&c, v2(0.0, 1.0), v2(0.0, 1.0), true, 0.0, v2(0.0, -1.0)); + // Inside, solid: contact at the origin, inward radial normal. + expect_hit(&c, v2(0.1, 1.0), v2(0.0, 1.0), true, 0.0, v2(-1.0, 0.0)); // Inside, hollow: the exit, inward normal. expect_hit(&c, v2(0.0, 1.0), v2(0.0, 1.0), false, 1.0, v2(0.0, -1.0)); // Degenerate zero-length ray, inside / outside. From 59d966ca8e3c7ba136e462014ece9e7c26a95868 Mon Sep 17 00:00:00 2001 From: Joonatan Saarhelo Date: Tue, 8 Sep 2026 13:00:23 +0200 Subject: [PATCH 3/6] good but missing inside --- src/query/ray/ray_capsule.rs | 152 ++++++++++++++++++++++++++++++----- 1 file changed, 132 insertions(+), 20 deletions(-) diff --git a/src/query/ray/ray_capsule.rs b/src/query/ray/ray_capsule.rs index a023c402..0552ab14 100644 --- a/src/query/ray/ray_capsule.rs +++ b/src/query/ray/ray_capsule.rs @@ -1,4 +1,4 @@ -use crate::math::Real; +use crate::math::{Real, Vector}; use crate::query::{Ray, RayCast, RayIntersection}; use crate::shape::{Capsule, FeatureId, Segment}; @@ -33,7 +33,7 @@ impl RayCast for Capsule { /// The cap quadratics are built from the body's scalars /// ("extend the quadratic", cf. PhysX's `Gu::intersectRayCapsule`). #[inline] -fn ray_toi_with_capsule( +fn ray_toi_with_capsule_ai( segment: &Segment, radius: Real, ray: &Ray, @@ -135,6 +135,80 @@ fn ray_toi_with_capsule( } } +/// Changed to compute with any ray without normalizing. +/// +fn ray_toi_with_capsule( + segment: &Segment, + radius: Real, + ray: &Ray, + solid: bool, +) -> (bool, Option) { + let ab = segment.b - segment.a; + let ao = ray.origin - segment.a; + + let ab_ab = ab.length_squared(); + let dir_dir = ray.dir.length_squared(); + let ab_dir = ab.dot(ray.dir); + let ab_ao = ab.dot(ao); + + // do a circle intersection on the plane perpendicular to the capsule's axis. + // all these variables are scaled by ab^2 + let dir_on_plane = cross(ray.dir, ab); + let origin_on_plane = cross(ao, ab); + let ray_step = dir_on_plane.length_squared(); + let b = dir_on_plane.dot(origin_on_plane); + let separation = origin_on_plane.length_squared() - radius * radius * ab_ab; + let h = diff_of_products(b, b, ray_step, separation); + + if h >= 0.0 { + let t = (-b - h.sqrt()) / ray_step; + let y = ab_ao + t * ab_dir; + // body + if 0.0 < y && y < ab_ab && t >= 0.0 { + return (false, Some(t)); + } + // caps + // y = NaN means the ray is parallel to the capsule, + // so it can only hit one of the caps + let oc = if y <= 0.0 || y.is_nan() && ab_dir > 0.0 { + ao + } else { + ray.origin - segment.b + }; + let b = ray.dir.dot(oc); + let c = oc.length_squared() - radius * radius; + let h = diff_of_products(b, b, c, dir_dir); + let t = -b - h.sqrt(); + if h >= 0.0 && t >= 0.0 { + return (false, Some(t / dir_dir)); + } + } + return (false, None); +} + +/// Computes ab - cd accurately via Kahan's algorithm +#[inline] +fn diff_of_products(a: Real, b: Real, c: Real, d: Real) -> Real { + let cd = c * d; + let diff = a.mul_add(b, -cd); + let error = (-c).mul_add(d, cd); + diff + error +} + +#[cfg(feature = "dim3")] +#[inline] +fn cross(v: Vector, segment: Vector) -> Vector { + v.cross(segment) +} + +/// Returns a vector with zero y, which is complete nonsense +/// but makes the 2D case work with the same code as the 3D case. +#[cfg(feature = "dim2")] +#[inline] +fn cross(v: Vector, segment: Vector) -> Vector { + Vector::new(v.x * segment.y - v.y * segment.x, 0.0) +} + /// Computes the time of impact and contact normal of a ray on a capsule. fn ray_toi_and_normal_with_capsule( segment: &Segment, @@ -145,22 +219,31 @@ fn ray_toi_and_normal_with_capsule( let (inside, inter) = ray_toi_with_capsule(segment, radius, ray, solid); inter.map(|t| { - let p = ray.origin + ray.dir * t; - let a_to_p = p - segment.a; - let seg = segment.b - segment.a; - let seg_squared = seg.length_squared(); - - // the projection of the point onto the capsule's axis times the segment's length - let proj_times_seg = a_to_p.dot(seg); - - let normal = if proj_times_seg <= 0.0 { - (a_to_p).normalize() - } else if proj_times_seg >= seg_squared { - (p - segment.b).normalize() + let normal = if solid && inside { + Vector::ZERO } else { - (a_to_p - (proj_times_seg / seg_squared) * seg).normalize() + let p = ray.origin + ray.dir * t; + let a_to_p = p - segment.a; + let seg = segment.b - segment.a; + let seg_squared = seg.length_squared(); + + // the projection of the point onto the capsule's axis times the segment's length + let proj_times_seg = a_to_p.dot(seg); + + let n = if proj_times_seg <= 0.0 { + (a_to_p).normalize() + } else if proj_times_seg >= seg_squared { + (p - segment.b).normalize() + } else { + (a_to_p - (proj_times_seg / seg_squared) * seg).normalize() + }; + if inside { + -n + } else { + n + } }; - RayIntersection::new(t, if inside { -normal } else { normal }, FeatureId::Face(0)) + RayIntersection::new(t, normal, FeatureId::Face(0)) }) } @@ -194,12 +277,40 @@ mod tests { assert!(c .cast_local_ray(&Ray::new(v2(10.0, 5.0), v2(0.0, 0.1)), 50.0, true) .is_none()); + + // Outside-origin misses where the ray's line crosses the tube (or a cap + // sphere) only BEHIND the origin: the entry root is negative and must + // not be reported. The fuzz can't catch these (it only aims rays at + // interior points, so its entries are always positive). + // Inside the infinite tube past the b-cap, receding. The tube entry is + // behind (t = -0.9) with a phantom axis coordinate inside the band. + assert!(c + .cast_local_ray(&Ray::new(v2(0.4, 1.85), v2(1.0, 1.0)), 50.0, true) + .is_none()); + // On-axis past the b-cap, parallel, receding (phantom t = -2.1). + assert!(c + .cast_local_ray(&Ray::new(v2(0.0, 2.1), v2(0.0, 1.0)), 50.0, true) + .is_none()); + // Inside the tube past the b-cap, receding at an angle. Phantom axis + // coordinate beyond the slab, cap entry behind (t = -0.87). + assert!(c + .cast_local_ray(&Ray::new(v2(0.4, 1.85), v2(1.0, 0.2)), 50.0, true) + .is_none()); + // Outside everything, receding. The line crosses the tube behind the + // origin, phantom axis coordinate in the band (t = -2.5). + assert!(c + .cast_local_ray(&Ray::new(v2(2.0, 1.0), v2(1.0, 0.0)), 50.0, true) + .is_none()); + // Same, with the phantom axis coordinate exactly on the a-end boundary. + assert!(c + .cast_local_ray(&Ray::new(v2(2.0, 3.0), v2(1.0, 1.0)), 50.0, true) + .is_none()); // Inside, solid: contact at the origin, inward radial normal. - expect_hit(&c, v2(0.1, 1.0), v2(0.0, 1.0), true, 0.0, v2(-1.0, 0.0)); + // TODO expect_hit(&c, v2(0.1, 1.0), v2(0.0, 1.0), true, 0.0, v2(-1.0, 0.0)); // Inside, hollow: the exit, inward normal. - expect_hit(&c, v2(0.0, 1.0), v2(0.0, 1.0), false, 1.0, v2(0.0, -1.0)); + //expect_hit(&c, v2(0.0, 1.0), v2(0.0, 1.0), false, 1.0, v2(0.0, -1.0)); // Degenerate zero-length ray, inside / outside. - expect_hit(&c, v2(0.1, 1.0), v2(0.0, 0.0), true, 0.0, v2(-1.0, 0.0)); + //expect_hit(&c, v2(0.1, 1.0), v2(0.0, 0.0), true, 0.0, v2(-1.0, 0.0)); assert!(c .cast_local_ray(&Ray::new(v2(0.1, 3.0), v2(0.0, 0.0)), 50.0, true) .is_none()); @@ -224,6 +335,7 @@ mod tests { ) } + #[track_caller] fn expect_hit(c: &Capsule, o: Vector, d: Vector, solid: bool, et: Real, en: Vector) { let i = c .cast_local_ray_and_get_normal(&Ray::new(o, d), 50.0, solid) @@ -244,7 +356,7 @@ mod tests { #[test] fn fuzz_capsule_ray_casts() { - let epsilon = 0.003; + let epsilon = 0.002; let mut rng = Rand32::new(42); for _ in 0..100_000 { From 2476394ff8ccf78945e0882ddd9991f4efa7d0e0 Mon Sep 17 00:00:00 2001 From: Joonatan Saarhelo Date: Tue, 8 Sep 2026 18:36:55 +0200 Subject: [PATCH 4/6] hand-authored clean version --- src/query/ray/ray_capsule.rs | 223 +++++++++++++---------------------- 1 file changed, 81 insertions(+), 142 deletions(-) diff --git a/src/query/ray/ray_capsule.rs b/src/query/ray/ray_capsule.rs index 0552ab14..6f500ab1 100644 --- a/src/query/ray/ray_capsule.rs +++ b/src/query/ray/ray_capsule.rs @@ -2,8 +2,6 @@ use crate::math::{Real, Vector}; use crate::query::{Ray, RayCast, RayIntersection}; use crate::shape::{Capsule, FeatureId, Segment}; -use num::Zero; - impl RayCast for Capsule { #[inline] fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option { @@ -27,116 +25,11 @@ impl RayCast for Capsule { /// Computes the time of impact of a ray on a capsule. /// Returns true if the ray started inside the capsule and the time of impact. /// -/// Adapted from Inigo Quilez (https://iquilezles.org/articles/intersectors/), -/// extended for unnormalized directions, and an explicit axis-parallel special case -/// (the original depends on GLSL zero division behaviour). -/// The cap quadratics are built from the body's scalars -/// ("extend the quadratic", cf. PhysX's `Gu::intersectRayCapsule`). -#[inline] -fn ray_toi_with_capsule_ai( - segment: &Segment, - radius: Real, - ray: &Ray, - solid: bool, -) -> (bool, Option) { - let r = radius; - let o = ray.origin; - let d = ray.dir; - let ba = segment.b - segment.a; - let oa = o - segment.a; - let l2 = ba.length_squared(); - let dd = d.length_squared(); - let bard = ba.dot(d); - let baoa = ba.dot(oa); - let rdoa = d.dot(oa); - let oaoa = oa.length_squared(); - let a = l2 * dd - bard * bard; - let b = l2 * rdoa - baoa * bard; - let c = l2 * oaoa - baoa * baoa - r * r * l2; - let h = b * b - a * c; - let axis_coord = |t: Real| baoa + t * bard; - - // The sphere of radius `r` around the cap center (segment.a or segment.b) - // as a quadratic in `t`, scaled by |d|^2 and built from the body's - // scalars. `root` = -1.0 is the entry, +1.0 the exit. - let cap_toi = |b_end: bool, root: Real| -> Option { - let b2 = if b_end { rdoa - bard } else { rdoa }; - let c2 = if b_end { - oaoa - 2.0 * baoa + l2 - r * r - } else { - oaoa - r * r - }; - let h2 = b2 * b2 - dd * c2; - (h2 >= 0.0) - .then(|| { - let t = (-b2 + root * h2.sqrt()) / dd; - (t >= 0.0).then_some(t) - }) - .flatten() - }; - - // Inside the capsule (division-free; the band test is scaled by l2). - let inside = oaoa <= r * r - || oaoa - 2.0 * baoa + l2 <= r * r - || (baoa > 0.0 && baoa < l2 && (oaoa - r * r) * l2 <= baoa * baoa); - - // A degenerate (zero-length) ray: contact iff the origin is inside. - if dd.is_zero() { - return (inside, inside.then_some(0.0)); - } - - if inside { - if solid { - // Contact at the origin. - return (true, Some(0.0)); - } - // Hollow: the exit, i.e. the latest boundary crossing. - let mut best: Option = None; - if a > 0.0 && h >= 0.0 { - let t = (-b + h.sqrt()) / a; - let y = axis_coord(t); - if y > 0.0 && y < l2 { - best = Some(t); - } - } - for b_end in [false, true] { - if let Some(t) = cap_toi(b_end, 1.0) { - let y = axis_coord(t); - let valid = (b_end && y >= l2) || (!b_end && y <= 0.0); - if valid && best.is_none_or(|x| t > x) { - best = Some(t); - } - } - } - return (true, best); - } - - // Outside: the first contact. - if a > 0.0 { - if h >= 0.0 { - let t = (-b - h.sqrt()) / a; - let y = axis_coord(t); - if y > 0.0 && y < l2 && t >= 0.0 { - return (false, Some(t)); - } - // The cap on the side the (possibly phantom) root points to. - return (false, cap_toi(y > 0.0, -1.0)); - } - // The closest approach to the axis stays beyond r, and so do the caps. - return (false, None); - } - // Ray parallel to the axis: only the cap on the current side is reachable. - if baoa <= 0.0 { - (false, cap_toi(false, -1.0)) - } else if baoa >= l2 { - (false, cap_toi(true, -1.0)) - } else { - (false, None) - } -} - -/// Changed to compute with any ray without normalizing. -/// +/// Adapted from Inigo Quilez (https://iquilezles.org/articles/intersectors/). +/// Adapted to unnormalized ray direction. +/// Made robust to degenerate cases and ray origin inside the capsule. +/// Switched to projecting onto the plane with cross products +/// because they introduce much less error than a difference of dot products. fn ray_toi_with_capsule( segment: &Segment, radius: Real, @@ -150,49 +43,58 @@ fn ray_toi_with_capsule( let dir_dir = ray.dir.length_squared(); let ab_dir = ab.dot(ray.dir); let ab_ao = ab.dot(ao); + let radius_squared = radius * radius; // do a circle intersection on the plane perpendicular to the capsule's axis. - // all these variables are scaled by ab^2 + // all these variables are scaled by ab let dir_on_plane = cross(ray.dir, ab); let origin_on_plane = cross(ao, ab); let ray_step = dir_on_plane.length_squared(); let b = dir_on_plane.dot(origin_on_plane); - let separation = origin_on_plane.length_squared() - radius * radius * ab_ab; - let h = diff_of_products(b, b, ray_step, separation); + let separation = origin_on_plane.length_squared() - radius_squared * ab_ab; + let h = b * b - ray_step * separation; + + let inside = separation <= 0.0 + && (0.0 < ab_ao || ao.length_squared() <= radius_squared) + && (ab_ao < ab_ab || (ray.origin - segment.b).length_squared() <= radius_squared); + + if inside && solid { + return (true, Some(0.0)); + } if h >= 0.0 { - let t = (-b - h.sqrt()) / ray_step; - let y = ab_ao + t * ab_dir; - // body - if 0.0 < y && y < ab_ab && t >= 0.0 { - return (false, Some(t)); - } + let check_sphere_a = if ray_step == 0.0 { + // the ray is parallel to the capsule, + // so it can only hit one of the caps + (ab_dir > 0.0) ^ inside + } else { + // cylinder part + // when outside, take the first intersection, when inside, the second + let radical = h.sqrt(); + let t = (-b + if inside { radical } else { -radical }) / ray_step; + let y = ab_ao + t * ab_dir; + if 0.0 < y && y < ab_ab && t >= 0.0 { + return (inside, Some(t)); + } + y <= 0.0 + }; + // caps - // y = NaN means the ray is parallel to the capsule, - // so it can only hit one of the caps - let oc = if y <= 0.0 || y.is_nan() && ab_dir > 0.0 { + let oc = if check_sphere_a { ao } else { ray.origin - segment.b }; let b = ray.dir.dot(oc); - let c = oc.length_squared() - radius * radius; - let h = diff_of_products(b, b, c, dir_dir); - let t = -b - h.sqrt(); + let c = oc.length_squared() - radius_squared; + let h = b * b - c * dir_dir; + let radical = h.sqrt(); + let t = -b + if inside { radical } else { -radical }; if h >= 0.0 && t >= 0.0 { - return (false, Some(t / dir_dir)); + return (inside, Some(t / dir_dir)); } } - return (false, None); -} - -/// Computes ab - cd accurately via Kahan's algorithm -#[inline] -fn diff_of_products(a: Real, b: Real, c: Real, d: Real) -> Real { - let cd = c * d; - let diff = a.mul_add(b, -cd); - let error = (-c).mul_add(d, cd); - diff + error + return (inside, None); } #[cfg(feature = "dim3")] @@ -305,15 +207,30 @@ mod tests { assert!(c .cast_local_ray(&Ray::new(v2(2.0, 3.0), v2(1.0, 1.0)), 50.0, true) .is_none()); - // Inside, solid: contact at the origin, inward radial normal. - // TODO expect_hit(&c, v2(0.1, 1.0), v2(0.0, 1.0), true, 0.0, v2(-1.0, 0.0)); + // Inside, solid: contact at the origin, zero normal. + expect_hit(&c, v2(0.1, 1.0), v2(0.0, 1.0), true, 0.0, v2(0.0, 0.0)); // Inside, hollow: the exit, inward normal. - //expect_hit(&c, v2(0.0, 1.0), v2(0.0, 1.0), false, 1.0, v2(0.0, -1.0)); + expect_hit(&c, v2(0.0, 1.0), v2(0.0, 1.0), false, 1.0, v2(0.0, -1.0)); + // Same, toward the a-cap (the other parallel routing branch). + expect_hit(&c, v2(0.0, 1.0), v2(0.0, -1.0), false, 1.0, v2(0.0, 1.0)); + // Inside, hollow: exit through the cylinder's side (the band exit + // root, not a cap). + expect_hit(&c, v2(0.0, 1.0), v2(1.0, 0.0), false, 0.5, v2(-1.0, 0.0)); + // Same, oblique. + expect_hit(&c, v2(0.1, 0.8), v2(1.0, 0.5), false, 0.4, v2(-1.0, 0.0)); + // Inside the b-cap sphere past the slab: the b-sphere exit (t = 0.6) + // is an intermediate crossing and must be skipped in favor of the + // last one (the a-sphere exit at t = 1.6). + expect_hit(&c, v2(0.0, 1.6), v2(0.0, -1.0), false, 1.6, v2(0.0, 1.0)); // Degenerate zero-length ray, inside / outside. - //expect_hit(&c, v2(0.1, 1.0), v2(0.0, 0.0), true, 0.0, v2(-1.0, 0.0)); + expect_hit(&c, v2(0.1, 1.0), v2(0.0, 0.0), true, 0.0, v2(0.0, 0.0)); assert!(c .cast_local_ray(&Ray::new(v2(0.1, 3.0), v2(0.0, 0.0)), 50.0, true) .is_none()); + // Degenerate capsule (a == b): behaves as a ball of radius 1 at (0, 1). + let ball = Capsule::new(v2(0.0, 1.0), v2(0.0, 1.0), 1.0); + expect_hit(&ball, v2(0.0, 5.0), v2(0.0, -1.0), true, 3.0, v2(0.0, 1.0)); + expect_hit(&ball, v2(0.5, 1.0), v2(1.0, 0.0), true, 0.0, v2(0.0, 0.0)); // max_toi filtering (the top-cap hit above is at t = 15). assert!(c .cast_local_ray(&Ray::new(v2(0.0, 5.0), v2(0.0, -0.2)), 14.9, true) @@ -396,6 +313,28 @@ mod tests { "nudging outward along the normal should go outside the capsule" ); + // A ray from the interior point to the far point (hollow) must + // exit the capsule; its normal points inward. + let i_in = capsule + .cast_local_ray_and_get_normal(&Ray::new(inside, o - inside), 1000.0, false) + .expect("a ray from inside toward the outside must exit"); + let hit_in = inside + (o - inside) * i_in.time_of_impact; + assert!( + capsule.contains_local_point(hit_in + i_in.normal * epsilon), + "nudging along the inward normal should stay inside" + ); + assert!( + !capsule.contains_local_point(hit_in - i_in.normal * epsilon), + "nudging against the inward normal should go outside" + ); + + assert!( + capsule + .cast_local_ray(&Ray::new(o, -d), 1000.0, true) + .is_none(), + "a retreating ray must miss" + ); + #[cfg(feature = "dim2")] let tangent = Vector::new(-i.normal.y, i.normal.x); #[cfg(feature = "dim3")] From 6f8e077cf32f86fa1f77c96ac7503991c52602aa Mon Sep 17 00:00:00 2001 From: Joonatan Saarhelo Date: Tue, 8 Sep 2026 18:43:03 +0200 Subject: [PATCH 5/6] avoid all NaNs --- src/query/ray/ray_capsule.rs | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/query/ray/ray_capsule.rs b/src/query/ray/ray_capsule.rs index 6f500ab1..e7749f1b 100644 --- a/src/query/ray/ray_capsule.rs +++ b/src/query/ray/ray_capsule.rs @@ -88,10 +88,12 @@ fn ray_toi_with_capsule( let b = ray.dir.dot(oc); let c = oc.length_squared() - radius_squared; let h = b * b - c * dir_dir; - let radical = h.sqrt(); - let t = -b + if inside { radical } else { -radical }; - if h >= 0.0 && t >= 0.0 { - return (inside, Some(t / dir_dir)); + if h >= 0.0 { + let radical = h.sqrt(); + let t = -b + if inside { radical } else { -radical }; + if t >= 0.0 && dir_dir != 0.0 { + return (inside, Some(t / dir_dir)); + } } } return (inside, None); From 1bda1f71e7c617773ba288030372b641180103fd Mon Sep 17 00:00:00 2001 From: Joonatan Saarhelo Date: Tue, 8 Sep 2026 18:47:37 +0200 Subject: [PATCH 6/6] make clippy happy --- src/query/ray/ray_capsule.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/query/ray/ray_capsule.rs b/src/query/ray/ray_capsule.rs index e7749f1b..63666713 100644 --- a/src/query/ray/ray_capsule.rs +++ b/src/query/ray/ray_capsule.rs @@ -96,7 +96,7 @@ fn ray_toi_with_capsule( } } } - return (inside, None); + (inside, None) } #[cfg(feature = "dim3")]