848e715c6c
Full engine port in libs/box3d: math, geometry, GJK/TOI, hull builder, dynamic tree, manifolds, constraint graph, solver (serial, scalar SIMD path), all 8 joint types, sensors, mover, world API. 147 ported C unit tests green in debug and release. See libs/box3d/README.md for the upstream revision and sync notes, PORTING.md for conventions. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
788 lines
33 KiB
Rust
788 lines
33 KiB
Rust
// Port of box3d/test/test_shape.c
|
|
|
|
use std::sync::Arc;
|
|
|
|
use makepad_box3d::capsule::{compute_capsule_aabb, compute_capsule_mass, ray_cast_capsule};
|
|
use makepad_box3d::hull::{compute_hull_aabb, compute_hull_mass, create_hull, make_box_hull, make_transformed_box_hull, ray_cast_hull};
|
|
use makepad_box3d::math_functions::*;
|
|
use makepad_box3d::sphere::{compute_sphere_aabb, compute_sphere_mass, ray_cast_sphere};
|
|
use makepad_box3d::types::{Capsule, CastOutput, HullData, RayCastInput, Sphere};
|
|
use makepad_box3d::{ensure, ensure_small};
|
|
|
|
const N: usize = 4;
|
|
|
|
fn test_capsule() -> Capsule {
|
|
Capsule { center1: vec3(-1.0, 0.0, 0.0), center2: vec3(1.0, 0.0, 0.0), radius: 1.0 }
|
|
}
|
|
|
|
fn test_sphere() -> Sphere {
|
|
Sphere { center: vec3(1.0, 0.0, 0.0), radius: 1.0 }
|
|
}
|
|
|
|
fn test_box() -> Arc<HullData> {
|
|
make_box_hull(1.0, 1.0, 1.0)
|
|
}
|
|
|
|
#[test]
|
|
fn shape_mass_test() {
|
|
let sphere = test_sphere();
|
|
let capsule = test_capsule();
|
|
let box_hull = test_box();
|
|
|
|
// Sphere
|
|
{
|
|
let md = compute_sphere_mass(&sphere, 1.0);
|
|
let mass = 4.0 / 3.0 * PI;
|
|
ensure_small!(md.mass - mass, f32::EPSILON);
|
|
ensure!(md.center.x == 1.0 && md.center.y == 0.0);
|
|
|
|
// Inertia is now about the shape center of mass, so the offset does not appear.
|
|
let inertia = 2.0 / 5.0 * mass;
|
|
ensure_small!(md.inertia.cx.x - inertia, f32::EPSILON);
|
|
ensure_small!(md.inertia.cy.y - inertia, f32::EPSILON);
|
|
ensure_small!(md.inertia.cz.z - inertia, f32::EPSILON);
|
|
}
|
|
|
|
// Analytic box hull
|
|
{
|
|
let md = compute_hull_mass(&box_hull, 1.0);
|
|
let mass = 2.0 * 2.0 * 2.0;
|
|
ensure_small!(md.mass - mass, f32::EPSILON);
|
|
ensure_small!(md.center.x, f32::EPSILON);
|
|
ensure_small!(md.center.y, f32::EPSILON);
|
|
ensure_small!(md.center.z, f32::EPSILON);
|
|
let inertia = (1.0 / 12.0) * mass * (2.0 * 2.0 + 2.0 * 2.0);
|
|
ensure_small!(md.inertia.cx.x - inertia, 2.0 * f32::EPSILON);
|
|
ensure_small!(md.inertia.cy.y - inertia, 2.0 * f32::EPSILON);
|
|
ensure_small!(md.inertia.cz.z - inertia, 2.0 * f32::EPSILON);
|
|
}
|
|
|
|
// Translated box
|
|
{
|
|
let offset = vec3(0.4, -0.7, 0.1);
|
|
let transform = Transform { p: offset, q: Quat::IDENTITY };
|
|
let h = vec3(0.25, 0.5, 0.3);
|
|
let b1 = make_box_hull(h.x, h.y, h.z);
|
|
let b2 = make_transformed_box_hull(h.x, h.y, h.z, transform);
|
|
|
|
let m1 = compute_hull_mass(&b1, 1.0);
|
|
let m2 = compute_hull_mass(&b2, 1.0);
|
|
|
|
ensure_small!(m1.mass - m2.mass, f32::EPSILON);
|
|
|
|
let d = sub_mm(b1.central_inertia, b2.central_inertia);
|
|
ensure_small!(d.cx.x, f32::EPSILON);
|
|
ensure_small!(d.cx.y, f32::EPSILON);
|
|
ensure_small!(d.cx.z, f32::EPSILON);
|
|
ensure_small!(d.cy.x, f32::EPSILON);
|
|
ensure_small!(d.cy.y, f32::EPSILON);
|
|
ensure_small!(d.cy.z, f32::EPSILON);
|
|
ensure_small!(d.cz.x, f32::EPSILON);
|
|
ensure_small!(d.cz.y, f32::EPSILON);
|
|
ensure_small!(d.cz.z, f32::EPSILON);
|
|
|
|
ensure_small!(m2.center.x - offset.x, f32::EPSILON);
|
|
ensure_small!(m2.center.y - offset.y, f32::EPSILON);
|
|
ensure_small!(m2.center.z - offset.z, f32::EPSILON);
|
|
}
|
|
|
|
// Rotated box
|
|
{
|
|
let h1 = vec3(0.25, 0.5, 0.3);
|
|
let h2 = vec3(0.25, 0.3, 0.5);
|
|
let q = compute_quat_between_unit_vectors(Vec3::AXIS_Y, Vec3::AXIS_Z);
|
|
let transform = Transform { p: Vec3::ZERO, q };
|
|
let b1 = make_transformed_box_hull(h1.x, h1.y, h1.z, transform);
|
|
let b2 = make_box_hull(h2.x, h2.y, h2.z);
|
|
|
|
let m1 = compute_hull_mass(&b1, 1.0);
|
|
let m2 = compute_hull_mass(&b2, 1.0);
|
|
|
|
ensure_small!(m1.mass - m2.mass, f32::EPSILON);
|
|
|
|
let d = sub_mm(b1.central_inertia, b2.central_inertia);
|
|
ensure_small!(d.cx.x, f32::EPSILON);
|
|
ensure_small!(d.cx.y, f32::EPSILON);
|
|
ensure_small!(d.cx.z, f32::EPSILON);
|
|
ensure_small!(d.cy.x, f32::EPSILON);
|
|
ensure_small!(d.cy.y, f32::EPSILON);
|
|
ensure_small!(d.cy.z, f32::EPSILON);
|
|
ensure_small!(d.cz.x, f32::EPSILON);
|
|
ensure_small!(d.cz.y, f32::EPSILON);
|
|
ensure_small!(d.cz.z, f32::EPSILON);
|
|
|
|
ensure_small!(m1.center.x - m2.center.x, f32::EPSILON);
|
|
ensure_small!(m1.center.y - m2.center.y, f32::EPSILON);
|
|
ensure_small!(m1.center.z - m2.center.z, f32::EPSILON);
|
|
}
|
|
|
|
// Transformed box
|
|
{
|
|
let offset = vec3(0.4, -0.7, 0.1);
|
|
let h1 = vec3(0.25, 0.5, 0.3);
|
|
let h2 = vec3(0.25, 0.3, 0.5);
|
|
let q = compute_quat_between_unit_vectors(Vec3::AXIS_Y, Vec3::AXIS_Z);
|
|
let transform = Transform { p: offset, q };
|
|
let b1 = make_transformed_box_hull(h1.x, h1.y, h1.z, transform);
|
|
let b2 = make_box_hull(h2.x, h2.y, h2.z);
|
|
|
|
let m1 = compute_hull_mass(&b1, 1.0);
|
|
let m2 = compute_hull_mass(&b2, 1.0);
|
|
|
|
ensure_small!(m1.mass - m2.mass, f32::EPSILON);
|
|
|
|
let d = sub_mm(b1.central_inertia, b2.central_inertia);
|
|
ensure_small!(d.cx.x, f32::EPSILON);
|
|
ensure_small!(d.cx.y, f32::EPSILON);
|
|
ensure_small!(d.cx.z, f32::EPSILON);
|
|
ensure_small!(d.cy.x, f32::EPSILON);
|
|
ensure_small!(d.cy.y, f32::EPSILON);
|
|
ensure_small!(d.cy.z, f32::EPSILON);
|
|
ensure_small!(d.cz.x, f32::EPSILON);
|
|
ensure_small!(d.cz.y, f32::EPSILON);
|
|
ensure_small!(d.cz.z, f32::EPSILON);
|
|
|
|
ensure_small!(m1.center.x - offset.x, f32::EPSILON);
|
|
ensure_small!(m1.center.y - offset.y, f32::EPSILON);
|
|
ensure_small!(m1.center.z - offset.z, f32::EPSILON);
|
|
}
|
|
|
|
// Capsule
|
|
{
|
|
let radius = capsule.radius;
|
|
let length = distance(capsule.center1, capsule.center2);
|
|
|
|
// Capsule along x-axis
|
|
let md = compute_capsule_mass(&capsule, 1.0);
|
|
|
|
// Box that fully contains capsule. Upper bound on capsule mass.
|
|
let r = make_box_hull(radius + 0.5 * length, radius, radius);
|
|
let md_upper = compute_hull_mass(&r, 1.0);
|
|
|
|
// Approximate capsule using convex hull. This should be a lower bound on the
|
|
// capsule mass.
|
|
let mut points = [Vec3::ZERO; 2 * N * N];
|
|
let d = PI / (N as f32 - 1.0);
|
|
let mut angle1 = -0.5 * PI;
|
|
let mut index = 0;
|
|
for _i in 0..N {
|
|
let s1 = angle1.sin();
|
|
let c1 = angle1.cos();
|
|
let mut angle2 = -0.5 * PI;
|
|
for _j in 0..N {
|
|
points[index].x = 1.0 + radius * c1;
|
|
points[index].y = radius * s1 * angle2.cos();
|
|
points[index].z = radius * s1 * angle2.sin();
|
|
angle2 += d;
|
|
index += 1;
|
|
}
|
|
|
|
angle1 += d;
|
|
}
|
|
|
|
let mut angle1 = 0.5 * PI;
|
|
for _i in 0..N {
|
|
let s1 = angle1.sin();
|
|
let c1 = angle1.cos();
|
|
let mut angle2 = -0.5 * PI;
|
|
for _j in 0..N {
|
|
points[index].x = -1.0 + radius * c1;
|
|
points[index].y = radius * s1 * angle2.cos();
|
|
points[index].z = radius * s1 * angle2.sin();
|
|
angle2 += d;
|
|
index += 1;
|
|
}
|
|
|
|
angle1 += d;
|
|
}
|
|
|
|
ensure!(index == 2 * N * N);
|
|
|
|
let hull = create_hull(&points, (2 * N * N) as i32).unwrap();
|
|
let md_lower = compute_hull_mass(&hull, 1.0);
|
|
|
|
ensure!(md_lower.mass < md.mass && md.mass < md_upper.mass);
|
|
ensure!(md_lower.inertia.cx.x < md.inertia.cx.x && md.inertia.cx.x < md_upper.inertia.cx.x);
|
|
ensure!(md_lower.inertia.cy.y < md.inertia.cy.y && md.inertia.cy.y < md_upper.inertia.cy.y);
|
|
ensure!(md_lower.inertia.cz.z < md.inertia.cz.z && md.inertia.cz.z < md_upper.inertia.cz.z);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn shape_aabb_test() {
|
|
let sphere = test_sphere();
|
|
let capsule = test_capsule();
|
|
let box_hull = test_box();
|
|
|
|
{
|
|
let b = compute_sphere_aabb(&sphere, Transform::IDENTITY);
|
|
ensure_small!(b.lower_bound.x, f32::EPSILON);
|
|
ensure_small!(b.lower_bound.y + 1.0, f32::EPSILON);
|
|
ensure_small!(b.lower_bound.z + 1.0, f32::EPSILON);
|
|
ensure_small!(b.upper_bound.x - 2.0, f32::EPSILON);
|
|
ensure_small!(b.upper_bound.y - 1.0, f32::EPSILON);
|
|
ensure_small!(b.upper_bound.z - 1.0, f32::EPSILON);
|
|
}
|
|
|
|
{
|
|
let b = compute_capsule_aabb(&capsule, Transform::IDENTITY);
|
|
ensure_small!(b.lower_bound.x + 2.0, f32::EPSILON);
|
|
ensure_small!(b.lower_bound.y + 1.0, f32::EPSILON);
|
|
ensure_small!(b.lower_bound.z + 1.0, f32::EPSILON);
|
|
ensure_small!(b.upper_bound.x - 2.0, f32::EPSILON);
|
|
ensure_small!(b.upper_bound.y - 1.0, f32::EPSILON);
|
|
ensure_small!(b.upper_bound.z - 1.0, f32::EPSILON);
|
|
}
|
|
{
|
|
let b = compute_hull_aabb(&box_hull, Transform::IDENTITY);
|
|
ensure_small!(b.lower_bound.x + 1.0, f32::EPSILON);
|
|
ensure_small!(b.lower_bound.y + 1.0, f32::EPSILON);
|
|
ensure_small!(b.lower_bound.z + 1.0, f32::EPSILON);
|
|
ensure_small!(b.upper_bound.x - 1.0, f32::EPSILON);
|
|
ensure_small!(b.upper_bound.y - 1.0, f32::EPSILON);
|
|
ensure_small!(b.upper_bound.z - 1.0, f32::EPSILON);
|
|
}
|
|
}
|
|
|
|
// Shared assertions for a surface hit. The normal points outward toward the ray,
|
|
// the point sits on the surface, and the point lies on the ray at the reported fraction.
|
|
fn check_cast_hit(out: CastOutput, origin: Vec3, translation: Vec3, point: Vec3, normal: Vec3, fraction: f32, tol: f32) {
|
|
ensure!(out.hit);
|
|
ensure_small!(out.fraction - fraction, tol);
|
|
ensure_small!(out.point.x - point.x, tol);
|
|
ensure_small!(out.point.y - point.y, tol);
|
|
ensure_small!(out.point.z - point.z, tol);
|
|
ensure_small!(out.normal.x - normal.x, tol);
|
|
ensure_small!(out.normal.y - normal.y, tol);
|
|
ensure_small!(out.normal.z - normal.z, tol);
|
|
|
|
let on_ray = mul_add(origin, out.fraction, translation);
|
|
ensure_small!(distance(out.point, on_ray), tol);
|
|
}
|
|
|
|
// The shared initial overlap convention: a ray starting inside a solid reports the origin
|
|
// with zero fraction and no normal.
|
|
fn check_initial_overlap(out: CastOutput, origin: Vec3) {
|
|
ensure!(out.hit);
|
|
ensure!(out.fraction == 0.0);
|
|
ensure_small!(distance(out.point, origin), f32::EPSILON);
|
|
ensure!(out.normal.x == 0.0 && out.normal.y == 0.0 && out.normal.z == 0.0);
|
|
}
|
|
|
|
#[test]
|
|
fn ray_cast_sphere_hit_test() {
|
|
let s = Sphere { center: vec3(0.0, 0.0, 0.0), radius: 1.0 };
|
|
|
|
// Hit along each principal axis. Surface at distance 3 over a length 8 ray.
|
|
{
|
|
let input = RayCastInput { origin: vec3(-4.0, 0.0, 0.0), translation: vec3(8.0, 0.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_sphere(&s, &input);
|
|
check_cast_hit(out, input.origin, input.translation, vec3(-1.0, 0.0, 0.0), vec3(-1.0, 0.0, 0.0), 3.0 / 8.0, 1e-5);
|
|
}
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.0, 4.0, 0.0), translation: vec3(0.0, -8.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_sphere(&s, &input);
|
|
check_cast_hit(out, input.origin, input.translation, vec3(0.0, 1.0, 0.0), vec3(0.0, 1.0, 0.0), 3.0 / 8.0, 1e-5);
|
|
}
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.0, 0.0, -4.0), translation: vec3(0.0, 0.0, 8.0), max_fraction: 1.0 };
|
|
let out = ray_cast_sphere(&s, &input);
|
|
check_cast_hit(out, input.origin, input.translation, vec3(0.0, 0.0, -1.0), vec3(0.0, 0.0, -1.0), 3.0 / 8.0, 1e-5);
|
|
}
|
|
|
|
// Offset center, hit partway along the ray.
|
|
{
|
|
let s2 = Sphere { center: vec3(5.0, 0.0, 0.0), radius: 2.0 };
|
|
let input = RayCastInput { origin: vec3(0.0, 0.0, 0.0), translation: vec3(10.0, 0.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_sphere(&s2, &input);
|
|
check_cast_hit(out, input.origin, input.translation, vec3(3.0, 0.0, 0.0), vec3(-1.0, 0.0, 0.0), 0.3, 1e-5);
|
|
}
|
|
|
|
// Diagonal ray straight through the center.
|
|
{
|
|
let k = 0.70710678;
|
|
let input = RayCastInput { origin: vec3(-3.0, -3.0, 0.0), translation: vec3(6.0, 6.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_sphere(&s, &input);
|
|
check_cast_hit(out, input.origin, input.translation, vec3(-k, -k, 0.0), vec3(-k, -k, 0.0), 0.382149, 1e-4);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn ray_cast_sphere_miss_test() {
|
|
let s = Sphere { center: vec3(0.0, 0.0, 0.0), radius: 1.0 };
|
|
|
|
// Pointing away.
|
|
{
|
|
let input = RayCastInput { origin: vec3(-4.0, 0.0, 0.0), translation: vec3(-8.0, 0.0, 0.0), max_fraction: 1.0 };
|
|
ensure!(!ray_cast_sphere(&s, &input).hit);
|
|
}
|
|
// Passes wide of the sphere.
|
|
{
|
|
let input = RayCastInput { origin: vec3(-4.0, 3.0, 0.0), translation: vec3(8.0, 0.0, 0.0), max_fraction: 1.0 };
|
|
ensure!(!ray_cast_sphere(&s, &input).hit);
|
|
}
|
|
// Aimed at the sphere but the translation stops short.
|
|
{
|
|
let input = RayCastInput { origin: vec3(-4.0, 0.0, 0.0), translation: vec3(8.0, 0.0, 0.0), max_fraction: 0.3 };
|
|
ensure!(!ray_cast_sphere(&s, &input).hit);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn ray_cast_sphere_clip_test() {
|
|
let s = Sphere { center: vec3(0.0, 0.0, 0.0), radius: 1.0 };
|
|
|
|
// The surface is reached at fraction 3/8. Straddle it with max_fraction.
|
|
{
|
|
let input = RayCastInput { origin: vec3(-4.0, 0.0, 0.0), translation: vec3(8.0, 0.0, 0.0), max_fraction: 0.374 };
|
|
ensure!(!ray_cast_sphere(&s, &input).hit);
|
|
}
|
|
{
|
|
let input = RayCastInput { origin: vec3(-4.0, 0.0, 0.0), translation: vec3(8.0, 0.0, 0.0), max_fraction: 0.376 };
|
|
let out = ray_cast_sphere(&s, &input);
|
|
ensure!(out.hit);
|
|
ensure_small!(out.fraction - 3.0 / 8.0, 1e-5);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn ray_cast_sphere_interior_test() {
|
|
let s = Sphere { center: vec3(0.0, 0.0, 0.0), radius: 1.0 };
|
|
|
|
// Origin inside reports the origin with zero fraction.
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.3, 0.0, 0.0), translation: vec3(8.0, 0.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_sphere(&s, &input);
|
|
ensure!(out.hit);
|
|
ensure!(out.fraction == 0.0);
|
|
ensure_small!(distance(out.point, input.origin), f32::EPSILON);
|
|
}
|
|
// Zero length ray inside.
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.5, 0.0, 0.0), translation: vec3(0.0, 0.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_sphere(&s, &input);
|
|
ensure!(out.hit);
|
|
ensure_small!(distance(out.point, input.origin), f32::EPSILON);
|
|
}
|
|
// Zero length ray outside.
|
|
{
|
|
let input = RayCastInput { origin: vec3(3.0, 0.0, 0.0), translation: vec3(0.0, 0.0, 0.0), max_fraction: 1.0 };
|
|
ensure!(!ray_cast_sphere(&s, &input).hit);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn ray_cast_sphere_graze_test() {
|
|
let s = Sphere { center: vec3(0.0, 0.0, 0.0), radius: 1.0 };
|
|
|
|
// Just inside the radius grazes a hit, just outside misses.
|
|
{
|
|
let input = RayCastInput { origin: vec3(-4.0, 0.999, 0.0), translation: vec3(8.0, 0.0, 0.0), max_fraction: 1.0 };
|
|
ensure!(ray_cast_sphere(&s, &input).hit);
|
|
}
|
|
{
|
|
let input = RayCastInput { origin: vec3(-4.0, 1.001, 0.0), translation: vec3(8.0, 0.0, 0.0), max_fraction: 1.0 };
|
|
ensure!(!ray_cast_sphere(&s, &input).hit);
|
|
}
|
|
}
|
|
|
|
// Capsule along x from -2 to 2, radius 1. Reused by the capsule ray cast subtests.
|
|
fn ray_capsule() -> Capsule {
|
|
Capsule { center1: vec3(-2.0, 0.0, 0.0), center2: vec3(2.0, 0.0, 0.0), radius: 1.0 }
|
|
}
|
|
|
|
#[test]
|
|
fn ray_cast_capsule_side_test() {
|
|
let ray_capsule = ray_capsule();
|
|
|
|
// Perpendicular hit on the cylindrical side. Surface at distance 2 over a length 6 ray.
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.0, 3.0, 0.0), translation: vec3(0.0, -6.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_capsule(&ray_capsule, &input);
|
|
check_cast_hit(out, input.origin, input.translation, vec3(0.0, 1.0, 0.0), vec3(0.0, 1.0, 0.0), 1.0 / 3.0, 1e-5);
|
|
}
|
|
// Same from +z to exercise the other transverse direction.
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.0, 0.0, 3.0), translation: vec3(0.0, 0.0, -6.0), max_fraction: 1.0 };
|
|
let out = ray_cast_capsule(&ray_capsule, &input);
|
|
check_cast_hit(out, input.origin, input.translation, vec3(0.0, 0.0, 1.0), vec3(0.0, 0.0, 1.0), 1.0 / 3.0, 1e-5);
|
|
}
|
|
// Side hit nearer the c1 end.
|
|
{
|
|
let input = RayCastInput { origin: vec3(-1.0, 3.0, 0.0), translation: vec3(0.0, -6.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_capsule(&ray_capsule, &input);
|
|
check_cast_hit(out, input.origin, input.translation, vec3(-1.0, 1.0, 0.0), vec3(0.0, 1.0, 0.0), 1.0 / 3.0, 1e-5);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn ray_cast_capsule_oblique_test() {
|
|
let ray_capsule = ray_capsule();
|
|
|
|
// Oblique ray in the z=0 plane. It crosses y=1 inside the cylinder span, so the
|
|
// normal stays transverse. Exercises the non perpendicular ray/axis solve where
|
|
// dot(axis, rayAxis) != 0.
|
|
let input = RayCastInput { origin: vec3(-3.0, 3.0, 0.0), translation: vec3(4.0, -4.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_capsule(&ray_capsule, &input);
|
|
check_cast_hit(out, input.origin, input.translation, vec3(-1.0, 1.0, 0.0), vec3(0.0, 1.0, 0.0), 0.5, 1e-4);
|
|
}
|
|
|
|
#[test]
|
|
fn ray_cast_capsule_cap_test() {
|
|
let ray_capsule = ray_capsule();
|
|
let k = 0.70710678;
|
|
|
|
// Collinear ray hits the c2 hemisphere from beyond the end.
|
|
{
|
|
let input = RayCastInput { origin: vec3(5.0, 0.0, 0.0), translation: vec3(-8.0, 0.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_capsule(&ray_capsule, &input);
|
|
check_cast_hit(out, input.origin, input.translation, vec3(3.0, 0.0, 0.0), vec3(1.0, 0.0, 0.0), 1.0 / 4.0, 1e-5);
|
|
}
|
|
// Off-axis ray through the c2 cap center, approaching from outside the cylinder.
|
|
{
|
|
let input = RayCastInput { origin: vec3(4.0, 2.0, 0.0), translation: vec3(-4.0, -4.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_capsule(&ray_capsule, &input);
|
|
check_cast_hit(out, input.origin, input.translation, vec3(2.0 + k, k, 0.0), vec3(k, k, 0.0), 0.323223, 1e-4);
|
|
}
|
|
// Mirror through the c1 cap center.
|
|
{
|
|
let input = RayCastInput { origin: vec3(-4.0, 2.0, 0.0), translation: vec3(4.0, -4.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_capsule(&ray_capsule, &input);
|
|
check_cast_hit(out, input.origin, input.translation, vec3(-2.0 - k, k, 0.0), vec3(-k, k, 0.0), 0.323223, 1e-4);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn ray_cast_capsule_miss_test() {
|
|
let ray_capsule = ray_capsule();
|
|
|
|
// Pointing away.
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.0, 3.0, 0.0), translation: vec3(0.0, 4.0, 0.0), max_fraction: 1.0 };
|
|
ensure!(!ray_cast_capsule(&ray_capsule, &input).hit);
|
|
}
|
|
// Crosses above the axis more than a radius away.
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.0, 4.0, 2.0), translation: vec3(0.0, -8.0, 0.0), max_fraction: 1.0 };
|
|
ensure!(!ray_cast_capsule(&ray_capsule, &input).hit);
|
|
}
|
|
// Aimed at the side but the translation stops short.
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.0, 5.0, 0.0), translation: vec3(0.0, -1.0, 0.0), max_fraction: 1.0 };
|
|
ensure!(!ray_cast_capsule(&ray_capsule, &input).hit);
|
|
}
|
|
// Parallel to the axis and outside the cylinder.
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.0, 3.0, 0.0), translation: vec3(8.0, 0.0, 0.0), max_fraction: 1.0 };
|
|
ensure!(!ray_cast_capsule(&ray_capsule, &input).hit);
|
|
}
|
|
// Descends past the rounded c2 end, beyond cap reach.
|
|
{
|
|
let input = RayCastInput { origin: vec3(4.0, 3.0, 0.0), translation: vec3(0.0, -6.0, 0.0), max_fraction: 1.0 };
|
|
ensure!(!ray_cast_capsule(&ray_capsule, &input).hit);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn ray_cast_capsule_interior_test() {
|
|
let ray_capsule = ray_capsule();
|
|
|
|
// Origin on the axis between the caps.
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.0, 0.0, 0.0), translation: vec3(0.0, -5.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_capsule(&ray_capsule, &input);
|
|
ensure!(out.hit);
|
|
ensure!(out.fraction == 0.0);
|
|
ensure_small!(distance(out.point, input.origin), f32::EPSILON);
|
|
}
|
|
// Origin inside the c2 hemisphere, past the cylinder end.
|
|
{
|
|
let input = RayCastInput { origin: vec3(2.5, 0.0, 0.0), translation: vec3(0.0, 0.0, 5.0), max_fraction: 1.0 };
|
|
let out = ray_cast_capsule(&ray_capsule, &input);
|
|
ensure!(out.hit);
|
|
ensure!(out.fraction == 0.0);
|
|
ensure_small!(distance(out.point, input.origin), f32::EPSILON);
|
|
}
|
|
// Zero length ray inside.
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.0, 0.0, 0.0), translation: vec3(0.0, 0.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_capsule(&ray_capsule, &input);
|
|
ensure!(out.hit);
|
|
ensure_small!(distance(out.point, input.origin), f32::EPSILON);
|
|
}
|
|
// Zero length ray outside.
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.0, 3.0, 0.0), translation: vec3(0.0, 0.0, 0.0), max_fraction: 1.0 };
|
|
ensure!(!ray_cast_capsule(&ray_capsule, &input).hit);
|
|
}
|
|
}
|
|
|
|
#[test]
|
|
fn ray_cast_capsule_degenerate_test() {
|
|
// Coincident centers collapse to a sphere.
|
|
let c = Capsule { center1: vec3(0.0, 0.0, 0.0), center2: vec3(0.0, 0.0, 0.0), radius: 1.0 };
|
|
let input = RayCastInput { origin: vec3(-4.0, 0.0, 0.0), translation: vec3(8.0, 0.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_capsule(&c, &input);
|
|
check_cast_hit(out, input.origin, input.translation, vec3(-1.0, 0.0, 0.0), vec3(-1.0, 0.0, 0.0), 3.0 / 8.0, 1e-5);
|
|
}
|
|
|
|
#[test]
|
|
fn ray_cast_capsule_clip_test() {
|
|
let ray_capsule = ray_capsule();
|
|
|
|
// The side hit occurs at fraction 1/3. Straddle it with max_fraction.
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.0, 3.0, 0.0), translation: vec3(0.0, -6.0, 0.0), max_fraction: 0.3 };
|
|
ensure!(!ray_cast_capsule(&ray_capsule, &input).hit);
|
|
}
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.0, 3.0, 0.0), translation: vec3(0.0, -6.0, 0.0), max_fraction: 0.5 };
|
|
let out = ray_cast_capsule(&ray_capsule, &input);
|
|
ensure!(out.hit);
|
|
ensure_small!(out.fraction - 1.0 / 3.0, 1e-5);
|
|
}
|
|
}
|
|
|
|
// A ray within a hair of the capsule axis must still hit when it slowly converges onto the
|
|
// surface. The closest point solver is ill conditioned in this band, so this guards the near
|
|
// parallel fallback that intersects the infinite cylinder directly.
|
|
#[test]
|
|
fn ray_cast_capsule_parallel_test() {
|
|
let ray_capsule = ray_capsule();
|
|
|
|
// Capsule along y. A long ray almost parallel to the axis drifts inward from just outside the
|
|
// cylinder and dips through the far endcap. The naive solve loses this hit to a determinant of zero.
|
|
let axis_y = Capsule { center1: vec3(0.0, 0.0, 0.0), center2: vec3(0.0, 10.0, 0.0), radius: 1.0 };
|
|
{
|
|
let input = RayCastInput { origin: vec3(1.0001, 100.0, 0.0), translation: vec3(-0.001, -200.0, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_capsule(&axis_y, &input);
|
|
ensure!(out.hit);
|
|
|
|
// The hit lands on the capsule surface and on the ray.
|
|
let on_seg = point_to_segment_distance(axis_y.center1, axis_y.center2, out.point);
|
|
ensure_small!(distance(out.point, on_seg) - axis_y.radius, 1e-3);
|
|
let on_ray = mul_add(input.origin, out.fraction, input.translation);
|
|
ensure_small!(distance(out.point, on_ray), 1e-3);
|
|
}
|
|
|
|
// Near parallel ray converging onto the x-axis capsule from far away.
|
|
{
|
|
let input = RayCastInput { origin: vec3(-1000.0, 1.0001, 0.0), translation: vec3(2000.0, -0.001, 0.0), max_fraction: 1.0 };
|
|
let out = ray_cast_capsule(&ray_capsule, &input);
|
|
ensure!(out.hit);
|
|
let on_seg = point_to_segment_distance(ray_capsule.center1, ray_capsule.center2, out.point);
|
|
ensure_small!(distance(out.point, on_seg) - ray_capsule.radius, 1e-3);
|
|
}
|
|
|
|
// Exactly parallel and outside the cylinder still misses.
|
|
{
|
|
let input = RayCastInput { origin: vec3(0.0, 3.0, 0.0), translation: vec3(8.0, 0.0, 0.0), max_fraction: 1.0 };
|
|
ensure!(!ray_cast_capsule(&ray_capsule, &input).hit);
|
|
}
|
|
}
|
|
|
|
// Zero length rays and initial overlap behave the same across the solid shapes. A moving ray
|
|
// and a zero length ray that both start inside report the origin with zero fraction, and a zero
|
|
// length ray that starts outside misses.
|
|
#[test]
|
|
fn ray_cast_overlap_convention_test() {
|
|
let ray_capsule = ray_capsule();
|
|
let box_hull = test_box();
|
|
|
|
let zero = vec3(0.0, 0.0, 0.0);
|
|
let ray = vec3(8.0, 0.0, 0.0);
|
|
|
|
// Sphere
|
|
{
|
|
let s = Sphere { center: vec3(0.0, 0.0, 0.0), radius: 1.0 };
|
|
let inside = vec3(0.2, 0.0, 0.0);
|
|
let outside = vec3(3.0, 0.0, 0.0);
|
|
let moving = RayCastInput { origin: inside, translation: ray, max_fraction: 1.0 };
|
|
let point_inside = RayCastInput { origin: inside, translation: zero, max_fraction: 1.0 };
|
|
let point_outside = RayCastInput { origin: outside, translation: zero, max_fraction: 1.0 };
|
|
check_initial_overlap(ray_cast_sphere(&s, &moving), inside);
|
|
check_initial_overlap(ray_cast_sphere(&s, &point_inside), inside);
|
|
ensure!(!ray_cast_sphere(&s, &point_outside).hit);
|
|
}
|
|
|
|
// Capsule
|
|
{
|
|
let inside = vec3(0.0, 0.0, 0.0);
|
|
let outside = vec3(0.0, 3.0, 0.0);
|
|
let moving = RayCastInput { origin: inside, translation: ray, max_fraction: 1.0 };
|
|
let point_inside = RayCastInput { origin: inside, translation: zero, max_fraction: 1.0 };
|
|
let point_outside = RayCastInput { origin: outside, translation: zero, max_fraction: 1.0 };
|
|
check_initial_overlap(ray_cast_capsule(&ray_capsule, &moving), inside);
|
|
check_initial_overlap(ray_cast_capsule(&ray_capsule, &point_inside), inside);
|
|
ensure!(!ray_cast_capsule(&ray_capsule, &point_outside).hit);
|
|
}
|
|
|
|
// Hull
|
|
{
|
|
let inside = vec3(0.3, 0.2, 0.1);
|
|
let outside = vec3(3.0, 0.0, 0.0);
|
|
let moving = RayCastInput { origin: inside, translation: ray, max_fraction: 1.0 };
|
|
let point_inside = RayCastInput { origin: inside, translation: zero, max_fraction: 1.0 };
|
|
let point_outside = RayCastInput { origin: outside, translation: zero, max_fraction: 1.0 };
|
|
check_initial_overlap(ray_cast_hull(&box_hull, &moving), inside);
|
|
check_initial_overlap(ray_cast_hull(&box_hull, &point_inside), inside);
|
|
ensure!(!ray_cast_hull(&box_hull, &point_outside).hit);
|
|
}
|
|
}
|
|
|
|
// Distance, in double precision, from a single precision hit point to the analytic first
|
|
// ray/sphere intersection of the same float ray. Isolates the single precision method error:
|
|
// the reference carries no float rounding, so what remains is purely what the method lost.
|
|
fn sphere_hit_error(shape: Sphere, input: &RayCastInput, point: Vec3) -> f64 {
|
|
let r = shape.radius as f64;
|
|
let sx = input.origin.x as f64 - shape.center.x as f64;
|
|
let sy = input.origin.y as f64 - shape.center.y as f64;
|
|
let sz = input.origin.z as f64 - shape.center.z as f64;
|
|
let (tx, ty, tz) = (input.translation.x as f64, input.translation.y as f64, input.translation.z as f64);
|
|
let len = (tx * tx + ty * ty + tz * tz).sqrt();
|
|
let (dx, dy, dz) = (tx / len, ty / len, tz / len);
|
|
let b = sx * dx + sy * dy + sz * dz;
|
|
let c = sx * sx + sy * sy + sz * sz - r * r;
|
|
let t = -b - (b * b - c).sqrt();
|
|
let ex = point.x as f64 - (input.origin.x as f64 + t * dx);
|
|
let ey = point.y as f64 - (input.origin.y as f64 + t * dy);
|
|
let ez = point.z as f64 - (input.origin.z as f64 + t * dz);
|
|
(ex * ex + ey * ey + ez * ez).sqrt()
|
|
}
|
|
|
|
// Same idea for the ray/infinite-cylinder intersection, used where the hit lands on the side.
|
|
fn capsule_hit_error(shape: Capsule, input: &RayCastInput, point: Vec3) -> f64 {
|
|
let mut ax = shape.center2.x as f64 - shape.center1.x as f64;
|
|
let mut ay = shape.center2.y as f64 - shape.center1.y as f64;
|
|
let mut az = shape.center2.z as f64 - shape.center1.z as f64;
|
|
let alen = (ax * ax + ay * ay + az * az).sqrt();
|
|
ax /= alen;
|
|
ay /= alen;
|
|
az /= alen;
|
|
|
|
let sx = input.origin.x as f64 - shape.center1.x as f64;
|
|
let sy = input.origin.y as f64 - shape.center1.y as f64;
|
|
let sz = input.origin.z as f64 - shape.center1.z as f64;
|
|
let (tx, ty, tz) = (input.translation.x as f64, input.translation.y as f64, input.translation.z as f64);
|
|
let tlen = (tx * tx + ty * ty + tz * tz).sqrt();
|
|
let (dx, dy, dz) = (tx / tlen, ty / tlen, tz / tlen);
|
|
|
|
let sa = sx * ax + sy * ay + sz * az;
|
|
let da = dx * ax + dy * ay + dz * az;
|
|
let (spx, spy, spz) = (sx - sa * ax, sy - sa * ay, sz - sa * az);
|
|
let (dpx, dpy, dpz) = (dx - da * ax, dy - da * ay, dz - da * az);
|
|
let a = dpx * dpx + dpy * dpy + dpz * dpz;
|
|
let b = 2.0 * (spx * dpx + spy * dpy + spz * dpz);
|
|
let r = shape.radius as f64;
|
|
let c = spx * spx + spy * spy + spz * spz - r * r;
|
|
let tau = (-b - (b * b - 4.0 * a * c).sqrt()) / (2.0 * a);
|
|
let ex = point.x as f64 - (input.origin.x as f64 + tau * dx);
|
|
let ey = point.y as f64 - (input.origin.y as f64 + tau * dy);
|
|
let ez = point.z as f64 - (input.origin.z as f64 + tau * dz);
|
|
(ex * ex + ey * ey + ez * ez).sqrt()
|
|
}
|
|
|
|
// A miss is the worst possible outcome, so fold it into the error as a large sentinel.
|
|
const RAY_MISS: f64 = 1.0e30;
|
|
|
|
#[test]
|
|
fn ray_cast_far_origin_test() {
|
|
let s = Sphere { center: vec3(0.0, 0.0, 0.0), radius: 1.0 };
|
|
let c = Capsule { center1: vec3(-2.0, 0.0, 0.0), center2: vec3(2.0, 0.0, 0.0), radius: 1.0 };
|
|
|
|
// (0,0,1) lies on the unit sphere and on the capsule side. The ray dives in from a far origin
|
|
// along H + D*u for a fan of directions u skewed off the surface normal, so the capsule solve
|
|
// sees a real perpendicular gap rather than a free exact cancellation.
|
|
let h = vec3(0.0, 0.0, 1.0);
|
|
let offsets = [-0.7f32, 0.0, 0.7];
|
|
let distances = [1e1f32, 1e2, 1e3, 1e4, 1e5, 1e6, 1e7];
|
|
|
|
let mut worst_sphere = [0.0f64; 7];
|
|
let mut worst_capsule = [0.0f64; 7];
|
|
|
|
println!(" worst hit point error over a fan of skew rays, by origin distance:");
|
|
println!(" {:<9} {:<13} {:<13}", "distance", "sphere", "capsule");
|
|
for i in 0..distances.len() {
|
|
let d = distances[i];
|
|
let mut max_s = 0.0f64;
|
|
let mut max_c = 0.0f64;
|
|
|
|
for ia in 0..offsets.len() {
|
|
for ib in 0..offsets.len() {
|
|
let u = normalize(vec3(offsets[ia], offsets[ib], 1.0));
|
|
let origin = mul_add(h, d, u);
|
|
let translation = mul_sv(-2.0 * d, u);
|
|
let input = RayCastInput { origin, translation, max_fraction: 1.0 };
|
|
|
|
let os = ray_cast_sphere(&s, &input);
|
|
let oc = ray_cast_capsule(&c, &input);
|
|
|
|
let err_s = if os.hit { sphere_hit_error(s, &input, os.point) } else { RAY_MISS };
|
|
let err_c = if oc.hit { capsule_hit_error(c, &input, oc.point) } else { RAY_MISS };
|
|
|
|
max_s = if err_s > max_s { err_s } else { max_s };
|
|
max_c = if err_c > max_c { err_c } else { max_c };
|
|
}
|
|
}
|
|
|
|
worst_sphere[i] = max_s;
|
|
worst_capsule[i] = max_c;
|
|
println!(" {:<9.0e} {:<13.3e} {:<13.3e}", d, max_s, max_c);
|
|
}
|
|
|
|
// The closest point formulation keeps the error at the single precision floor: it grows only
|
|
// linearly with origin distance, error ~ distance * FLT_EPSILON, with no catastrophic loss. This
|
|
// holds out to a million units. At ten million the origin coordinates carry a meter sized ULP,
|
|
// larger than the unit radius, so the ray genuinely drops the hit. That row is printed for insight
|
|
// but left unasserted rather than baking in the breakdown.
|
|
for i in 0..distances.len() {
|
|
if distances[i] < 1.0e7 {
|
|
let floor = 16.0 * distances[i] as f64 * f32::EPSILON as f64 + 2.0e-6;
|
|
ensure!(worst_sphere[i] < floor);
|
|
ensure!(worst_capsule[i] < floor);
|
|
}
|
|
}
|
|
|
|
// Still a clean sub-meter hit at a million units out.
|
|
ensure!(worst_sphere[5] < 0.5 && worst_capsule[5] < 0.5);
|
|
}
|
|
|
|
#[test]
|
|
fn ray_cast_shape_test() {
|
|
let sphere = test_sphere();
|
|
let capsule = test_capsule();
|
|
let box_hull = test_box();
|
|
|
|
let input = RayCastInput {
|
|
origin: vec3(-4.0, 0.0, 0.0),
|
|
translation: vec3(8.0, 0.0, 0.0),
|
|
max_fraction: 1.0,
|
|
};
|
|
|
|
{
|
|
let output = ray_cast_sphere(&sphere, &input);
|
|
ensure!(output.hit);
|
|
ensure_small!(output.normal.x + 1.0, f32::EPSILON);
|
|
ensure_small!(output.normal.y, f32::EPSILON);
|
|
ensure_small!(output.normal.z, f32::EPSILON);
|
|
ensure_small!(output.fraction - 0.5, f32::EPSILON);
|
|
}
|
|
|
|
{
|
|
let output = ray_cast_capsule(&capsule, &input);
|
|
ensure!(output.hit);
|
|
ensure_small!(output.normal.x + 1.0, f32::EPSILON);
|
|
ensure_small!(output.normal.y, f32::EPSILON);
|
|
ensure_small!(output.normal.z, f32::EPSILON);
|
|
ensure_small!(output.fraction - 1.0 / 4.0, f32::EPSILON);
|
|
}
|
|
|
|
{
|
|
let output = ray_cast_hull(&box_hull, &input);
|
|
ensure!(output.hit);
|
|
ensure_small!(output.normal.x + 1.0, f32::EPSILON);
|
|
ensure_small!(output.normal.y, f32::EPSILON);
|
|
ensure_small!(output.normal.z, f32::EPSILON);
|
|
ensure_small!(output.fraction - 3.0 / 8.0, f32::EPSILON);
|
|
}
|
|
}
|