60ec705ab2
- contact solver wide ops + V32 now have real SSE2 and NEON paths selected by target arch; scalar fallback behind the disable-simd feature. All three paths are bit-identical (cross-arch determinism verified: same ragdoll hash on NEON, SSE2 under Rosetta, and scalar). - double-precision feature (C BOX3D_DOUBLE_PRECISION): f64 world positions with the exact C boundary-function semantics; enables the far-from-origin test halves (157 tests in DP mode, 151 default). - world snapshots: recording substrate subset (buffer/writers/geometry registry/readers) + world_snapshot.c port; bit-identical continuation after restore, corrupt-image rejection. - examples/benchmark.rs: all 10 C benchmark scenarios; serial Rust runs 1.05-1.55x slower than C -O2 at one worker (geomean ~1.3x with fat LTO). Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
536 lines
17 KiB
Rust
536 lines
17 KiB
Rust
// Port of box3d/test/test_body_query.c
|
|
//
|
|
// The per-body query functions take an explicit world origin and a world body transform.
|
|
// Everything is re-centered on the origin so the float collision math stays accurate far from
|
|
// the world origin. These tests pin that framing: results come back in world space, the supplied
|
|
// transform drives the geometry (not the body's stored pose), and a large origin offset must not
|
|
// change a hit fraction or normal.
|
|
//
|
|
// CastRay and CastShape never touch the body's stored transform, so a static body at the origin
|
|
// holding local-frame shapes is enough. No step is needed for any query.
|
|
|
|
use makepad_box3d::body::*;
|
|
use makepad_box3d::hull::make_box_hull;
|
|
use makepad_box3d::math_functions::*;
|
|
use makepad_box3d::physics_world::*;
|
|
use makepad_box3d::types::*;
|
|
use makepad_box3d::{ensure, ensure_small};
|
|
|
|
fn create_query_world() -> (World, makepad_box3d::id::BodyId) {
|
|
let world_def = default_world_def();
|
|
let mut world = create_world(&world_def);
|
|
let body_def = default_body_def();
|
|
let body_id = create_body(&mut world, &body_def);
|
|
(world, body_id)
|
|
}
|
|
|
|
fn identity_at(x: f32, y: f32, z: f32) -> WorldTransform {
|
|
WorldTransform { p: pos(x, y, z), q: Quat::IDENTITY }
|
|
}
|
|
|
|
// CastRay ----------------------------------------------------------------------------------
|
|
|
|
#[test]
|
|
fn cast_ray_hits_sphere() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
let sphere = Sphere { center: vec3(0.0, 0.0, 0.0), radius: 1.0 };
|
|
let shape_def = default_shape_def();
|
|
makepad_box3d::shape::create_sphere_shape(&mut world, body_id, &shape_def, &sphere);
|
|
|
|
// Body sphere at world (5,0,0), ray straight at it along +X.
|
|
let body_transform = identity_at(5.0, 0.0, 0.0);
|
|
let result = body_cast_ray(
|
|
&world,
|
|
body_id,
|
|
pos(0.0, 0.0, 0.0),
|
|
vec3(10.0, 0.0, 0.0),
|
|
default_query_filter(),
|
|
1.0,
|
|
body_transform,
|
|
);
|
|
|
|
ensure!(result.hit);
|
|
ensure!(shape_is_valid(&world, result.shape_id));
|
|
ensure_small!(result.fraction - 0.4, 1e-5);
|
|
ensure_small!(result.normal.x + 1.0, 1e-5);
|
|
ensure_small!(result.normal.y, 1e-5);
|
|
ensure_small!(result.normal.z, 1e-5);
|
|
|
|
let point = to_vec3(result.point);
|
|
ensure_small!(point.x - 4.0, 1e-4);
|
|
ensure_small!(point.y, 1e-4);
|
|
ensure_small!(point.z, 1e-4);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
#[test]
|
|
fn cast_ray_miss() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
let sphere = Sphere { center: vec3(0.0, 0.0, 0.0), radius: 1.0 };
|
|
let shape_def = default_shape_def();
|
|
makepad_box3d::shape::create_sphere_shape(&mut world, body_id, &shape_def, &sphere);
|
|
|
|
// Ray runs parallel to the body, never reaching it.
|
|
let body_transform = identity_at(5.0, 0.0, 0.0);
|
|
let result = body_cast_ray(
|
|
&world,
|
|
body_id,
|
|
pos(0.0, 0.0, 0.0),
|
|
vec3(0.0, 10.0, 0.0),
|
|
default_query_filter(),
|
|
1.0,
|
|
body_transform,
|
|
);
|
|
|
|
ensure!(result.hit == false);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
#[test]
|
|
fn cast_ray_closest_shape() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
let shape_def = default_shape_def();
|
|
let near_sphere = Sphere { center: vec3(0.0, 0.0, 0.0), radius: 1.0 };
|
|
let far_sphere = Sphere { center: vec3(4.0, 0.0, 0.0), radius: 1.0 };
|
|
let near_id = makepad_box3d::shape::create_sphere_shape(&mut world, body_id, &shape_def, &near_sphere);
|
|
makepad_box3d::shape::create_sphere_shape(&mut world, body_id, &shape_def, &far_sphere);
|
|
|
|
// Ray crosses both spheres; the loop must shrink maxFraction to the nearer hit.
|
|
let body_transform = identity_at(0.0, 0.0, 0.0);
|
|
let result = body_cast_ray(
|
|
&world,
|
|
body_id,
|
|
pos(-5.0, 0.0, 0.0),
|
|
vec3(10.0, 0.0, 0.0),
|
|
default_query_filter(),
|
|
1.0,
|
|
body_transform,
|
|
);
|
|
|
|
ensure!(result.hit);
|
|
ensure!(result.shape_id.index1 == near_id.index1);
|
|
ensure!(result.shape_id.generation == near_id.generation);
|
|
ensure_small!(result.fraction - 0.4, 1e-5);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
#[test]
|
|
fn cast_ray_rotated_body() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
// Local center (0,2,0) rotated +90 deg about Z lands at world (-2,0,0).
|
|
let sphere = Sphere { center: vec3(0.0, 2.0, 0.0), radius: 0.5 };
|
|
let shape_def = default_shape_def();
|
|
makepad_box3d::shape::create_sphere_shape(&mut world, body_id, &shape_def, &sphere);
|
|
|
|
let body_transform = WorldTransform {
|
|
p: pos(0.0, 0.0, 0.0),
|
|
q: make_quat_from_axis_angle(vec3(0.0, 0.0, 1.0), 0.5 * PI),
|
|
};
|
|
let result = body_cast_ray(
|
|
&world,
|
|
body_id,
|
|
pos(0.0, 0.0, 0.0),
|
|
vec3(-4.0, 0.0, 0.0),
|
|
default_query_filter(),
|
|
1.0,
|
|
body_transform,
|
|
);
|
|
|
|
ensure!(result.hit);
|
|
ensure_small!(result.fraction - 0.375, 1e-5);
|
|
ensure_small!(result.normal.x - 1.0, 1e-5);
|
|
|
|
let point = to_vec3(result.point);
|
|
ensure_small!(point.x + 1.5, 1e-4);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
#[test]
|
|
fn cast_ray_far_from_origin() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
let sphere = Sphere { center: vec3(0.0, 0.0, 0.0), radius: 1.0 };
|
|
let shape_def = default_shape_def();
|
|
makepad_box3d::shape::create_sphere_shape(&mut world, body_id, &shape_def, &sphere);
|
|
|
|
// Same geometry as cast_ray_hits_sphere shifted far from the world origin. The relative
|
|
// framing keeps the subtraction exact, so fraction and normal must be unchanged.
|
|
let origin = pos(1.0e6, -2.0e6, 5.0e5);
|
|
let body_transform = WorldTransform { p: offset_pos(origin, vec3(5.0, 0.0, 0.0)), q: Quat::IDENTITY };
|
|
let result = body_cast_ray(
|
|
&world,
|
|
body_id,
|
|
origin,
|
|
vec3(10.0, 0.0, 0.0),
|
|
default_query_filter(),
|
|
1.0,
|
|
body_transform,
|
|
);
|
|
|
|
ensure!(result.hit);
|
|
ensure_small!(result.fraction - 0.4, 1e-5);
|
|
ensure_small!(result.normal.x + 1.0, 1e-5);
|
|
ensure_small!(result.normal.y, 1e-5);
|
|
ensure_small!(result.normal.z, 1e-5);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
// CastShape --------------------------------------------------------------------------------
|
|
|
|
#[test]
|
|
fn cast_shape_hits_box() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
let box_hull = make_box_hull(1.0, 1.0, 1.0);
|
|
let shape_def = default_shape_def();
|
|
makepad_box3d::shape::create_hull_shape(&mut world, body_id, &shape_def, &box_hull);
|
|
|
|
// Sphere proxy of radius 0.5 cast along +X into a box whose front face is at world x = 4.
|
|
let point = [vec3(0.0, 0.0, 0.0)];
|
|
let proxy = ShapeProxy { points: &point, radius: 0.5 };
|
|
let body_transform = identity_at(5.0, 0.0, 0.0);
|
|
let result = body_cast_shape(
|
|
&world,
|
|
body_id,
|
|
pos(0.0, 0.0, 0.0),
|
|
&proxy,
|
|
vec3(10.0, 0.0, 0.0),
|
|
default_query_filter(),
|
|
1.0,
|
|
false,
|
|
body_transform,
|
|
);
|
|
|
|
// Front face at world x = 4. The fraction carries a small shape-cast skin, the contact point
|
|
// and normal do not.
|
|
ensure!(result.hit);
|
|
ensure!(shape_is_valid(&world, result.shape_id));
|
|
ensure_small!(result.fraction - 0.35, 1e-2);
|
|
ensure_small!(result.normal.x + 1.0, 1e-4);
|
|
|
|
let hit = to_vec3(result.point);
|
|
ensure_small!(hit.x - 4.0, 1e-3);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
#[test]
|
|
fn cast_shape_miss() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
let box_hull = make_box_hull(1.0, 1.0, 1.0);
|
|
let shape_def = default_shape_def();
|
|
makepad_box3d::shape::create_hull_shape(&mut world, body_id, &shape_def, &box_hull);
|
|
|
|
let point = [vec3(0.0, 0.0, 0.0)];
|
|
let proxy = ShapeProxy { points: &point, radius: 0.5 };
|
|
let body_transform = identity_at(5.0, 0.0, 0.0);
|
|
let result = body_cast_shape(
|
|
&world,
|
|
body_id,
|
|
pos(0.0, 0.0, 0.0),
|
|
&proxy,
|
|
vec3(0.0, 10.0, 0.0),
|
|
default_query_filter(),
|
|
1.0,
|
|
false,
|
|
body_transform,
|
|
);
|
|
|
|
ensure!(result.hit == false);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
#[test]
|
|
fn cast_shape_rotated_body() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
// Body sphere local center (0,2,0) rotated +90 deg about Z lands at world (-2,0,0).
|
|
let sphere = Sphere { center: vec3(0.0, 2.0, 0.0), radius: 1.0 };
|
|
let shape_def = default_shape_def();
|
|
makepad_box3d::shape::create_sphere_shape(&mut world, body_id, &shape_def, &sphere);
|
|
|
|
let point = [vec3(0.0, 0.0, 0.0)];
|
|
let proxy = ShapeProxy { points: &point, radius: 0.5 };
|
|
let body_transform = WorldTransform {
|
|
p: pos(0.0, 0.0, 0.0),
|
|
q: make_quat_from_axis_angle(vec3(0.0, 0.0, 1.0), 0.5 * PI),
|
|
};
|
|
let result = body_cast_shape(
|
|
&world,
|
|
body_id,
|
|
pos(0.0, 0.0, 0.0),
|
|
&proxy,
|
|
vec3(-4.0, 0.0, 0.0),
|
|
default_query_filter(),
|
|
1.0,
|
|
false,
|
|
body_transform,
|
|
);
|
|
|
|
ensure!(result.hit);
|
|
ensure_small!(result.fraction - 0.125, 1e-2);
|
|
ensure_small!(result.normal.x - 1.0, 1e-4);
|
|
|
|
let hit = to_vec3(result.point);
|
|
ensure_small!(hit.x + 1.0, 1e-3);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
#[test]
|
|
fn cast_shape_far_from_origin() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
let box_hull = make_box_hull(1.0, 1.0, 1.0);
|
|
let shape_def = default_shape_def();
|
|
makepad_box3d::shape::create_hull_shape(&mut world, body_id, &shape_def, &box_hull);
|
|
|
|
let point = [vec3(0.0, 0.0, 0.0)];
|
|
let proxy = ShapeProxy { points: &point, radius: 0.5 };
|
|
let origin = pos(1.0e6, -2.0e6, 5.0e5);
|
|
let body_transform = WorldTransform { p: offset_pos(origin, vec3(5.0, 0.0, 0.0)), q: Quat::IDENTITY };
|
|
let result = body_cast_shape(
|
|
&world,
|
|
body_id,
|
|
origin,
|
|
&proxy,
|
|
vec3(10.0, 0.0, 0.0),
|
|
default_query_filter(),
|
|
1.0,
|
|
false,
|
|
body_transform,
|
|
);
|
|
|
|
ensure!(result.hit);
|
|
ensure_small!(result.fraction - 0.35, 1e-2);
|
|
ensure_small!(result.normal.x + 1.0, 1e-4);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
// OverlapShape -----------------------------------------------------------------------------
|
|
|
|
#[test]
|
|
fn overlap_true() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
let box_hull = make_box_hull(1.0, 1.0, 1.0);
|
|
let shape_def = default_shape_def();
|
|
makepad_box3d::shape::create_hull_shape(&mut world, body_id, &shape_def, &box_hull);
|
|
|
|
// Proxy sits at the box center.
|
|
let point = [vec3(0.0, 0.0, 0.0)];
|
|
let proxy = ShapeProxy { points: &point, radius: 0.5 };
|
|
let body_transform = identity_at(5.0, 0.0, 0.0);
|
|
let overlaps = body_overlap_shape(&world, body_id, pos(5.0, 0.0, 0.0), &proxy, default_query_filter(), body_transform);
|
|
|
|
ensure!(overlaps);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
#[test]
|
|
fn overlap_false() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
let box_hull = make_box_hull(1.0, 1.0, 1.0);
|
|
let shape_def = default_shape_def();
|
|
makepad_box3d::shape::create_hull_shape(&mut world, body_id, &shape_def, &box_hull);
|
|
|
|
let point = [vec3(0.0, 0.0, 0.0)];
|
|
let proxy = ShapeProxy { points: &point, radius: 0.5 };
|
|
let body_transform = identity_at(5.0, 0.0, 0.0);
|
|
let overlaps = body_overlap_shape(&world, body_id, pos(20.0, 0.0, 0.0), &proxy, default_query_filter(), body_transform);
|
|
|
|
ensure!(overlaps == false);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
#[test]
|
|
fn overlap_respects_body_transform() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
let box_hull = make_box_hull(1.0, 1.0, 1.0);
|
|
let shape_def = default_shape_def();
|
|
makepad_box3d::shape::create_hull_shape(&mut world, body_id, &shape_def, &box_hull);
|
|
|
|
// Fixed proxy and origin: only the supplied transform decides the overlap.
|
|
let point = [vec3(0.0, 0.0, 0.0)];
|
|
let proxy = ShapeProxy { points: &point, radius: 0.5 };
|
|
let origin = pos(0.0, 0.0, 0.0);
|
|
|
|
ensure!(body_overlap_shape(&world, body_id, origin, &proxy, default_query_filter(), identity_at(0.0, 0.0, 0.0)));
|
|
ensure!(
|
|
body_overlap_shape(&world, body_id, origin, &proxy, default_query_filter(), identity_at(20.0, 0.0, 0.0)) == false
|
|
);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
#[test]
|
|
fn overlap_filter() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
let box_hull = make_box_hull(1.0, 1.0, 1.0);
|
|
let shape_def = default_shape_def();
|
|
makepad_box3d::shape::create_hull_shape(&mut world, body_id, &shape_def, &box_hull);
|
|
|
|
let point = [vec3(0.0, 0.0, 0.0)];
|
|
let proxy = ShapeProxy { points: &point, radius: 0.5 };
|
|
let body_transform = identity_at(0.0, 0.0, 0.0);
|
|
|
|
// Geometry overlaps, but a zero mask rejects every category.
|
|
let mut filter = default_query_filter();
|
|
filter.mask_bits = 0;
|
|
let overlaps = body_overlap_shape(&world, body_id, pos(0.0, 0.0, 0.0), &proxy, filter, body_transform);
|
|
|
|
ensure!(overlaps == false);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
// CollideMover -----------------------------------------------------------------------------
|
|
|
|
#[test]
|
|
fn mover_touches_box() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
let box_hull = make_box_hull(0.5, 0.5, 0.5);
|
|
let shape_def = default_shape_def();
|
|
makepad_box3d::shape::create_hull_shape(&mut world, body_id, &shape_def, &box_hull);
|
|
|
|
// Mover core runs above the +Y face; its 0.2 radius reaches 0.1 into it.
|
|
let mover = Capsule { center1: vec3(-0.3, 0.6, 0.0), center2: vec3(0.3, 0.6, 0.0), radius: 0.2 };
|
|
let mut planes = [BodyPlaneResult::default(); 4];
|
|
let body_transform = identity_at(0.0, 0.0, 0.0);
|
|
let count = body_collide_mover(
|
|
&world,
|
|
body_id,
|
|
&mut planes,
|
|
pos(0.0, 0.0, 0.0),
|
|
&mover,
|
|
default_query_filter(),
|
|
body_transform,
|
|
);
|
|
|
|
ensure!(count == 1);
|
|
ensure!(shape_is_valid(&world, planes[0].shape_id));
|
|
ensure!(is_normalized(planes[0].result.plane.normal));
|
|
ensure!(planes[0].result.plane.normal.y > 0.99);
|
|
ensure_small!(planes[0].result.plane.offset - 0.1, 1e-4);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
#[test]
|
|
fn mover_separated() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
let box_hull = make_box_hull(0.5, 0.5, 0.5);
|
|
let shape_def = default_shape_def();
|
|
makepad_box3d::shape::create_hull_shape(&mut world, body_id, &shape_def, &box_hull);
|
|
|
|
let mover = Capsule { center1: vec3(-0.3, 5.0, 0.0), center2: vec3(0.3, 5.0, 0.0), radius: 0.2 };
|
|
let mut planes = [BodyPlaneResult::default(); 4];
|
|
let body_transform = identity_at(0.0, 0.0, 0.0);
|
|
let count = body_collide_mover(
|
|
&world,
|
|
body_id,
|
|
&mut planes,
|
|
pos(0.0, 0.0, 0.0),
|
|
&mover,
|
|
default_query_filter(),
|
|
body_transform,
|
|
);
|
|
|
|
ensure!(count == 0);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
#[test]
|
|
fn mover_rotated_body() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
let box_hull = make_box_hull(0.5, 0.5, 0.5);
|
|
let shape_def = default_shape_def();
|
|
makepad_box3d::shape::create_hull_shape(&mut world, body_id, &shape_def, &box_hull);
|
|
|
|
// Rotating +90 deg about X turns the local +Y face toward world +Z. The mover sits above the
|
|
// world +Z face, so the returned normal must come back rotated into world space.
|
|
let mover = Capsule { center1: vec3(-0.3, 0.0, 0.6), center2: vec3(0.3, 0.0, 0.6), radius: 0.2 };
|
|
let mut planes = [BodyPlaneResult::default(); 4];
|
|
let body_transform = WorldTransform {
|
|
p: pos(0.0, 0.0, 0.0),
|
|
q: make_quat_from_axis_angle(vec3(1.0, 0.0, 0.0), 0.5 * PI),
|
|
};
|
|
let count = body_collide_mover(
|
|
&world,
|
|
body_id,
|
|
&mut planes,
|
|
pos(0.0, 0.0, 0.0),
|
|
&mover,
|
|
default_query_filter(),
|
|
body_transform,
|
|
);
|
|
|
|
ensure!(count == 1);
|
|
ensure!(is_normalized(planes[0].result.plane.normal));
|
|
ensure!(planes[0].result.plane.normal.z > 0.99);
|
|
ensure_small!(planes[0].result.plane.offset - 0.1, 1e-4);
|
|
|
|
destroy_world(world);
|
|
}
|
|
|
|
#[test]
|
|
fn mover_capacity() {
|
|
let (mut world, body_id) = create_query_world();
|
|
|
|
// Two spheres each touch a mover that runs between them along X at y = 0.
|
|
let shape_def = default_shape_def();
|
|
let left = Sphere { center: vec3(-0.4, 0.6, 0.0), radius: 0.5 };
|
|
let right = Sphere { center: vec3(0.4, 0.6, 0.0), radius: 0.5 };
|
|
makepad_box3d::shape::create_sphere_shape(&mut world, body_id, &shape_def, &left);
|
|
makepad_box3d::shape::create_sphere_shape(&mut world, body_id, &shape_def, &right);
|
|
|
|
let mover = Capsule { center1: vec3(-1.0, 0.0, 0.0), center2: vec3(1.0, 0.0, 0.0), radius: 0.2 };
|
|
let mut planes = [BodyPlaneResult::default(); 4];
|
|
let body_transform = identity_at(0.0, 0.0, 0.0);
|
|
|
|
// Capacity caps the result and prevents writing past the buffer.
|
|
let capped = body_collide_mover(
|
|
&world,
|
|
body_id,
|
|
&mut planes[..1],
|
|
pos(0.0, 0.0, 0.0),
|
|
&mover,
|
|
default_query_filter(),
|
|
body_transform,
|
|
);
|
|
ensure!(capped == 1);
|
|
|
|
let full = body_collide_mover(
|
|
&world,
|
|
body_id,
|
|
&mut planes,
|
|
pos(0.0, 0.0, 0.0),
|
|
&mover,
|
|
default_query_filter(),
|
|
body_transform,
|
|
);
|
|
ensure!(full == 2);
|
|
|
|
destroy_world(world);
|
|
}
|