From 7abdec9f2aab086e5100e1832770fead71a6c845 Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Sat, 19 Sep 2026 22:00:13 -0300 Subject: [PATCH 1/2] shape: the sphere, the capsule and the shape of any kind aephysics.shape, Box3D's sphere.c, capsule.c and the geometric half of shape.c in Aether: the sphere's and capsule's mass, bounds, ray casts in the closest-point forms that keep their precision far from the origin, shape casts, overlap and mover planes; the hull's mover; and the Shape of any kind (sphere, capsule, hull, mesh, height field, the compound reserved) with its dispatch under a transform: bounds, swept and fat bounds, centroid, areas, mass, extent, ray and shape casts, overlap, the mover's planes, the proxy; and the collision filters. test_shape.ae: the reference's masses (sphere, analytic, translated, rotated and transformed boxes, the capsule bracketed by hulls), bounds, the sphere and capsule ray cases (hits, misses, clip, interior, graze, side, oblique, caps, degenerate, near parallel), the overlap convention across the solids, the far-origin precision, the cast through the dispatch; plus every kind under one transform through every query, and the filters. 440 checks. The name and flag tests need a world and come with the dynamics. bench/shape.ae against bench/shape_box3d.c: the same hits and sums on every phase; sphere rays and masses at parity, capsule rays 0.8x, the mover 1.45x, the GJK cast and overlap on proxies with radii 2-2.5x, recorded to profile. --- README.md | 3 +- aephysics/shape/module.ae | 716 ++++++++++++++++++++++++++++++++ aephysics/test_shape.ae | 833 ++++++++++++++++++++++++++++++++++++++ bench/RESULTS.md | 29 ++ bench/shape.ae | 134 ++++++ bench/shape_box3d.c | 151 +++++++ design.md | 30 +- 7 files changed, 1889 insertions(+), 7 deletions(-) create mode 100644 aephysics/shape/module.ae create mode 100644 aephysics/test_shape.ae create mode 100644 bench/shape.ae create mode 100644 bench/shape_box3d.c diff --git a/README.md b/README.md index bac6610..9b297f5 100644 --- a/README.md +++ b/README.md @@ -34,7 +34,8 @@ so a test written against the reference reads the same here. | `aephysics.triangle_manifold` | one mesh triangle against a sphere, capsule or hull: back-side cull with hysteresis, GJK shallow, the separating axis test deep with the triangle's edges as zero-area faces, the feature recorded for the mesh contact's ghost-collision reduction | done, `test_triangle_manifold.ae` (1.5k checks); [same manifolds as the reference, within 10% on hulls](bench/RESULTS.md#triangle_manifold) | | `aephysics.mesh` | the triangle mesh: a BVH by binned SAH or median split with the triangles in depth-first order, vertex welding, edge flags, any scale including mirrored; overlap, ray cast, shape cast, the mover's planes, a box query | done, `test_mesh.ae` (1.6k checks); [same trees as the reference, traversals 1.7-2x](bench/RESULTS.md#mesh) | | `aephysics.height_field` | the height field: quantised heights on a fixed diagonal, materials and holes per cell, edge flags per triangle, either winding; overlap, ray and shape casts by a walk along the grid, the mover's planes, a box query | done, `test_height_field.ae` (113 checks, casts against a brute force over a wave); [same results as the reference, query at parity, casts 1.4-2x](bench/RESULTS.md#height_field) | -| `aephysics.shape` | shapes with mass properties, ray and shape casts per shape, compounds | next | +| `aephysics.shape` | the shape of any kind: sphere and capsule geometry (mass, bounds, ray casts with the far-origin precision, casts, overlap, mover), the dispatch over every kind under a transform (bounds, swept and fat bounds, centroid, areas, mass, extent, ray and shape casts, overlap, the mover's planes, the proxy), the collision filters | done, `test_shape.ae` (440 checks); [same results as the reference, rays and masses at parity, radius casts 2.5x](bench/RESULTS.md#shape) | +| `aephysics.compound` | the baked compound of child shapes under a static tree, with material and hull sharing | next | | `aephysics.dynamics` | bodies, contacts, the constraint graph, islands, the Soft Step solver, joints (spherical, revolute, prismatic, distance, motor, weld, wheel), sensors, the character mover, the world | | | `aephysics` | the public API | | diff --git a/aephysics/shape/module.ae b/aephysics/shape/module.ae new file mode 100644 index 0000000..68286db --- /dev/null +++ b/aephysics/shape/module.ae @@ -0,0 +1,716 @@ +// aephysics.shape -- the shape: one of a sphere, a capsule, a hull, a +// mesh or a height field (the compound comes with its own module), with +// what every query needs regardless of kind: the bounds under a +// transform, the swept and fat bounds, the centroid, the areas, the mass +// properties, the extent for sleeping, the ray cast, the shape cast, the +// overlap, the character mover's planes and the proxy; and the sphere's +// and the capsule's own geometry, which had no module of their own. +// +// The shapes are Box3D's (Erin Catto, MIT) sphere.c, capsule.c and the +// geometric half of shape.c, the reference this engine is measured +// against: the ray against a sphere by the closest point on the ray to +// the centre (the Ray Tracing Gems form that keeps its precision far +// from the origin), the ray against a capsule by the closest points of +// the two lines with the near-parallel fallback through the axial +// circle, the initial-overlap convention (the origin, fraction zero, no +// normal), and the mover's deep-overlap directions. Names are the +// reference's without its prefix, in snake case: b3RayCastCapsule is +// ray_cast_capsule. The world-bound half of shape.c (creation on a body, +// the broad-phase proxy, materials, events, filters at run time) comes +// with the dynamics. +// +// Differences: no union; a Shape holds every kind's fields and a kind. +import std.string +import aephysics.math +import aephysics.core +import aephysics.hull +import aephysics.distance +import aephysics.manifold +import aephysics.mesh +import aephysics.height_field + +exports ( + Shape, Filter, QueryFilter, + SHAPE_SPHERE, SHAPE_CAPSULE, SHAPE_HULL, SHAPE_MESH, SHAPE_HEIGHT_FIELD, SHAPE_COMPOUND, SHAPE_KIND_COUNT, + OVERLAP_SLOP, + sphere_shape, capsule_shape, hull_shape, mesh_shape, height_field_shape, + default_filter, default_query_filter, should_shapes_collide, should_query_collide, is_convex, + compute_sphere_mass, compute_sphere_aabb, compute_swept_sphere_aabb, overlap_sphere, + ray_cast_sphere, ray_cast_hollow_sphere, shape_cast_sphere, collide_mover_and_sphere, + compute_capsule_mass, compute_capsule_aabb, compute_swept_capsule_aabb, overlap_capsule, + ray_cast_capsule, shape_cast_capsule, collide_mover_and_capsule, + collide_mover_and_hull, + compute_shape_aabb, compute_fat_shape_aabb, compute_swept_shape_aabb, get_shape_centroid, + get_shape_area, get_shape_projected_area, compute_shape_mass, compute_shape_extent, + ray_cast_shape, shape_cast_shape, overlap_shape, collide_mover, make_shape_proxy, + farthest_point_on_aabb +) + +extern sqrt(x: float) -> float + +const SHAPE_SPHERE = 0 +const SHAPE_CAPSULE = 1 +const SHAPE_HULL = 2 +const SHAPE_MESH = 3 +const SHAPE_HEIGHT_FIELD = 4 +const SHAPE_COMPOUND = 5 +const SHAPE_KIND_COUNT = 6 + +// Closer than this is an overlap (a tenth of the linear slop). +const OVERLAP_SLOP = 0.0005 + +// A shape of one kind: the fields of the other kinds are unused. +struct Shape { + kind: int + density: float + material_count: int + sphere: Sphere + capsule: Capsule + hull: *HullData + mesh: Mesh + height_field: *HeightFieldData + compound: ptr // *CompoundData, the next layer +} + +// Who collides with whom: a shape is in the categories of its bits and +// collides with the categories in its mask; a shared positive group +// always collides, a shared negative group never. +struct Filter { + category_bits: long + mask_bits: long + group_index: int +} + +struct QueryFilter { + category_bits: long + mask_bits: long +} + +// --- construction --------------------------------------------------------------------- + +empty_shape() -> Shape { + return Shape { kind: SHAPE_SPHERE, density: 1.0, material_count: 1, + sphere: Sphere { center: math.vec3_zero(), radius: 0.0 }, + capsule: Capsule { center1: math.vec3_zero(), center2: math.vec3_zero(), radius: 0.0 }, + hull: null, mesh: Mesh { data: null, scale: math.vec3_one() }, height_field: null, compound: null } +} + +sphere_shape(s: Sphere, density: float) -> Shape { + shape = empty_shape() + shape.kind = SHAPE_SPHERE + shape.density = density + shape.sphere = s + return shape +} + +capsule_shape(c: Capsule, density: float) -> Shape { + shape = empty_shape() + shape.kind = SHAPE_CAPSULE + shape.density = density + shape.capsule = c + return shape +} + +hull_shape(h: *HullData, density: float) -> Shape { + shape = empty_shape() + shape.kind = SHAPE_HULL + shape.density = density + shape.hull = h + return shape +} + +mesh_shape(m: Mesh, material_count: int) -> Shape { + shape = empty_shape() + shape.kind = SHAPE_MESH + shape.density = 0.0 + shape.mesh = m + shape.material_count = math.max_int(material_count, 1) + return shape +} + +height_field_shape(h: *HeightFieldData, material_count: int) -> Shape { + shape = empty_shape() + shape.kind = SHAPE_HEIGHT_FIELD + shape.density = 0.0 + shape.height_field = h + shape.material_count = math.max_int(material_count, 1) + return shape +} + +// --- filters ---------------------------------------------------------------------------- + +default_filter() -> Filter { return Filter { category_bits: 1 as long, mask_bits: all_bits(), group_index: 0 } } +default_query_filter() -> QueryFilter { return QueryFilter { category_bits: 1 as long, mask_bits: all_bits() } } + +all_bits() -> long { return (0 as long) - (1 as long) } + +should_shapes_collide(a: Filter, b: Filter) -> bool { + if a.group_index == b.group_index && a.group_index != 0 { return a.group_index > 0 } + return (a.mask_bits & b.category_bits) != (0 as long) && (a.category_bits & b.mask_bits) != (0 as long) +} + +should_query_collide(shape_filter: Filter, query_filter: QueryFilter) -> bool { + return (shape_filter.category_bits & query_filter.mask_bits) != (0 as long) && + (shape_filter.mask_bits & query_filter.category_bits) != (0 as long) +} + +is_convex(kind: int) -> bool { return kind == SHAPE_SPHERE || kind == SHAPE_CAPSULE || kind == SHAPE_HULL } + +// --- the sphere ----------------------------------------------------------------------------- + +compute_sphere_mass(s: Sphere, density: float) -> MassData { + radius = s.radius + volume = 4.0 / 3.0 * math.PI * radius * radius * radius + mass = volume * density + ixx = 0.4 * mass * radius * radius + // The inertia is about the centre of mass. + return MassData { mass: mass, center: s.center, inertia: math.make_diagonal_matrix(ixx, ixx, ixx) } +} + +compute_sphere_aabb(s: Sphere, t: Transform) -> AABB { + center = math.transform_point(t, s.center) + extent = math.vec3(s.radius, s.radius, s.radius) + return AABB { lower: math.sub(center, extent), upper: math.add(center, extent) } +} + +compute_swept_sphere_aabb(s: Sphere, xf1: Transform, xf2: Transform) -> AABB { + r = math.vec3(s.radius, s.radius, s.radius) + center1 = math.transform_point(xf1, s.center) + center2 = math.transform_point(xf2, s.center) + return AABB { lower: math.sub(math.min_vec3(center1, center2), r), upper: math.add(math.max_vec3(center1, center2), r) } +} + +var g_center: ptr = null // Vec3[2]: a sphere's centre or a capsule's two, for a proxy + +center_buffer() -> Vec3[] { + if g_center == null { g_center = core.alloc(2 * sizeof(Vec3)) } + return g_center as Vec3[] +} + +sphere_proxy(s: Sphere) -> ShapeProxy { + buffer = center_buffer() + buffer[0] = s.center + return distance.shape_proxy(g_center, 1, s.radius) +} + +capsule_proxy(c: Capsule) -> ShapeProxy { + buffer = center_buffer() + buffer[0] = c.center1 + buffer[1] = c.center2 + return distance.shape_proxy(g_center, 2, c.radius) +} + +overlap_sphere(s: Sphere, shape_transform: Transform, proxy: ShapeProxy) -> bool { + input = DistanceInput { proxy_a: sphere_proxy(s), proxy_b: proxy, + transform: math.inv_mul_transforms(shape_transform, math.transform_identity()), use_radii: true } + cache = distance.empty_cache() + output = distance.shape_distance(&input, &cache, null, 0) + return output.distance < OVERLAP_SLOP +} + +// The ray against the sphere through the closest point on the ray to the +// centre, which keeps the precision of a far origin (Ray Tracing Gems +// 2019, and codercorner.com/blog/?p=321). A ray starting inside, or of +// zero length inside, reports its origin at fraction zero with no normal. +ray_cast_sphere(s: Sphere, origin: Vec3, translation: Vec3, max_fraction: float) -> CastOutput { + output = distance.empty_cast_output() + p = s.center + // The ray with the centre at the origin. + shifted = math.sub(origin, p) + r = s.radius + rr = r * r + length_out = length_scratch() + d = math.get_length_and_normalize(length_out, translation) + length = length_out[0] + if length == 0.0 { + if math.length_squared(shifted) < rr { + output.point = origin + output.hit = true + } + return output + } + // The closest point on the line to the centre: dot(s + t d, d) = 0. + t = 0.0 - math.dot(shifted, d) + c = math.mul_add(shifted, t, d) + cc = math.dot(c, c) + if cc > rr { return output } + h = sqrt(rr - cc) + fraction = t - h + if fraction < 0.0 || max_fraction * length < fraction { + // The intersection is off the segment; an origin inside still counts. + if math.length_squared(shifted) < rr { + output.point = origin + output.hit = true + } + return output + } + hit_point = math.mul_add(shifted, fraction, d) + output.fraction = fraction / length + if output.fraction > max_fraction { output.fraction = max_fraction } + output.normal = math.normalize(hit_point) + output.point = math.mul_add(p, s.radius, output.normal) + output.hit = true + return output +} + +// The same against the sphere's surface only, so a ray from inside hits +// the far side on its way out. +ray_cast_hollow_sphere(s: Sphere, origin: Vec3, translation: Vec3, max_fraction: float) -> CastOutput { + output = distance.empty_cast_output() + p = s.center + shifted = math.sub(origin, p) + d = math.normalize(translation) + t = 0.0 - math.dot(shifted, d) + c = math.mul_add(shifted, t, d) + cc = math.dot(c, c) + r = s.radius + rr = r * r + if cc > rr { return output } + h = sqrt(rr - cc) + fraction = t - h + if fraction < 0.0 { fraction = t + h } + if fraction < 0.0 { return output } + if fraction > max_fraction { return output } + hit_point = math.mul_add(shifted, fraction, d) + output.fraction = fraction + output.normal = math.normalize(hit_point) + output.point = math.mul_add(p, s.radius, output.normal) + output.hit = true + return output +} + +shape_cast_sphere(s: Sphere, proxy: ShapeProxy, translation: Vec3, max_fraction: float, can_encroach: bool) -> CastOutput { + pair = ShapeCastPairInput { proxy_a: sphere_proxy(s), proxy_b: proxy, transform: math.transform_identity(), + translation_b: translation, max_fraction: max_fraction, can_encroach: can_encroach } + return distance.shape_cast(&pair) +} + +var g_length: ptr = null // float[1], the out-parameter of get_length_and_normalize + +length_scratch() -> float[] { + if g_length == null { g_length = core.alloc(8) } + return g_length as float[] +} + +// The mover's plane against a sphere: the normal from the sphere to the +// closest point of the mover's axis, or, when the axis runs through the +// centre, perpendicular to the axis. Returns how many planes (0 or 1). +collide_mover_and_sphere(planes: PlaneResult[], s: Sphere, mover: Capsule) -> int { + total_radius = mover.radius + s.radius + closest = math.point_to_segment_distance(mover.center1, mover.center2, s.center) + length_out = length_scratch() + normal = math.get_length_and_normalize(length_out, math.sub(closest, s.center)) + dist = length_out[0] + if dist > total_radius { return 0 } + if dist < math.LINEAR_SLOP { + axis = math.get_length_and_normalize(length_out, math.sub(mover.center2, mover.center1)) + normal = math.vec3_axis_y() + if length_out[0] > math.LINEAR_SLOP { normal = math.perp(axis) } + dist = 0.0 + } + planes[0] = PlaneResult { plane: Plane { normal: normal, offset: total_radius - dist }, point: s.center, + triangle_index: 0, child_index: 0, material_index: 0 } + return 1 +} + +// --- the capsule ------------------------------------------------------------------------------ + +// The capsule's mass: a cylinder and a sphere (the two caps) about the +// midpoint, the caps carried out to the ends by Steiner, then turned +// from the y axis onto the capsule's. +compute_capsule_mass(c: Capsule, density: float) -> MassData { + c1 = c.center1 + c2 = c.center2 + r = c.radius + cylinder_height = math.distance(c1, c2) + cylinder_volume = math.PI * r * r * cylinder_height + cylinder_mass = cylinder_volume * density + sphere_volume = (4.0 / 3.0) * math.PI * r * r * r + sphere_mass = sphere_volume * density + inertia = math.add_mm(math.cylinder_inertia(cylinder_mass, r, cylinder_height), math.sphere_inertia(sphere_mass, r)) + steiner = 0.125 * sphere_mass * (3.0 * r + 2.0 * cylinder_height) * cylinder_height + inertia.cx.x = inertia.cx.x + steiner + inertia.cz.z = inertia.cz.z + steiner + rotation = math.mat3_identity() + if cylinder_height * cylinder_height > 1000.0 * math.TINY { + direction = math.normalize(math.sub(c2, c1)) + q = math.compute_quat_between_unit_vectors(math.vec3_axis_y(), direction) + rotation = math.make_matrix_from_quat(q) + } + return MassData { mass: sphere_mass + cylinder_mass, center: math.mul_sv(0.5, math.add(c1, c2)), + inertia: math.mul_mm(rotation, math.mul_mm(inertia, math.transpose(rotation))) } +} + +compute_capsule_aabb(c: Capsule, t: Transform) -> AABB { + center1 = math.transform_point(t, c.center1) + center2 = math.transform_point(t, c.center2) + extent = math.vec3(c.radius, c.radius, c.radius) + return AABB { lower: math.sub(math.min_vec3(center1, center2), extent), upper: math.add(math.max_vec3(center1, center2), extent) } +} + +compute_swept_capsule_aabb(c: Capsule, xf1: Transform, xf2: Transform) -> AABB { + r = math.vec3(c.radius, c.radius, c.radius) + a = math.transform_point(xf1, c.center1) + b = math.transform_point(xf1, c.center2) + cc = math.transform_point(xf2, c.center1) + d = math.transform_point(xf2, c.center2) + return AABB { lower: math.sub(math.min_vec3(math.min_vec3(a, b), math.min_vec3(cc, d)), r), + upper: math.add(math.max_vec3(math.max_vec3(a, b), math.max_vec3(cc, d)), r) } +} + +overlap_capsule(c: Capsule, shape_transform: Transform, proxy: ShapeProxy) -> bool { + input = DistanceInput { proxy_a: capsule_proxy(c), proxy_b: proxy, + transform: math.inv_mul_transforms(shape_transform, math.transform_identity()), use_radii: true } + cache = distance.empty_cache() + output = distance.shape_distance(&input, &cache, null, 0) + return output.distance < OVERLAP_SLOP +} + +// The ray against the capsule: an origin inside the infinite cylinder is +// inside the capsule, or casts against the nearer cap; otherwise the +// closest points of the ray's line and the axis give the side hit (the +// axial circle when they are nearly parallel), handed to a cap when the +// hit falls beyond an end. Real-Time Collision Detection 5.1.9. +ray_cast_capsule(c: Capsule, origin: Vec3, translation: Vec3, max_fraction: float) -> CastOutput { + c1 = c.center1 + c2 = c.center2 + r = c.radius + output = distance.empty_cast_output() + d = math.sub(c2, c1) + // A short capsule is a sphere. + tol = 0.01 * math.LINEAR_SLOP + length_squared = math.length_squared(d) + if length_squared < tol * tol { + return ray_cast_sphere(Sphere { center: math.mul_sv(0.5, math.add(c1, c2)), radius: r }, origin, translation, max_fraction) + } + // The ray's origin from the first centre, projected onto the axis. + s = math.sub(origin, c1) + length = sqrt(length_squared) + axis = math.mul_sv(1.0 / length, d) + u = math.dot(s, axis) + c_axis = math.mul_sv(u, axis) + sc = math.sub(s, c_axis) + sc2 = math.length_squared(sc) + if sc2 < r * r { + // Inside the infinite cylinder: inside the capsule, or facing a cap. + u_clamped = math.clamp_float(u, 0.0, length) + cp = math.mul_sv(u_clamped, axis) + scp = math.sub(s, cp) + if math.length_squared(scp) < r * r { + output.hit = true + output.point = origin + return output + } + return ray_cast_sphere(Sphere { center: math.add(c1, cp), radius: r }, origin, translation, max_fraction) + } + // A zero-length ray out here starts outside, so it misses. + length_out = length_scratch() + ray_axis = math.get_length_and_normalize(length_out, translation) + ray_length = length_out[0] + if ray_length == 0.0 { return output } + // The projected ray entirely beyond an end misses. + v = u + max_fraction * math.dot(translation, axis) + if (u < 0.0 - r && v < 0.0 - r) || (length + r < u && length + r < v) { return output } + + a1 = axis + a2 = ray_axis + a12 = math.dot(a1, a2) + // The ray's distance to the near intersection with the infinite cylinder, in length units. + tr = 0.0 + det = 1.0 - a12 * a12 + if det < math.EPSILON { + // Nearly parallel: the ray against the circle that is the cylinder + // seen along its axis, from the ray's origin. + perp = math.mul_sub(a2, a12, a1) + perp2 = math.length_squared(perp) + beta = math.dot(sc, perp) + gamma = sc2 - r * r + disc = beta * beta - perp2 * gamma + // Casting away from the axis, or never closing to the radius. + if beta >= 0.0 || disc < 0.0 { return output } + // The near root in the form that avoids the cancellation as the ray nears parallel. + tr = gamma / (0.0 - beta + sqrt(disc)) + } else { + // The closest points of the infinite ray and the infinite axis. + inv_det = 1.0 / det + sa1 = u + sa2 = math.dot(s, a2) + t1 = (sa1 - a12 * sa2) * inv_det + t2 = (a12 * sa1 - sa2) * inv_det + p1 = math.mul_sv(t1, a1) + p2 = math.mul_add(s, t2, a2) + g = math.sub(p2, p1) + g2 = math.length_squared(g) + if g2 > r * r { return output } + // The ray into the cylinder, relative to the closest point (like the sphere). + h = sqrt((r * r - g2) * inv_det) + tr = t2 - h + } + if tr < 0.0 || max_fraction * ray_length < tr { return output } + // The hit's place along the axis. + tc = u + tr * a12 + if tc < 0.0 { return ray_cast_sphere(Sphere { center: c1, radius: r }, origin, translation, max_fraction) } + if length < tc { return ray_cast_sphere(Sphere { center: c2, radius: r }, origin, translation, max_fraction) } + // On the side, relative to c1. + p = math.mul_add(s, tr, ray_axis) + output.point = math.add(c1, p) + output.normal = math.normalize(math.mul_sub(p, tc, axis)) + output.fraction = math.clamp_float(tr / ray_length, 0.0, max_fraction) + output.hit = true + return output +} + +shape_cast_capsule(c: Capsule, proxy: ShapeProxy, translation: Vec3, max_fraction: float, can_encroach: bool) -> CastOutput { + pair = ShapeCastPairInput { proxy_a: capsule_proxy(c), proxy_b: proxy, transform: math.transform_identity(), + translation_b: translation, max_fraction: max_fraction, can_encroach: can_encroach } + return distance.shape_cast(&pair) +} + +// The mover's plane against a capsule: the normal between the closest +// points of the two axes, or, when they cross, perpendicular to the +// mover's axis. +collide_mover_and_capsule(planes: PlaneResult[], c: Capsule, mover: Capsule) -> int { + total_radius = mover.radius + c.radius + approach = math.segment_distance(c.center1, c.center2, mover.center1, mover.center2) + length_out = length_scratch() + normal = math.get_length_and_normalize(length_out, math.sub(approach.point2, approach.point1)) + dist = length_out[0] + if dist > total_radius { return 0 } + if dist < math.LINEAR_SLOP { + mover_axis = math.get_length_and_normalize(length_out, math.sub(mover.center2, mover.center1)) + normal = math.vec3_axis_y() + if length_out[0] > math.LINEAR_SLOP { normal = math.perp(mover_axis) } + dist = 0.0 + } + planes[0] = PlaneResult { plane: Plane { normal: normal, offset: total_radius - dist }, point: approach.point1, + triangle_index: 0, child_index: 0, material_index: 0 } + return 1 +} + +// --- the hull's mover ------------------------------------------------------------------------- + +// The mover's plane against a hull by GJK on the core segment: none when +// the segment is inside (a mesh could not do better, so a hull does not). +collide_mover_and_hull(planes: PlaneResult[], h: *HullData, mover: Capsule) -> int { + buffer = center_buffer() + buffer[0] = mover.center1 + buffer[1] = mover.center2 + input = DistanceInput { proxy_a: hull.hull_proxy(h), proxy_b: distance.shape_proxy(g_center, 2, mover.radius), + transform: math.transform_identity(), use_radii: false } + cache = distance.empty_cache() + output = distance.shape_distance(&input, &cache, null, 0) + if output.distance == 0.0 { return 0 } + if output.distance <= mover.radius { + planes[0] = PlaneResult { plane: Plane { normal: output.normal, offset: mover.radius - output.distance }, point: output.point_a, + triangle_index: 0, child_index: 0, material_index: 0 } + return 1 + } + return 0 +} + +// --- any shape ------------------------------------------------------------------------------------ + +compute_shape_aabb(shape: *Shape, t: Transform) -> AABB { + kind = shape.kind + if kind == SHAPE_CAPSULE { return compute_capsule_aabb(shape.capsule, t) } + if kind == SHAPE_HEIGHT_FIELD { return height_field.compute_height_field_aabb(shape.height_field, t) } + if kind == SHAPE_HULL { return hull.compute_hull_aabb(shape.hull, t) } + if kind == SHAPE_MESH { return mesh.compute_mesh_aabb(shape.mesh.data, t, shape.mesh.scale) } + if kind == SHAPE_SPHERE { return compute_sphere_aabb(shape.sphere, t) } + return AABB { lower: t.p, upper: t.p } +} + +// The bounds under a transform inflated by a margin: built in the shape's +// frame and inflated before the translation, so the margin survives far +// from the origin. +compute_fat_shape_aabb(shape: *Shape, t: Transform, extra: float) -> AABB { + r = math.vec3(extra, extra, extra) + rotation = Transform { p: math.vec3_zero(), q: t.q } + local_box = compute_shape_aabb(shape, rotation) + local_box.lower = math.sub(local_box.lower, r) + local_box.upper = math.add(local_box.upper, r) + return math.offset_aabb(local_box, t.p) +} + +// The bounds covering a convex shape from the start of a sweep to a time along it. +compute_swept_shape_aabb(shape: *Shape, sweep: Sweep, time: float) -> AABB { + xf1 = Transform { p: math.sub(sweep.c1, math.rotate_vector(sweep.q1, sweep.local_center)), q: sweep.q1 } + xf2 = distance.get_sweep_transform(sweep, time) + kind = shape.kind + if kind == SHAPE_CAPSULE { return compute_swept_capsule_aabb(shape.capsule, xf1, xf2) } + if kind == SHAPE_HULL { return hull.compute_swept_hull_aabb(shape.hull, xf1, xf2) } + if kind == SHAPE_SPHERE { return compute_swept_sphere_aabb(shape.sphere, xf1, xf2) } + return AABB { lower: xf1.p, upper: xf1.p } +} + +get_shape_centroid(shape: *Shape) -> Vec3 { + kind = shape.kind + if kind == SHAPE_CAPSULE { return math.lerp(shape.capsule.center1, shape.capsule.center2, 0.5) } + if kind == SHAPE_SPHERE { return shape.sphere.center } + if kind == SHAPE_HULL { return shape.hull.center } + if kind == SHAPE_MESH { return math.aabb_center(mesh.compute_mesh_aabb(shape.mesh.data, math.transform_identity(), shape.mesh.scale)) } + if kind == SHAPE_HEIGHT_FIELD { return math.aabb_center(height_field.compute_height_field_aabb(shape.height_field, math.transform_identity())) } + return math.vec3_zero() +} + +// The surface area, as the reference has it (a rough measure for explosions). +get_shape_area(shape: *Shape) -> float { + kind = shape.kind + if kind == SHAPE_CAPSULE { return 2.0 * math.length(math.sub(shape.capsule.center1, shape.capsule.center2)) + 2.0 * math.PI * shape.capsule.radius } + if kind == SHAPE_HULL { return shape.hull.surface_area } + if kind == SHAPE_SPHERE { return 2.0 * math.PI * shape.sphere.radius } + return 0.0 +} + +// The area of the shape's shadow on a plane. +get_shape_projected_area(shape: *Shape, plane_normal: Vec3) -> float { + kind = shape.kind + if kind == SHAPE_CAPSULE { + radius = shape.capsule.radius + axis = math.sub(shape.capsule.center2, shape.capsule.center1) + projected_length = math.length(math.cross(axis, plane_normal)) + return math.PI * radius * radius + 2.0 * radius * projected_length + } + if kind == SHAPE_HULL { return hull.compute_hull_projected_area(shape.hull, plane_normal) } + if kind == SHAPE_SPHERE { return math.PI * shape.sphere.radius * shape.sphere.radius } + return 0.0 +} + +compute_shape_mass(shape: *Shape) -> MassData { + kind = shape.kind + if kind == SHAPE_CAPSULE { return compute_capsule_mass(shape.capsule, shape.density) } + if kind == SHAPE_HULL { return hull.compute_hull_mass(shape.hull, shape.density) } + if kind == SHAPE_SPHERE { return compute_sphere_mass(shape.sphere, shape.density) } + return MassData { mass: 0.0, center: math.vec3_zero(), inertia: math.make_diagonal_matrix(0.0, 0.0, 0.0) } +} + +// The corner of a box farthest from a point. +farthest_point_on_aabb(b: AABB, p: Vec3) -> Vec3 { + x = b.upper.x + if p.x - b.lower.x > b.upper.x - p.x { x = b.lower.x } + y = b.upper.y + if p.y - b.lower.y > b.upper.y - p.y { y = b.lower.y } + z = b.upper.z + if p.z - b.lower.z > b.upper.z - p.z { z = b.lower.z } + return math.vec3(x, y, z) +} + +// The shape's reach from a local centre: the least radius (for the sleep +// and continuous thresholds) and the farthest extent along each axis. +compute_shape_extent(shape: *Shape, local_center: Vec3) -> ShapeExtent { + kind = shape.kind + if kind == SHAPE_CAPSULE { + radius = shape.capsule.radius + c1 = math.sub(shape.capsule.center1, local_center) + c2 = math.sub(shape.capsule.center2, local_center) + r = math.vec3(radius, radius, radius) + return ShapeExtent { min_extent: radius, max_extent: math.add(math.max_vec3(math.abs_vec3(c1), math.abs_vec3(c2)), r) } + } + if kind == SHAPE_SPHERE { + radius = shape.sphere.radius + h = math.abs_vec3(math.sub(shape.sphere.center, local_center)) + r = math.vec3(radius, radius, radius) + return ShapeExtent { min_extent: radius, max_extent: math.add(h, r) } + } + if kind == SHAPE_HULL { return hull.compute_hull_extent(shape.hull, local_center) } + if kind == SHAPE_MESH { + // Needed for a kinematic mesh to sleep. + return aabb_extent(mesh.compute_mesh_aabb(shape.mesh.data, math.transform_identity(), shape.mesh.scale), local_center) + } + if kind == SHAPE_HEIGHT_FIELD { + return aabb_extent(height_field.compute_height_field_aabb(shape.height_field, math.transform_identity()), local_center) + } + return ShapeExtent { min_extent: 0.0, max_extent: math.vec3_zero() } +} + +aabb_extent(box: AABB, local_center: Vec3) -> ShapeExtent { + r1 = math.length(math.sub(box.lower, local_center)) + r2 = math.length(math.sub(box.upper, local_center)) + p = farthest_point_on_aabb(box, local_center) + return ShapeExtent { min_extent: math.min_float(r1, r2), max_extent: math.abs_vec3(math.sub(p, local_center)) } +} + +// A ray in world space against the shape under its transform. +ray_cast_shape(shape: *Shape, t: Transform, origin: Vec3, translation: Vec3, max_fraction: float) -> CastOutput { + local_origin = math.inv_transform_point(t, origin) + local_translation = math.inv_rotate_vector(t.q, translation) + output = distance.empty_cast_output() + kind = shape.kind + if kind == SHAPE_CAPSULE { output = ray_cast_capsule(shape.capsule, local_origin, local_translation, max_fraction) } + else if kind == SHAPE_SPHERE { output = ray_cast_sphere(shape.sphere, local_origin, local_translation, max_fraction) } + else if kind == SHAPE_HULL { output = hull.ray_cast_hull(shape.hull, local_origin, local_translation, max_fraction) } + else if kind == SHAPE_MESH { output = mesh.ray_cast_mesh(shape.mesh, local_origin, local_translation, max_fraction) } + else if kind == SHAPE_HEIGHT_FIELD { output = height_field.ray_cast_height_field(shape.height_field, local_origin, local_translation, max_fraction) } + else { return output } + output.point = math.transform_point(t, output.point) + output.normal = math.rotate_vector(t.q, output.normal) + return output +} + +var g_local: ptr = null // Vec3[MAX_SHAPE_CAST_POINTS], a proxy brought into the shape's frame + +local_buffer() -> ptr { + if g_local == null { g_local = core.alloc(distance.MAX_SHAPE_CAST_POINTS * sizeof(Vec3)) } + return g_local +} + +// A proxy in world space swept against the shape under its transform. +shape_cast_shape(shape: *Shape, t: Transform, proxy: ShapeProxy, translation: Vec3, max_fraction: float, can_encroach: bool) -> CastOutput { + buffer = local_buffer() + local_proxy = mesh.make_local_proxy(proxy, t, buffer) + local_translation = math.inv_rotate_vector(t.q, translation) + output = distance.empty_cast_output() + kind = shape.kind + if kind == SHAPE_CAPSULE { output = shape_cast_capsule(shape.capsule, local_proxy, local_translation, max_fraction, can_encroach) } + else if kind == SHAPE_HEIGHT_FIELD { output = height_field.shape_cast_height_field(shape.height_field, local_proxy, local_translation, max_fraction, can_encroach) } + else if kind == SHAPE_HULL { output = hull.shape_cast_hull(shape.hull, local_proxy, local_translation, max_fraction, can_encroach) } + else if kind == SHAPE_MESH { output = mesh.shape_cast_mesh(shape.mesh, local_proxy, local_translation, max_fraction, can_encroach) } + else if kind == SHAPE_SPHERE { output = shape_cast_sphere(shape.sphere, local_proxy, local_translation, max_fraction, can_encroach) } + else { return output } + output.point = math.transform_point(t, output.point) + output.normal = math.rotate_vector(t.q, output.normal) + return output +} + +overlap_shape(shape: *Shape, t: Transform, proxy: ShapeProxy) -> bool { + kind = shape.kind + if kind == SHAPE_CAPSULE { return overlap_capsule(shape.capsule, t, proxy) } + if kind == SHAPE_HEIGHT_FIELD { return height_field.overlap_height_field(shape.height_field, t, proxy) } + if kind == SHAPE_HULL { return hull.overlap_hull(shape.hull, t, proxy) } + if kind == SHAPE_MESH { return mesh.overlap_mesh(shape.mesh, t, proxy) } + if kind == SHAPE_SPHERE { return overlap_sphere(shape.sphere, t, proxy) } + return false +} + +// The planes of the shape against the character mover's capsule, in +// world space, with the material index clamped to the shape's. +collide_mover(planes: PlaneResult[], capacity: int, shape: *Shape, t: Transform, mover: Capsule) -> int { + if capacity == 0 { return 0 } + local_mover = Capsule { center1: math.inv_transform_point(t, mover.center1), center2: math.inv_transform_point(t, mover.center2), + radius: mover.radius } + plane_count = 0 + kind = shape.kind + if kind == SHAPE_CAPSULE { plane_count = collide_mover_and_capsule(planes, shape.capsule, local_mover) } + else if kind == SHAPE_SPHERE { plane_count = collide_mover_and_sphere(planes, shape.sphere, local_mover) } + else if kind == SHAPE_HULL { plane_count = collide_mover_and_hull(planes, shape.hull, local_mover) } + else if kind == SHAPE_MESH { plane_count = mesh.collide_mover_and_mesh(planes, capacity, shape.mesh, local_mover) } + else if kind == SHAPE_HEIGHT_FIELD { plane_count = height_field.collide_mover_and_height_field(planes, capacity, shape.height_field, local_mover) } + i = 0 + while i < plane_count { + planes[i].plane.normal = math.rotate_vector(t.q, planes[i].plane.normal) + planes[i].point = math.transform_point(t, planes[i].point) + planes[i].material_index = math.clamp_int(planes[i].material_index, 0, shape.material_count - 1) + i = i + 1 + } + return plane_count +} + +// A convex shape as a proxy for GJK; the sphere's and capsule's points +// live in the module's buffer until the next call. +make_shape_proxy(shape: *Shape) -> ShapeProxy { + kind = shape.kind + if kind == SHAPE_CAPSULE { return capsule_proxy(shape.capsule) } + if kind == SHAPE_SPHERE { return sphere_proxy(shape.sphere) } + if kind == SHAPE_HULL { return hull.hull_proxy(shape.hull) } + return distance.shape_proxy(null, 0, 0.0) +} diff --git a/aephysics/test_shape.ae b/aephysics/test_shape.ae new file mode 100644 index 0000000..3ef824f --- /dev/null +++ b/aephysics/test_shape.ae @@ -0,0 +1,833 @@ +// aephysics.shape against the reference's (Box3D's) test_shape.c: the +// masses of a sphere, an analytic box, translated, rotated and +// transformed boxes and a capsule bracketed by hulls, the bounds, the +// ray against a sphere (hits, misses, the clip, the interior, the +// graze), against a capsule (the side, oblique, the caps, misses, the +// interior, degenerate, the clip, near parallel), the overlap convention +// across the solids, the far origin's precision, and the cast through +// the dispatch; and beyond it the dispatch of every kind under a +// transform (bounds, fat and swept bounds, centroid, areas, mass, extent, +// ray, shape cast, overlap, the mover) and the filters. The shape name +// and flag tests need a world and come with the dynamics. + +import std.string +import aephysics.math +import aephysics.core +import aephysics.hull +import aephysics.distance +import aephysics.manifold +import aephysics.mesh +import aephysics.height_field +import aephysics.shape + +extern calloc(count: int, size: int) -> ptr +extern exit(code: int) +extern free(p: ptr) +extern sin(x: float) -> float +extern cos(x: float) -> float +extern sqrt(x: float) -> float + +var failures = 0 +var checks = 0 + +ensure(name: string, ok: bool) { + checks = checks + 1 + if !ok { + println("shape: FAIL ${name}") + failures = failures + 1 + } +} + +small(name: string, value: float, tolerance: float) { + ensure("${name} (${value})", math.abs_float(value) < tolerance) +} + +// The reference's fixtures: a capsule along x, a sphere at x = 1, a unit cube. +test_capsule() -> Capsule { return manifold.capsule(math.vec3(0.0 - 1.0, 0.0, 0.0), math.vec3(1.0, 0.0, 0.0), 1.0) } +test_sphere() -> Sphere { return manifold.sphere(math.vec3(1.0, 0.0, 0.0), 1.0) } +// The capsule the ray tests share: along x from -2 to 2, radius 1. +ray_capsule() -> Capsule { return manifold.capsule(math.vec3(0.0 - 2.0, 0.0, 0.0), math.vec3(2.0, 0.0, 0.0), 1.0) } +unit_sphere() -> Sphere { return manifold.sphere(math.vec3_zero(), 1.0) } + +// A surface hit: the normal outward toward the ray, the point on the +// surface and on the ray at the fraction. +check_hit(name: string, out: CastOutput, origin: Vec3, translation: Vec3, point: Vec3, normal: Vec3, fraction: float, tol: float) { + ensure("${name} hit", out.hit) + small("${name} fraction", out.fraction - fraction, tol) + small("${name} point x", out.point.x - point.x, tol) + small("${name} point y", out.point.y - point.y, tol) + small("${name} point z", out.point.z - point.z, tol) + small("${name} normal x", out.normal.x - normal.x, tol) + small("${name} normal y", out.normal.y - normal.y, tol) + small("${name} normal z", out.normal.z - normal.z, tol) + on_ray = math.mul_add(origin, out.fraction, translation) + small("${name} on the ray", math.distance(out.point, on_ray), tol) +} + +// The initial-overlap convention: the origin at fraction zero with no normal. +check_initial_overlap(name: string, out: CastOutput, origin: Vec3) { + ensure("${name} hit", out.hit) + ensure("${name} fraction zero", out.fraction == 0.0) + small("${name} at the origin", math.distance(out.point, origin), math.EPSILON) + ensure("${name} no normal", out.normal.x == 0.0 && out.normal.y == 0.0 && out.normal.z == 0.0) +} + +check_inertia_equal(name: string, a: Matrix3, b: Matrix3, tol: float) { + d = math.sub_mm(a, b) + small("${name} xx", d.cx.x, tol) + small("${name} xy", d.cx.y, tol) + small("${name} xz", d.cx.z, tol) + small("${name} yx", d.cy.x, tol) + small("${name} yy", d.cy.y, tol) + small("${name} yz", d.cy.z, tol) + small("${name} zx", d.cz.x, tol) + small("${name} zy", d.cz.y, tol) + small("${name} zz", d.cz.z, tol) +} + +test_mass() { + // The sphere: its inertia is about its own centre, so the offset does not appear. + md = shape.compute_sphere_mass(test_sphere(), 1.0) + mass = 4.0 / 3.0 * math.PI + small("sphere mass", md.mass - mass, math.EPSILON) + ensure("sphere centre", md.center.x == 1.0 && md.center.y == 0.0) + inertia = 2.0 / 5.0 * mass + small("sphere inertia xx", md.inertia.cx.x - inertia, math.EPSILON) + small("sphere inertia yy", md.inertia.cy.y - inertia, math.EPSILON) + small("sphere inertia zz", md.inertia.cz.z - inertia, math.EPSILON) + + // The analytic box. + box = hull.make_box_hull(1.0, 1.0, 1.0) + md = hull.compute_hull_mass(box, 1.0) + mass = 8.0 + small("box mass", md.mass - mass, math.EPSILON) + small("box centre x", md.center.x, math.EPSILON) + small("box centre y", md.center.y, math.EPSILON) + small("box centre z", md.center.z, math.EPSILON) + inertia = (1.0 / 12.0) * mass * (4.0 + 4.0) + small("box inertia xx", md.inertia.cx.x - inertia, 2.0 * math.EPSILON) + small("box inertia yy", md.inertia.cy.y - inertia, 2.0 * math.EPSILON) + small("box inertia zz", md.inertia.cz.z - inertia, 2.0 * math.EPSILON) + + // A translated box keeps its central inertia and moves its centre. + offset = math.vec3(0.4, 0.0 - 0.7, 0.1) + t = Transform { p: offset, q: math.quat_identity() } + b1 = hull.make_box_hull(0.25, 0.5, 0.3) + b2 = hull.make_transformed_box_hull(0.25, 0.5, 0.3, t) + m1 = hull.compute_hull_mass(b1, 1.0) + m2 = hull.compute_hull_mass(b2, 1.0) + small("translated mass", m1.mass - m2.mass, math.EPSILON) + check_inertia_equal("translated inertia", b1.central_inertia, b2.central_inertia, math.EPSILON) + small("translated centre x", m2.center.x - offset.x, math.EPSILON) + small("translated centre y", m2.center.y - offset.y, math.EPSILON) + small("translated centre z", m2.center.z - offset.z, math.EPSILON) + hull.destroy_hull(b1) + hull.destroy_hull(b2) + + // A box rotated y onto z is the box with those extents swapped. + q = math.compute_quat_between_unit_vectors(math.vec3_axis_y(), math.vec3_axis_z()) + t = Transform { p: math.vec3_zero(), q: q } + b1 = hull.make_transformed_box_hull(0.25, 0.5, 0.3, t) + b2 = hull.make_box_hull(0.25, 0.3, 0.5) + m1 = hull.compute_hull_mass(b1, 1.0) + m2 = hull.compute_hull_mass(b2, 1.0) + small("rotated mass", m1.mass - m2.mass, math.EPSILON) + check_inertia_equal("rotated inertia", b1.central_inertia, b2.central_inertia, math.EPSILON) + small("rotated centre x", m1.center.x - m2.center.x, math.EPSILON) + small("rotated centre y", m1.center.y - m2.center.y, math.EPSILON) + small("rotated centre z", m1.center.z - m2.center.z, math.EPSILON) + hull.destroy_hull(b1) + hull.destroy_hull(b2) + + // Both at once. + t = Transform { p: offset, q: q } + b1 = hull.make_transformed_box_hull(0.25, 0.5, 0.3, t) + b2 = hull.make_box_hull(0.25, 0.3, 0.5) + m1 = hull.compute_hull_mass(b1, 1.0) + m2 = hull.compute_hull_mass(b2, 1.0) + small("transformed mass", m1.mass - m2.mass, math.EPSILON) + check_inertia_equal("transformed inertia", b1.central_inertia, b2.central_inertia, math.EPSILON) + small("transformed centre x", m1.center.x - offset.x, math.EPSILON) + small("transformed centre y", m1.center.y - offset.y, math.EPSILON) + small("transformed centre z", m1.center.z - offset.z, math.EPSILON) + hull.destroy_hull(b1) + hull.destroy_hull(b2) + + // The capsule between the box that contains it and the hull inside it. + c = test_capsule() + radius = c.radius + length = math.distance(c.center1, c.center2) + md = shape.compute_capsule_mass(c, 1.0) + r = hull.make_box_hull(radius + 0.5 * length, radius, radius) + md_upper = hull.compute_hull_mass(r, 1.0) + n = 4 + points_block = calloc(2 * n * n, sizeof(Vec3)) + points = points_block as Vec3[] + d = math.PI / ((n as float) - 1.0) + angle1 = 0.0 - 0.5 * math.PI + index = 0 + i = 0 + while i < n { + s1 = sin(angle1) + c1 = cos(angle1) + angle2 = 0.0 - 0.5 * math.PI + j = 0 + while j < n { + points[index] = math.vec3(1.0 + radius * c1, radius * s1 * cos(angle2), radius * s1 * sin(angle2)) + angle2 = angle2 + d + index = index + 1 + j = j + 1 + } + angle1 = angle1 + d + i = i + 1 + } + angle1 = 0.5 * math.PI + i = 0 + while i < n { + s1 = sin(angle1) + c1 = cos(angle1) + angle2 = 0.0 - 0.5 * math.PI + j = 0 + while j < n { + points[index] = math.vec3(0.0 - 1.0 + radius * c1, radius * s1 * cos(angle2), radius * s1 * sin(angle2)) + angle2 = angle2 + d + index = index + 1 + j = j + 1 + } + angle1 = angle1 + d + i = i + 1 + } + ensure("capsule sample count", index == 2 * n * n) + inner = hull.create_hull(points, 2 * n * n, 2 * n * n) + md_lower = hull.compute_hull_mass(inner, 1.0) + ensure("capsule mass bracketed", md_lower.mass < md.mass && md.mass < md_upper.mass) + ensure("capsule inertia xx bracketed", md_lower.inertia.cx.x < md.inertia.cx.x && md.inertia.cx.x < md_upper.inertia.cx.x) + ensure("capsule inertia yy bracketed", md_lower.inertia.cy.y < md.inertia.cy.y && md.inertia.cy.y < md_upper.inertia.cy.y) + ensure("capsule inertia zz bracketed", md_lower.inertia.cz.z < md.inertia.cz.z && md.inertia.cz.z < md_upper.inertia.cz.z) + hull.destroy_hull(inner) + hull.destroy_hull(r) + free(points_block) + + // A capsule along y has the analytic cylinder-plus-caps inertia about y. + cy = manifold.capsule(math.vec3(0.0, 0.0 - 1.0, 0.0), math.vec3(0.0, 1.0, 0.0), 0.5) + md = shape.compute_capsule_mass(cy, 2.0) + cylinder_mass = 2.0 * math.PI * 0.25 * 2.0 + sphere_mass = 2.0 * (4.0 / 3.0) * math.PI * 0.125 + small("capsule mass", md.mass - cylinder_mass - sphere_mass, 0.000001) + small("capsule inertia yy", md.inertia.cy.y - 0.5 * cylinder_mass * 0.25 - 0.4 * sphere_mass * 0.25, 0.000001) + small("capsule inertia xx = zz", md.inertia.cx.x - md.inertia.cz.z, 0.000001) + ensure("capsule inertia diagonal", math.abs_float(md.inertia.cx.y) < 0.000001 && math.abs_float(md.inertia.cy.z) < 0.000001) + // The same capsule along x: the roles of xx and yy swap. + cx = manifold.capsule(math.vec3(0.0 - 1.0, 0.0, 0.0), math.vec3(1.0, 0.0, 0.0), 0.5) + mx = shape.compute_capsule_mass(cx, 2.0) + small("capsule rotated xx", mx.inertia.cx.x - md.inertia.cy.y, 0.000001) + small("capsule rotated yy", mx.inertia.cy.y - md.inertia.cx.x, 0.000001) + + // The dispatch. + sh = shape.sphere_shape(test_sphere(), 1.0) + small("shape mass sphere exactly", shape.compute_shape_mass(&sh).mass - 4.0 / 3.0 * math.PI, math.EPSILON) + sh = shape.hull_shape(box, 2.0) + small("shape mass hull", shape.compute_shape_mass(&sh).mass - 16.0, math.EPSILON) + sh = shape.capsule_shape(cy, 2.0) + small("shape mass capsule", shape.compute_shape_mass(&sh).mass - cylinder_mass - sphere_mass, 0.000001) + hull.destroy_hull(box) +} + +test_aabb() { + b = shape.compute_sphere_aabb(test_sphere(), math.transform_identity()) + small("sphere lower x", b.lower.x, math.EPSILON) + small("sphere lower y", b.lower.y + 1.0, math.EPSILON) + small("sphere lower z", b.lower.z + 1.0, math.EPSILON) + small("sphere upper x", b.upper.x - 2.0, math.EPSILON) + small("sphere upper y", b.upper.y - 1.0, math.EPSILON) + small("sphere upper z", b.upper.z - 1.0, math.EPSILON) + b = shape.compute_capsule_aabb(test_capsule(), math.transform_identity()) + small("capsule lower x", b.lower.x + 2.0, math.EPSILON) + small("capsule lower y", b.lower.y + 1.0, math.EPSILON) + small("capsule lower z", b.lower.z + 1.0, math.EPSILON) + small("capsule upper x", b.upper.x - 2.0, math.EPSILON) + small("capsule upper y", b.upper.y - 1.0, math.EPSILON) + small("capsule upper z", b.upper.z - 1.0, math.EPSILON) + box = hull.make_box_hull(1.0, 1.0, 1.0) + b = hull.compute_hull_aabb(box, math.transform_identity()) + small("box lower x", b.lower.x + 1.0, math.EPSILON) + small("box lower y", b.lower.y + 1.0, math.EPSILON) + small("box lower z", b.lower.z + 1.0, math.EPSILON) + small("box upper x", b.upper.x - 1.0, math.EPSILON) + small("box upper y", b.upper.y - 1.0, math.EPSILON) + small("box upper z", b.upper.z - 1.0, math.EPSILON) + + // Under a transform, through the dispatch, and fat. + q = math.compute_quat_between_unit_vectors(math.vec3_axis_x(), math.vec3_axis_y()) + t = Transform { p: math.vec3(10.0, 20.0, 30.0), q: q } + sh = shape.capsule_shape(test_capsule(), 1.0) + b = shape.compute_shape_aabb(&sh, t) + small("rotated capsule lower x", b.lower.x - 9.0, 0.000001) + small("rotated capsule upper x", b.upper.x - 11.0, 0.000001) + small("rotated capsule lower y", b.lower.y - 18.0, 0.000001) + small("rotated capsule upper y", b.upper.y - 22.0, 0.000001) + fat = shape.compute_fat_shape_aabb(&sh, t, 0.1) + small("fat capsule lower y", fat.lower.y - 17.9, 0.000001) + small("fat capsule upper z", fat.upper.z - 31.1, 0.000001) + sh = shape.hull_shape(box, 1.0) + b = shape.compute_shape_aabb(&sh, t) + small("moved box lower z", b.lower.z - 29.0, 0.000001) + sh = shape.sphere_shape(test_sphere(), 1.0) + b = shape.compute_shape_aabb(&sh, t) + small("rotated sphere upper y", b.upper.y - 22.0, 0.000001) + small("rotated sphere upper x", b.upper.x - 11.0, 0.000001) + + // The swept bounds cover both ends of the sweep. + sweep = Sweep { local_center: math.vec3_zero(), c1: math.vec3_zero(), c2: math.vec3(5.0, 0.0, 0.0), q1: math.quat_identity(), q2: math.quat_identity() } + b = shape.compute_swept_shape_aabb(&sh, sweep, 1.0) + small("swept sphere lower x", b.lower.x, 0.000001) + small("swept sphere upper x", b.upper.x - 7.0, 0.000001) + b = shape.compute_swept_shape_aabb(&sh, sweep, 0.5) + small("half swept sphere upper x", b.upper.x - 4.5, 0.000001) + sh = shape.capsule_shape(test_capsule(), 1.0) + b = shape.compute_swept_shape_aabb(&sh, sweep, 1.0) + small("swept capsule upper x", b.upper.x - 7.0, 0.000001) + sh = shape.hull_shape(box, 1.0) + b = shape.compute_swept_shape_aabb(&sh, sweep, 1.0) + small("swept hull upper x", b.upper.x - 6.0, 0.000001) + small("swept hull lower x", b.lower.x + 1.0, 0.000001) + + // A mesh and a height field through the dispatch. + grid = mesh.create_grid_mesh(4, 4, 1.0, 1, false) + sh = shape.mesh_shape(mesh.mesh(grid, math.vec3_one()), 1) + b = shape.compute_shape_aabb(&sh, math.transform_identity()) + small("mesh extent x", b.upper.x - b.lower.x - 4.0, 0.000001) + field = height_field.create_grid(4, 4, math.vec3(2.0, 1.0, 2.0), false) + sh = shape.height_field_shape(field, 1) + b = shape.compute_shape_aabb(&sh, t) + small("height field lower x", b.lower.x - 10.0, 0.000001) + small("height field upper y", b.upper.y - 26.0, 0.01) + mesh.destroy_mesh(grid) + height_field.destroy_height_field(field) + hull.destroy_hull(box) +} + +test_ray_cast_sphere() { + s = unit_sphere() + // Along each axis: the surface at 3 over a ray of 8. + origin = math.vec3(0.0 - 4.0, 0.0, 0.0) + translation = math.vec3(8.0, 0.0, 0.0) + check_hit("sphere -x", shape.ray_cast_sphere(s, origin, translation, 1.0), origin, translation, math.vec3(0.0 - 1.0, 0.0, 0.0), math.vec3(0.0 - 1.0, 0.0, 0.0), 3.0 / 8.0, 0.00001) + origin = math.vec3(0.0, 4.0, 0.0) + translation = math.vec3(0.0, 0.0 - 8.0, 0.0) + check_hit("sphere +y", shape.ray_cast_sphere(s, origin, translation, 1.0), origin, translation, math.vec3(0.0, 1.0, 0.0), math.vec3(0.0, 1.0, 0.0), 3.0 / 8.0, 0.00001) + origin = math.vec3(0.0, 0.0, 0.0 - 4.0) + translation = math.vec3(0.0, 0.0, 8.0) + check_hit("sphere -z", shape.ray_cast_sphere(s, origin, translation, 1.0), origin, translation, math.vec3(0.0, 0.0, 0.0 - 1.0), math.vec3(0.0, 0.0, 0.0 - 1.0), 3.0 / 8.0, 0.00001) + // An offset centre, hit partway. + s2 = manifold.sphere(math.vec3(5.0, 0.0, 0.0), 2.0) + origin = math.vec3_zero() + translation = math.vec3(10.0, 0.0, 0.0) + check_hit("sphere offset", shape.ray_cast_sphere(s2, origin, translation, 1.0), origin, translation, math.vec3(3.0, 0.0, 0.0), math.vec3(0.0 - 1.0, 0.0, 0.0), 0.3, 0.00001) + // A diagonal through the centre. + k = 0.70710678 + origin = math.vec3(0.0 - 3.0, 0.0 - 3.0, 0.0) + translation = math.vec3(6.0, 6.0, 0.0) + check_hit("sphere diagonal", shape.ray_cast_sphere(s, origin, translation, 1.0), origin, translation, math.vec3(0.0 - k, 0.0 - k, 0.0), math.vec3(0.0 - k, 0.0 - k, 0.0), 0.382149, 0.0001) + + // Misses: pointing away, wide, stopping short. + ensure("sphere away", shape.ray_cast_sphere(s, math.vec3(0.0 - 4.0, 0.0, 0.0), math.vec3(0.0 - 8.0, 0.0, 0.0), 1.0).hit == false) + ensure("sphere wide", shape.ray_cast_sphere(s, math.vec3(0.0 - 4.0, 3.0, 0.0), math.vec3(8.0, 0.0, 0.0), 1.0).hit == false) + ensure("sphere short", shape.ray_cast_sphere(s, math.vec3(0.0 - 4.0, 0.0, 0.0), math.vec3(8.0, 0.0, 0.0), 0.3).hit == false) + // The clip straddling the surface at 3/8. + ensure("sphere clipped", shape.ray_cast_sphere(s, math.vec3(0.0 - 4.0, 0.0, 0.0), math.vec3(8.0, 0.0, 0.0), 0.374).hit == false) + out = shape.ray_cast_sphere(s, math.vec3(0.0 - 4.0, 0.0, 0.0), math.vec3(8.0, 0.0, 0.0), 0.376) + ensure("sphere just reached", out.hit) + small("sphere just reached fraction", out.fraction - 3.0 / 8.0, 0.00001) + // The interior: the origin at fraction zero; a zero-length ray inside and out. + origin = math.vec3(0.3, 0.0, 0.0) + out = shape.ray_cast_sphere(s, origin, math.vec3(8.0, 0.0, 0.0), 1.0) + ensure("sphere interior hit", out.hit) + ensure("sphere interior fraction", out.fraction == 0.0) + small("sphere interior point", math.distance(out.point, origin), math.EPSILON) + origin = math.vec3(0.5, 0.0, 0.0) + out = shape.ray_cast_sphere(s, origin, math.vec3_zero(), 1.0) + ensure("sphere zero ray inside", out.hit) + small("sphere zero ray point", math.distance(out.point, origin), math.EPSILON) + ensure("sphere zero ray outside", shape.ray_cast_sphere(s, math.vec3(3.0, 0.0, 0.0), math.vec3_zero(), 1.0).hit == false) + // The graze. + ensure("sphere graze hits", shape.ray_cast_sphere(s, math.vec3(0.0 - 4.0, 0.999, 0.0), math.vec3(8.0, 0.0, 0.0), 1.0).hit) + ensure("sphere graze misses", shape.ray_cast_sphere(s, math.vec3(0.0 - 4.0, 1.001, 0.0), math.vec3(8.0, 0.0, 0.0), 1.0).hit == false) + // The hollow sphere hits from inside. + out = shape.ray_cast_hollow_sphere(s, math.vec3(0.3, 0.0, 0.0), math.vec3(8.0, 0.0, 0.0), 1.0) + ensure("hollow sphere inside hit", out.hit) + small("hollow sphere inside point", out.point.x - 1.0, 0.00001) + small("hollow sphere inside normal", out.normal.x - 1.0, 0.00001) + // The hollow cast reports its fraction in length units against a unit ray. + out = shape.ray_cast_hollow_sphere(s, math.vec3(0.0 - 4.0, 0.0, 0.0), math.vec3(1.0, 0.0, 0.0), 8.0) + ensure("hollow sphere outside hit", out.hit) + small("hollow sphere outside point", out.point.x + 1.0, 0.00001) + small("hollow sphere outside fraction", out.fraction - 3.0, 0.00001) +} + +test_ray_cast_capsule() { + c = ray_capsule() + // The side, perpendicular: the surface at 2 over a ray of 6. + origin = math.vec3(0.0, 3.0, 0.0) + translation = math.vec3(0.0, 0.0 - 6.0, 0.0) + check_hit("capsule side y", shape.ray_cast_capsule(c, origin, translation, 1.0), origin, translation, math.vec3(0.0, 1.0, 0.0), math.vec3(0.0, 1.0, 0.0), 1.0 / 3.0, 0.00001) + origin = math.vec3(0.0, 0.0, 3.0) + translation = math.vec3(0.0, 0.0, 0.0 - 6.0) + check_hit("capsule side z", shape.ray_cast_capsule(c, origin, translation, 1.0), origin, translation, math.vec3(0.0, 0.0, 1.0), math.vec3(0.0, 0.0, 1.0), 1.0 / 3.0, 0.00001) + origin = math.vec3(0.0 - 1.0, 3.0, 0.0) + translation = math.vec3(0.0, 0.0 - 6.0, 0.0) + check_hit("capsule side near c1", shape.ray_cast_capsule(c, origin, translation, 1.0), origin, translation, math.vec3(0.0 - 1.0, 1.0, 0.0), math.vec3(0.0, 1.0, 0.0), 1.0 / 3.0, 0.00001) + // Oblique in the z = 0 plane, crossing y = 1 within the cylinder. + origin = math.vec3(0.0 - 3.0, 3.0, 0.0) + translation = math.vec3(4.0, 0.0 - 4.0, 0.0) + check_hit("capsule oblique", shape.ray_cast_capsule(c, origin, translation, 1.0), origin, translation, math.vec3(0.0 - 1.0, 1.0, 0.0), math.vec3(0.0, 1.0, 0.0), 0.5, 0.0001) + // The caps: collinear onto c2, off-axis through each cap's centre. + k = 0.70710678 + origin = math.vec3(5.0, 0.0, 0.0) + translation = math.vec3(0.0 - 8.0, 0.0, 0.0) + check_hit("capsule cap collinear", shape.ray_cast_capsule(c, origin, translation, 1.0), origin, translation, math.vec3(3.0, 0.0, 0.0), math.vec3(1.0, 0.0, 0.0), 0.25, 0.00001) + origin = math.vec3(4.0, 2.0, 0.0) + translation = math.vec3(0.0 - 4.0, 0.0 - 4.0, 0.0) + check_hit("capsule cap c2", shape.ray_cast_capsule(c, origin, translation, 1.0), origin, translation, math.vec3(2.0 + k, k, 0.0), math.vec3(k, k, 0.0), 0.323223, 0.0001) + origin = math.vec3(0.0 - 4.0, 2.0, 0.0) + translation = math.vec3(4.0, 0.0 - 4.0, 0.0) + check_hit("capsule cap c1", shape.ray_cast_capsule(c, origin, translation, 1.0), origin, translation, math.vec3(0.0 - 2.0 - k, k, 0.0), math.vec3(0.0 - k, k, 0.0), 0.323223, 0.0001) + // Misses. + ensure("capsule away", shape.ray_cast_capsule(c, math.vec3(0.0, 3.0, 0.0), math.vec3(0.0, 4.0, 0.0), 1.0).hit == false) + ensure("capsule over", shape.ray_cast_capsule(c, math.vec3(0.0, 4.0, 2.0), math.vec3(0.0, 0.0 - 8.0, 0.0), 1.0).hit == false) + ensure("capsule short", shape.ray_cast_capsule(c, math.vec3(0.0, 5.0, 0.0), math.vec3(0.0, 0.0 - 1.0, 0.0), 1.0).hit == false) + ensure("capsule parallel outside", shape.ray_cast_capsule(c, math.vec3(0.0, 3.0, 0.0), math.vec3(8.0, 0.0, 0.0), 1.0).hit == false) + ensure("capsule past the end", shape.ray_cast_capsule(c, math.vec3(4.0, 3.0, 0.0), math.vec3(0.0, 0.0 - 6.0, 0.0), 1.0).hit == false) + // The interior. + origin = math.vec3_zero() + out = shape.ray_cast_capsule(c, origin, math.vec3(0.0, 0.0 - 5.0, 0.0), 1.0) + ensure("capsule interior hit", out.hit) + ensure("capsule interior fraction", out.fraction == 0.0) + small("capsule interior point", math.distance(out.point, origin), math.EPSILON) + origin = math.vec3(2.5, 0.0, 0.0) + out = shape.ray_cast_capsule(c, origin, math.vec3(0.0, 0.0, 5.0), 1.0) + ensure("capsule cap interior hit", out.hit) + ensure("capsule cap interior fraction", out.fraction == 0.0) + small("capsule cap interior point", math.distance(out.point, origin), math.EPSILON) + out = shape.ray_cast_capsule(c, math.vec3_zero(), math.vec3_zero(), 1.0) + ensure("capsule zero ray inside", out.hit) + small("capsule zero ray point", math.length(out.point), math.EPSILON) + ensure("capsule zero ray outside", shape.ray_cast_capsule(c, math.vec3(0.0, 3.0, 0.0), math.vec3_zero(), 1.0).hit == false) + // Coincident centres are a sphere. + degenerate = manifold.capsule(math.vec3_zero(), math.vec3_zero(), 1.0) + origin = math.vec3(0.0 - 4.0, 0.0, 0.0) + translation = math.vec3(8.0, 0.0, 0.0) + check_hit("capsule degenerate", shape.ray_cast_capsule(degenerate, origin, translation, 1.0), origin, translation, math.vec3(0.0 - 1.0, 0.0, 0.0), math.vec3(0.0 - 1.0, 0.0, 0.0), 3.0 / 8.0, 0.00001) + // The clip straddling the side hit at 1/3. + ensure("capsule clipped", shape.ray_cast_capsule(c, math.vec3(0.0, 3.0, 0.0), math.vec3(0.0, 0.0 - 6.0, 0.0), 0.3).hit == false) + out = shape.ray_cast_capsule(c, math.vec3(0.0, 3.0, 0.0), math.vec3(0.0, 0.0 - 6.0, 0.0), 0.5) + ensure("capsule just reached", out.hit) + small("capsule just reached fraction", out.fraction - 1.0 / 3.0, 0.00001) + + // Near parallel: a long ray drifting in from just outside the cylinder. + axis_y = manifold.capsule(math.vec3_zero(), math.vec3(0.0, 10.0, 0.0), 1.0) + origin = math.vec3(1.0001, 100.0, 0.0) + translation = math.vec3(0.0 - 0.001, 0.0 - 200.0, 0.0) + out = shape.ray_cast_capsule(axis_y, origin, translation, 1.0) + ensure("near parallel y hit", out.hit) + on_segment = math.point_to_segment_distance(axis_y.center1, axis_y.center2, out.point) + small("near parallel y on the surface", math.distance(out.point, on_segment) - axis_y.radius, 0.001) + small("near parallel y on the ray", math.distance(out.point, math.mul_add(origin, out.fraction, translation)), 0.001) + origin = math.vec3(0.0 - 1000.0, 1.0001, 0.0) + translation = math.vec3(2000.0, 0.0 - 0.001, 0.0) + out = shape.ray_cast_capsule(c, origin, translation, 1.0) + ensure("near parallel x hit", out.hit) + on_segment = math.point_to_segment_distance(c.center1, c.center2, out.point) + small("near parallel x on the surface", math.distance(out.point, on_segment) - c.radius, 0.001) + ensure("exactly parallel outside misses", shape.ray_cast_capsule(c, math.vec3(0.0, 3.0, 0.0), math.vec3(8.0, 0.0, 0.0), 1.0).hit == false) +} + +test_overlap_convention() { + ray = math.vec3(8.0, 0.0, 0.0) + zero = math.vec3_zero() + s = unit_sphere() + inside = math.vec3(0.2, 0.0, 0.0) + check_initial_overlap("sphere moving", shape.ray_cast_sphere(s, inside, ray, 1.0), inside) + check_initial_overlap("sphere point", shape.ray_cast_sphere(s, inside, zero, 1.0), inside) + ensure("sphere point outside", shape.ray_cast_sphere(s, math.vec3(3.0, 0.0, 0.0), zero, 1.0).hit == false) + c = ray_capsule() + inside = math.vec3_zero() + check_initial_overlap("capsule moving", shape.ray_cast_capsule(c, inside, ray, 1.0), inside) + check_initial_overlap("capsule point", shape.ray_cast_capsule(c, inside, zero, 1.0), inside) + ensure("capsule point outside", shape.ray_cast_capsule(c, math.vec3(0.0, 3.0, 0.0), zero, 1.0).hit == false) + box = hull.make_box_hull(1.0, 1.0, 1.0) + inside = math.vec3(0.3, 0.2, 0.1) + check_initial_overlap("hull moving", hull.ray_cast_hull(box, inside, ray, 1.0), inside) + check_initial_overlap("hull point", hull.ray_cast_hull(box, inside, zero, 1.0), inside) + ensure("hull point outside", hull.ray_cast_hull(box, math.vec3(3.0, 0.0, 0.0), zero, 1.0).hit == false) + hull.destroy_hull(box) +} + +// The distance from a hit point to the analytic first intersection of +// the same ray with the sphere: what the method loses, since our floats +// are doubles and the reference's were floats. +sphere_hit_error(s: Sphere, origin: Vec3, translation: Vec3, point: Vec3) -> float { + r = s.radius + sv = math.sub(origin, s.center) + d = math.normalize(translation) + b = math.dot(sv, d) + cc = math.dot(sv, sv) - r * r + t = 0.0 - b - sqrt(b * b - cc) + return math.distance(point, math.mul_add(origin, t, d)) +} + +capsule_hit_error(c: Capsule, origin: Vec3, translation: Vec3, point: Vec3) -> float { + a = math.normalize(math.sub(c.center2, c.center1)) + sv = math.sub(origin, c.center1) + d = math.normalize(translation) + sp = math.mul_sub(sv, math.dot(sv, a), a) + dp = math.mul_sub(d, math.dot(d, a), a) + qa = math.dot(dp, dp) + qb = 2.0 * math.dot(sp, dp) + qc = math.dot(sp, sp) - c.radius * c.radius + tau = (0.0 - qb - sqrt(qb * qb - 4.0 * qa * qc)) / (2.0 * qa) + return math.distance(point, math.mul_add(origin, tau, d)) +} + +test_far_origin() { + s = unit_sphere() + c = ray_capsule() + // (0, 0, 1) is on the sphere and on the capsule's side; the ray dives + // in from far away along a fan of directions skewed off the normal. + h = math.vec3(0.0, 0.0, 1.0) + offsets_block = calloc(3, 8) + offsets = offsets_block as float[] + offsets[0] = 0.0 - 0.7 + offsets[1] = 0.0 + offsets[2] = 0.7 + distances_block = calloc(7, 8) + distances = distances_block as float[] + distances[0] = 10.0 + distances[1] = 100.0 + distances[2] = 1000.0 + distances[3] = 10000.0 + distances[4] = 100000.0 + distances[5] = 1000000.0 + distances[6] = 10000000.0 + ray_miss = 1000000000000.0 + println(" worst hit point error over a fan of skew rays, by origin distance (sphere, capsule):") + i = 0 + while i < 7 { + d = distances[i] + max_s = 0.0 + max_c = 0.0 + ia = 0 + while ia < 3 { + ib = 0 + while ib < 3 { + u = math.normalize(math.vec3(offsets[ia], offsets[ib], 1.0)) + origin = math.mul_add(h, d, u) + translation = math.mul_sv(0.0 - 2.0 * d, u) + os = shape.ray_cast_sphere(s, origin, translation, 1.0) + oc = shape.ray_cast_capsule(c, origin, translation, 1.0) + err_s = ray_miss + if os.hit { err_s = sphere_hit_error(s, origin, translation, os.point) } + err_c = ray_miss + if oc.hit { err_c = capsule_hit_error(c, origin, translation, oc.point) } + max_s = math.max_float(max_s, err_s) + max_c = math.max_float(max_c, err_c) + ib = ib + 1 + } + ia = ia + 1 + } + println(" ${d}: ${max_s} ${max_c}") + // The closest-point form keeps the error at the precision floor, + // growing only linearly with the distance: the reference's float + // bound is 16 * distance * FLT_EPSILON + 2e-6; ours in doubles must + // sit far below it, and at ten million units (where a float ray + // loses the hit) still within the same bound. + floor = 16.0 * d * math.EPSILON + 0.000002 + ensure("sphere error at ${d} under the float floor", max_s < floor) + ensure("capsule error at ${d} under the float floor", max_c < floor) + if d < 10000000.0 { + ensure("sphere error at ${d} at double precision", max_s < 0.001 * floor + 0.0000001) + ensure("capsule error at ${d} at double precision", max_c < 0.001 * floor + 0.0000001) + } + i = i + 1 + } + free(distances_block) + free(offsets_block) +} + +test_dispatch() { + origin = math.vec3(0.0 - 4.0, 0.0, 0.0) + translation = math.vec3(8.0, 0.0, 0.0) + // The reference's RayCastShape: the sphere at x 1, the capsule -1..1, the cube. + out = shape.ray_cast_sphere(test_sphere(), origin, translation, 1.0) + ensure("cast sphere hit", out.hit) + small("cast sphere normal x", out.normal.x + 1.0, math.EPSILON) + small("cast sphere normal y", out.normal.y, math.EPSILON) + small("cast sphere normal z", out.normal.z, math.EPSILON) + small("cast sphere fraction", out.fraction - 0.5, math.EPSILON) + out = shape.ray_cast_capsule(test_capsule(), origin, translation, 1.0) + ensure("cast capsule hit", out.hit) + small("cast capsule normal x", out.normal.x + 1.0, math.EPSILON) + small("cast capsule normal y", out.normal.y, math.EPSILON) + small("cast capsule normal z", out.normal.z, math.EPSILON) + small("cast capsule fraction", out.fraction - 0.25, math.EPSILON) + box = hull.make_box_hull(1.0, 1.0, 1.0) + out = hull.ray_cast_hull(box, origin, translation, 1.0) + ensure("cast hull hit", out.hit) + small("cast hull normal x", out.normal.x + 1.0, math.EPSILON) + small("cast hull normal y", out.normal.y, math.EPSILON) + small("cast hull normal z", out.normal.z, math.EPSILON) + small("cast hull fraction", out.fraction - 3.0 / 8.0, math.EPSILON) + + // Every kind under one transform (turned a quarter about z and moved), + // with a world ray along -y at the moved origin: the shape's local x + // now points along world y, so each hits its local +x face from above. + q = math.compute_quat_between_unit_vectors(math.vec3_axis_x(), math.vec3_axis_y()) + t = Transform { p: math.vec3(10.0, 20.0, 30.0), q: q } + world_origin = math.vec3(10.0, 24.0, 30.0) + world_translation = math.vec3(0.0, 0.0 - 8.0, 0.0) + grid = mesh.create_grid_mesh(4, 4, 1.0, 1, false) + field = height_field.create_grid(4, 4, math.vec3(1.0, 1.0, 1.0), false) + shapes_block = calloc(5, sizeof(Shape)) + shapes = shapes_block as Shape[] + shapes[0] = shape.sphere_shape(unit_sphere(), 1.0) + shapes[1] = shape.capsule_shape(ray_capsule(), 1.0) + shapes[2] = shape.hull_shape(box, 1.0) + // The grid mesh lies in the local xz plane from 0 to 4: its normal (local y) faces world -x. + shapes[3] = shape.mesh_shape(mesh.mesh(grid, math.vec3_one()), 1) + shapes[4] = shape.height_field_shape(field, 1) + // The sphere and the cube reach local x = 1 (world y = 21); the capsule x = 3 (world y = 23). + k = 0 + while k < 3 { + sh = shapes[k] + surface = 21.0 + if k == 1 { surface = 23.0 } + out = shape.ray_cast_shape(&sh, t, world_origin, world_translation, 1.0) + ensure("dispatch ray ${k} hit", out.hit) + small("dispatch ray ${k} fraction", out.fraction - (24.0 - surface) / 8.0, 0.00001) + small("dispatch ray ${k} normal", out.normal.y - 1.0, 0.00001) + small("dispatch ray ${k} point", math.distance(out.point, math.vec3(10.0, surface, 30.0)), 0.00001) + k = k + 1 + } + // The mesh (centred, -2..2) and the field (0..3) lie in their local xz + // plane, their +y face toward world -x: a ray along world +x from the + // world point of local (1, 3, 1), which is t.p + q (1, 3, 1) = (7, 21, 31). + k = 3 + while k < 5 { + sh = shapes[k] + out = shape.ray_cast_shape(&sh, t, math.vec3(7.0, 21.0, 31.0), math.vec3(8.0, 0.0, 0.0), 1.0) + ensure("dispatch ray ${k} hit", out.hit) + small("dispatch ray ${k} fraction", out.fraction - 3.0 / 8.0, 0.01) + small("dispatch ray ${k} normal", out.normal.x + 1.0, 0.00001) + k = k + 1 + } + // A miss leaves the output empty. + sh = shapes[0] + out = shape.ray_cast_shape(&sh, t, math.vec3(50.0, 50.0, 50.0), math.vec3(1.0, 0.0, 0.0), 1.0) + ensure("dispatch miss", out.hit == false) + + // The shape cast of a sphere of radius 0.5 down onto each convex kind. + point_block = calloc(1, sizeof(Vec3)) + point = point_block as Vec3[] + point[0] = world_origin + proxy = distance.shape_proxy(point_block, 1, 0.5) + k = 0 + while k < 3 { + sh = shapes[k] + surface = 21.0 + if k == 1 { surface = 23.0 } + point[0] = world_origin + out = shape.shape_cast_shape(&sh, t, proxy, world_translation, 1.0, false) + ensure("dispatch shape cast ${k} hit", out.hit) + small("dispatch shape cast ${k} fraction", out.fraction - (24.0 - surface - 0.5) / 8.0, 0.001) + small("dispatch shape cast ${k} normal", out.normal.y - 1.0, 0.001) + small("dispatch shape cast ${k} point", math.distance(out.point, math.vec3(10.0, surface, 30.0)), 0.001) + k = k + 1 + } + k = 3 + while k < 5 { + sh = shapes[k] + point[0] = math.vec3(7.0, 21.0, 31.0) + out = shape.shape_cast_shape(&sh, t, proxy, math.vec3(8.0, 0.0, 0.0), 1.0, false) + ensure("dispatch shape cast ${k} hit", out.hit) + small("dispatch shape cast ${k} fraction", out.fraction - 2.5 / 8.0, 0.01) + small("dispatch shape cast ${k} normal", out.normal.x + 1.0, 0.001) + k = k + 1 + } + + // The overlap: the same sphere sunk 0.1 into each kind's surface, and 0.1 clear of it. + k = 0 + while k < 3 { + sh = shapes[k] + surface = 21.0 + if k == 1 { surface = 23.0 } + point[0] = math.vec3(10.0, surface + 0.4, 30.0) + ensure("dispatch overlap ${k}", shape.overlap_shape(&sh, t, proxy)) + point[0] = math.vec3(10.0, surface + 0.6, 30.0) + ensure("dispatch clear ${k}", shape.overlap_shape(&sh, t, proxy) == false) + k = k + 1 + } + k = 3 + while k < 5 { + sh = shapes[k] + point[0] = math.vec3(9.6, 21.0, 31.0) + ensure("dispatch overlap ${k}", shape.overlap_shape(&sh, t, proxy)) + point[0] = math.vec3(9.4, 21.0, 31.0) + ensure("dispatch clear ${k}", shape.overlap_shape(&sh, t, proxy) == false) + k = k + 1 + } + + // The mover: a capsule standing on each kind, within its radius, gets + // a plane whose normal points out of the surface in world space. + planes_block = calloc(8, sizeof(PlaneResult)) + planes = planes_block as PlaneResult[] + k = 0 + while k < 3 { + sh = shapes[k] + surface = 21.0 + if k == 1 { surface = 23.0 } + mover = manifold.capsule(math.vec3(10.0, surface + 0.2, 30.0), math.vec3(10.0, surface + 1.2, 30.0), 0.3) + count = shape.collide_mover(planes, 8, &sh, t, mover) + ensure("dispatch mover ${k} plane", count == 1) + if count == 1 { + small("dispatch mover ${k} normal", planes[0].plane.normal.y - 1.0, 0.0001) + small("dispatch mover ${k} offset", planes[0].plane.offset - 0.1, 0.0001) + // The sphere and the capsule report their centre or axis point; the hull the surface point. + if k == 2 { small("dispatch mover ${k} point", planes[0].point.y - 21.0, 0.0001) } + if k == 0 { small("dispatch mover ${k} point", planes[0].point.y - 20.0, 0.0001) } + ensure("dispatch mover ${k} material", planes[0].material_index == 0) + } + k = k + 1 + } + // Standing on the mesh and the field: local (1, 0.2, 1) up to (1, 1.2, 1) is world (9.8, 21, 31) to (8.8, 21, 31). + mover = manifold.capsule(math.vec3(9.8, 21.0, 31.0), math.vec3(8.8, 21.0, 31.0), 0.3) + k = 3 + while k < 5 { + sh = shapes[k] + count = shape.collide_mover(planes, 8, &sh, t, mover) + ensure("dispatch mover ${k} planes (${count})", count >= 1) + if count >= 1 { + small("dispatch mover ${k} normal", planes[0].plane.normal.x + 1.0, 0.0001) + small("dispatch mover ${k} offset", planes[0].plane.offset - 0.1, 0.01) + } + k = k + 1 + } + // Hovering, none; capacity zero, none. + mover = manifold.capsule(math.vec3(10.0, 25.0, 30.0), math.vec3(10.0, 26.0, 30.0), 0.3) + sh = shapes[0] + ensure("mover hovering", shape.collide_mover(planes, 8, &sh, t, mover) == 0) + mover = manifold.capsule(math.vec3(10.0, 21.2, 30.0), math.vec3(10.0, 22.2, 30.0), 0.3) + ensure("mover no capacity", shape.collide_mover(planes, 0, &sh, t, mover) == 0) + // Deep overlap: the mover's axis through the sphere's centre pushes + // perpendicular to the axis; through a hull, nothing. + mover = manifold.capsule(math.vec3(10.0, 19.5, 30.0), math.vec3(10.0, 20.5, 30.0), 0.3) + count = shape.collide_mover(planes, 8, &sh, t, mover) + ensure("mover through the sphere", count == 1) + if count == 1 { + small("mover through the sphere: perpendicular", planes[0].plane.normal.y, 0.0001) + small("mover through the sphere: full push", planes[0].plane.offset - 1.3, 0.0001) + } + sh = shapes[2] + ensure("mover through the hull", shape.collide_mover(planes, 8, &sh, t, mover) == 0) + sh = shapes[1] + mover = manifold.capsule(math.vec3(10.0, 19.5, 30.0), math.vec3(10.0, 20.5, 30.0), 0.3) + count = shape.collide_mover(planes, 8, &sh, t, mover) + ensure("mover through the capsule", count == 1) + if count == 1 { small("mover through the capsule: full push", planes[0].plane.offset - 1.3, 0.0001) } + + // The centroid, areas, extent and proxy. + sh = shapes[1] + small("capsule centroid", math.length(shape.get_shape_centroid(&sh)), 0.000001) + small("capsule area", shape.get_shape_area(&sh) - 8.0 - 2.0 * math.PI, 0.000001) + small("capsule projected area on y", shape.get_shape_projected_area(&sh, math.vec3_axis_y()) - math.PI - 8.0, 0.000001) + small("capsule projected area on x", shape.get_shape_projected_area(&sh, math.vec3_axis_x()) - math.PI, 0.000001) + extent = shape.compute_shape_extent(&sh, math.vec3_zero()) + small("capsule min extent", extent.min_extent - 1.0, 0.000001) + small("capsule max extent x", extent.max_extent.x - 3.0, 0.000001) + small("capsule max extent y", extent.max_extent.y - 1.0, 0.000001) + p = shape.make_shape_proxy(&sh) + ensure("capsule proxy", p.count == 2 && p.radius == 1.0) + sh = shapes[0] + small("sphere area", shape.get_shape_area(&sh) - 2.0 * math.PI, 0.000001) + small("sphere projected area", shape.get_shape_projected_area(&sh, math.vec3_axis_y()) - math.PI, 0.000001) + extent = shape.compute_shape_extent(&sh, math.vec3(1.0, 0.0, 0.0)) + small("sphere max extent x", extent.max_extent.x - 2.0, 0.000001) + p = shape.make_shape_proxy(&sh) + ensure("sphere proxy", p.count == 1 && p.radius == 1.0) + sh = shapes[2] + small("hull area", shape.get_shape_area(&sh) - 24.0, 0.000001) + small("hull projected area", shape.get_shape_projected_area(&sh, math.vec3_axis_y()) - 4.0, 0.000001) + extent = shape.compute_shape_extent(&sh, math.vec3_zero()) + small("hull min extent", extent.min_extent - 1.0, 0.000001) + small("hull max extent", extent.max_extent.z - 1.0, 0.000001) + p = shape.make_shape_proxy(&sh) + ensure("hull proxy", p.count == 8 && p.radius == 0.0) + sh = shapes[3] + small("mesh centroid", math.length(shape.get_shape_centroid(&sh)), 0.000001) + extent = shape.compute_shape_extent(&sh, math.vec3_zero()) + small("mesh max extent", extent.max_extent.x - 2.0, 0.000001) + small("mesh min extent", extent.min_extent - sqrt(8.0), 0.000001) + extent = shape.compute_shape_extent(&sh, math.vec3(1.0, 0.0, 0.0)) + small("mesh max extent off centre", extent.max_extent.x - 3.0, 0.000001) + ensure("mesh has no mass", shape.compute_shape_mass(&sh).mass == 0.0) + ensure("mesh has no area", shape.get_shape_area(&sh) == 0.0) + sh = shapes[4] + small("field centroid", math.distance(shape.get_shape_centroid(&sh), math.vec3(1.5, 0.0, 1.5)), 0.01) + ensure("field proxy is empty", shape.make_shape_proxy(&sh).count == 0) + + free(planes_block) + free(point_block) + free(shapes_block) + mesh.destroy_mesh(grid) + height_field.destroy_height_field(field) + hull.destroy_hull(box) +} + +test_filters() { + a = shape.default_filter() + b = shape.default_filter() + ensure("defaults collide", shape.should_shapes_collide(a, b)) + b.category_bits = 2 as long + a.mask_bits = 1 as long + ensure("mask excludes", shape.should_shapes_collide(a, b) == false) + a.mask_bits = 3 as long + ensure("mask includes", shape.should_shapes_collide(a, b)) + a.group_index = 3 + b.group_index = 3 + a.mask_bits = 0 as long + ensure("a shared positive group always collides", shape.should_shapes_collide(a, b)) + a.group_index = 0 - 3 + b.group_index = 0 - 3 + a.mask_bits = 3 as long + ensure("a shared negative group never collides", shape.should_shapes_collide(a, b) == false) + a.group_index = 0 - 3 + b.group_index = 0 - 4 + ensure("different groups fall back to the masks", shape.should_shapes_collide(a, b)) + q = shape.default_query_filter() + ensure("default query collides", shape.should_query_collide(shape.default_filter(), q)) + q.mask_bits = 2 as long + ensure("query mask excludes", shape.should_query_collide(shape.default_filter(), q) == false) + ensure("convex kinds", shape.is_convex(shape.SHAPE_SPHERE) && shape.is_convex(shape.SHAPE_CAPSULE) && shape.is_convex(shape.SHAPE_HULL)) + ensure("concave kinds", shape.is_convex(shape.SHAPE_MESH) == false && shape.is_convex(shape.SHAPE_HEIGHT_FIELD) == false && shape.is_convex(shape.SHAPE_COMPOUND) == false) +} + +main() { + before = core.alloc_count() + test_mass() + test_aabb() + test_ray_cast_sphere() + test_ray_cast_capsule() + test_overlap_convention() + test_far_origin() + test_dispatch() + test_filters() + // The scratch stays allocated: three blocks here, three in mesh, three in the height field. + ensure("every other counted allocation was freed (${core.alloc_count() - before})", core.alloc_count() == before + 9) + + println("shape: ${checks} checks") + if failures == 0 { + println("shape: all checks passed") + } else { + println("shape: ${failures} failure(s)") + exit(1) + } +} diff --git a/bench/RESULTS.md b/bench/RESULTS.md index 11d213c..e7dc9fe 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -245,3 +245,32 @@ straddled cell on top of distance's 1.3-1.5x; the overlap 1.85x. The field is 4.2 MB here against 1.3 MB there: the reference packs 16-bit heights and 8-bit materials and flags, which Aether cannot yet address, so they are ints. + +## shape + +`bench/shape.ae` and `bench/shape_box3d.c`: 1,000,000 rays at a unit +sphere and 1,000,000 at a capsule from a fan of origins, a third of them +missing; 100,000 casts of a sphere at a capsule under a transform; +100,000 overlaps of a sphere with it; 100,000 mover planes against it; +100,000 capsule masses. + +| phase | aephysics | Box3D | +|---|---|---| +| 1,000,000 sphere ray casts | 33.2 ms | **31.5** | +| 1,000,000 capsule ray casts | **41.0** | 49.7 | +| 100,000 shape casts | 44.6 | **18.0** | +| 100,000 overlaps | 12.2 | **5.9** | +| 100,000 mover planes | 6.9 | **4.8** | +| 100,000 capsule masses | 4.5 | **4.3** | + +The same answers: the ray hits agree to one boundary case in a million +(349,452 sphere hits here against 349,451, the same 595,654 on the +capsule) with equal fraction sums, the casts hit the same 89,582 times +with equal sums, the overlaps (51,518), the mover's planes (45,071) and +the masses agree. The sphere ray and the masses are at parity, the +capsule ray is 0.8x (the reference's float division by the axis length +against our double), the mover 1.45x. The sphere-against-capsule cast +is 2.5x and the overlap 2x: both are one GJK call on proxies with +radii, where the box-against-box work of the distance layer sat at +1.3-1.5x; the radius handling in `distance.shape_cast` and +`shape_distance` is the place to profile (aephysics#8). diff --git a/bench/shape.ae b/bench/shape.ae new file mode 100644 index 0000000..4b1f9a5 --- /dev/null +++ b/bench/shape.ae @@ -0,0 +1,134 @@ +// The shapes on the same scenes as bench/shape_box3d.c: 1,000,000 rays +// at a unit sphere and 1,000,000 at a capsule from a fan of origins (a +// third missing), 100,000 sphere shape casts at a capsule under a +// transform through the dispatch, 100,000 overlaps, 100,000 mover planes +// against a capsule, and 100,000 capsule masses. Single thread, wall +// time per phase, with the hit counts and sums as the checksum. +import std.string +import std.os +import aephysics.math +import aephysics.distance +import aephysics.manifold +import aephysics.mesh +import aephysics.shape + +extern calloc(count: int, size: int) -> ptr +extern free(p: ptr) +extern sin(x: float) -> float +extern cos(x: float) -> float + +clock() -> long { return os.now_monotonic_ns() } +ms(ns: long) -> float { return (ns as float) / 1000000.0 } + +const RAYS = 1000000 +const CASTS = 100000 +const OVERLAPS = 100000 +const MOVERS = 100000 +const MASSES = 100000 + +main() { + sphere = manifold.sphere(math.vec3_zero(), 1.0) + capsule = manifold.capsule(math.vec3(0.0 - 2.0, 0.0, 0.0), math.vec3(2.0, 0.0, 0.0), 1.0) + + t0 = clock() + sphere_hits = 0 + sphere_sum = 0.0 + i = 0 + while i < RAYS { + t = (i as float) / (RAYS as float) + a = 6.2831853 * t + origin = math.vec3(5.0 * cos(7.0 * a), 5.0 * sin(7.0 * a), 5.0 * sin(3.0 * a)) + target = math.vec3(1.5 * sin(11.0 * a), 1.5 * cos(13.0 * a), 0.0) + out = shape.ray_cast_sphere(sphere, origin, math.sub(target, origin), 1.0) + if out.hit { + sphere_hits = sphere_hits + 1 + sphere_sum = sphere_sum + out.fraction + } + i = i + 1 + } + t1 = clock() + + capsule_hits = 0 + capsule_sum = 0.0 + i = 0 + while i < RAYS { + t = (i as float) / (RAYS as float) + a = 6.2831853 * t + origin = math.vec3(6.0 * cos(7.0 * a), 5.0 * sin(7.0 * a), 5.0 * sin(3.0 * a)) + target = math.vec3(3.0 * sin(11.0 * a), 1.5 * cos(13.0 * a), 0.0) + out = shape.ray_cast_capsule(capsule, origin, math.sub(target, origin), 1.0) + if out.hit { + capsule_hits = capsule_hits + 1 + capsule_sum = capsule_sum + out.fraction + } + i = i + 1 + } + t2 = clock() + + sh = shape.capsule_shape(capsule, 1.0) + q = math.make_quat_from_axis_angle(math.normalize(math.vec3(1.0, 2.0, 3.0)), 0.7) + transform = Transform { p: math.vec3(10.0, 20.0, 30.0), q: q } + start_block = calloc(1, sizeof(Vec3)) + start = start_block as Vec3[] + cast_hits = 0 + cast_sum = 0.0 + i = 0 + while i < CASTS { + t = (i as float) / (CASTS as float) + a = 6.2831853 * t + local = math.vec3(2.5 * cos(5.0 * a), 4.0, 1.2 * sin(5.0 * a)) + start[0] = math.transform_point(transform, local) + translation = math.rotate_vector(q, math.vec3(0.0, 0.0 - 8.0, 0.5 * sin(9.0 * a))) + out = shape.shape_cast_shape(&sh, transform, distance.shape_proxy(start_block, 1, 0.3), translation, 1.0, false) + if out.hit { + cast_hits = cast_hits + 1 + cast_sum = cast_sum + out.fraction + } + i = i + 1 + } + t3 = clock() + + overlap_hits = 0 + i = 0 + while i < OVERLAPS { + t = (i as float) / (OVERLAPS as float) + a = 6.2831853 * t + local = math.vec3(3.0 * cos(5.0 * a), 1.6 * sin(3.0 * a), 1.2 * sin(5.0 * a)) + start[0] = math.transform_point(transform, local) + if shape.overlap_shape(&sh, transform, distance.shape_proxy(start_block, 1, 0.5)) { overlap_hits = overlap_hits + 1 } + i = i + 1 + } + t4 = clock() + + planes_block = calloc(4, sizeof(PlaneResult)) + planes = planes_block as PlaneResult[] + mover_planes = 0 + mover_sum = 0.0 + i = 0 + while i < MOVERS { + t = (i as float) / (MOVERS as float) + a = 6.2831853 * t + local = math.vec3(3.0 * cos(5.0 * a), 1.0 + 0.6 * sin(3.0 * a), 1.0 * sin(5.0 * a)) + mover = manifold.capsule(math.transform_point(transform, local), math.transform_point(transform, math.add(local, math.vec3(0.0, 1.0, 0.0))), 0.3) + count = shape.collide_mover(planes, 4, &sh, transform, mover) + mover_planes = mover_planes + count + if count > 0 { mover_sum = mover_sum + planes[0].plane.offset } + i = i + 1 + } + t5 = clock() + + mass_sum = 0.0 + i = 0 + while i < MASSES { + t = (i as float) / (MASSES as float) + c = manifold.capsule(math.vec3(0.0 - 1.0 - t, 0.0, 0.0), math.vec3(1.0, t, 0.0), 0.5 + 0.5 * t) + md = shape.compute_capsule_mass(c, 1.0) + mass_sum = mass_sum + md.mass + md.inertia.cx.x + md.inertia.cy.y + md.inertia.cz.z + i = i + 1 + } + t6 = clock() + + println("aephysics shape: ${RAYS} sphere rays ${ms(t1 - t0)} ms (${sphere_hits} hits, sum ${sphere_sum}), ${RAYS} capsule rays ${ms(t2 - t1)} ms (${capsule_hits} hits, sum ${capsule_sum}), ${CASTS} shape casts ${ms(t3 - t2)} ms (${cast_hits} hits, sum ${cast_sum}), ${OVERLAPS} overlaps ${ms(t4 - t3)} ms (${overlap_hits} hits), ${MOVERS} movers ${ms(t5 - t4)} ms (${mover_planes} planes, sum ${mover_sum}), ${MASSES} masses ${ms(t6 - t5)} ms (sum ${mass_sum})") + free(planes_block) + free(start_block) +} diff --git a/bench/shape_box3d.c b/bench/shape_box3d.c new file mode 100644 index 0000000..8caa48d --- /dev/null +++ b/bench/shape_box3d.c @@ -0,0 +1,151 @@ +// The shapes of the reference on the same scenes as bench/shape.ae: +// 1,000,000 rays at a unit sphere and 1,000,000 at a capsule from a fan +// of origins (a third missing), 100,000 sphere shape casts at a capsule +// under a transform through the dispatch, 100,000 overlaps, 100,000 +// mover planes against a capsule, and 100,000 capsule masses. Single +// thread, wall time per phase, with the hit counts and sums as the +// checksum. +#include "box3d/collision.h" +#include "box3d/math_functions.h" +#include "box3d/types.h" + +// Internal to the reference (shape.h in src) but linked from its library. +int b3CollideMoverAndCapsule( b3PlaneResult* result, const b3Capsule* shape, const b3Capsule* mover ); + +#include +#include +#include + +static double now_ms( void ) +{ + struct timespec ts; + timespec_get( &ts, TIME_UTC ); + return ts.tv_sec * 1000.0 + ts.tv_nsec / 1.0e6; +} + +#define RAYS 1000000 +#define CASTS 100000 +#define OVERLAPS 100000 +#define MOVERS 100000 +#define MASSES 100000 + +int main( void ) +{ + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 1.0f }; + b3Capsule capsule = { { -2.0f, 0.0f, 0.0f }, { 2.0f, 0.0f, 0.0f }, 1.0f }; + + double t0 = now_ms(); + int sphereHits = 0; + double sphereSum = 0.0; + for ( int i = 0; i < RAYS; ++i ) + { + float t = (float)i / (float)RAYS; + float a = 6.2831853f * t; + b3Vec3 origin = { 5.0f * cosf( 7.0f * a ), 5.0f * sinf( 7.0f * a ), 5.0f * sinf( 3.0f * a ) }; + b3Vec3 target = { 1.5f * sinf( 11.0f * a ), 1.5f * cosf( 13.0f * a ), 0.0f }; + b3RayCastInput input = { origin, b3Sub( target, origin ), 1.0f }; + b3CastOutput out = b3RayCastSphere( &sphere, &input ); + if ( out.hit ) + { + sphereHits += 1; + sphereSum += out.fraction; + } + } + double t1 = now_ms(); + + int capsuleHits = 0; + double capsuleSum = 0.0; + for ( int i = 0; i < RAYS; ++i ) + { + float t = (float)i / (float)RAYS; + float a = 6.2831853f * t; + b3Vec3 origin = { 6.0f * cosf( 7.0f * a ), 5.0f * sinf( 7.0f * a ), 5.0f * sinf( 3.0f * a ) }; + b3Vec3 target = { 3.0f * sinf( 11.0f * a ), 1.5f * cosf( 13.0f * a ), 0.0f }; + b3RayCastInput input = { origin, b3Sub( target, origin ), 1.0f }; + b3CastOutput out = b3RayCastCapsule( &capsule, &input ); + if ( out.hit ) + { + capsuleHits += 1; + capsuleSum += out.fraction; + } + } + double t2 = now_ms(); + + // b3ShapeCastShape, b3OverlapShape and b3CollideMover are internal to the + // reference (shape.h in src); their dispatch is done here as they do it: + // the input into the shape's frame, the output back out. + b3Quat q = b3MakeQuatFromAxisAngle( b3Normalize( (b3Vec3){ 1.0f, 2.0f, 3.0f } ), 0.7f ); + b3Transform transform = { { 10.0f, 20.0f, 30.0f }, q }; + + int castHits = 0; + double castSum = 0.0; + for ( int i = 0; i < CASTS; ++i ) + { + float t = (float)i / (float)CASTS; + float a = 6.2831853f * t; + b3Vec3 local = { 2.5f * cosf( 5.0f * a ), 4.0f, 1.2f * sinf( 5.0f * a ) }; + b3Vec3 start = b3TransformPoint( transform, local ); + b3Vec3 translation = b3RotateVector( q, (b3Vec3){ 0.0f, -8.0f, 0.5f * sinf( 9.0f * a ) } ); + b3Vec3 localStart = b3InvTransformPoint( transform, start ); + b3ShapeCastInput input = { { &localStart, 1, 0.3f }, b3InvRotateVector( q, translation ), 1.0f, false }; + b3CastOutput out = b3ShapeCastCapsule( &capsule, &input ); + if ( out.hit ) + { + out.point = b3TransformPoint( transform, out.point ); + out.normal = b3RotateVector( q, out.normal ); + castHits += 1; + castSum += out.fraction; + } + } + double t3 = now_ms(); + + int overlapHits = 0; + for ( int i = 0; i < OVERLAPS; ++i ) + { + float t = (float)i / (float)OVERLAPS; + float a = 6.2831853f * t; + b3Vec3 local = { 3.0f * cosf( 5.0f * a ), 1.6f * sinf( 3.0f * a ), 1.2f * sinf( 5.0f * a ) }; + b3Vec3 center = b3TransformPoint( transform, local ); + b3ShapeProxy proxy = { ¢er, 1, 0.5f }; + if ( b3OverlapCapsule( &capsule, transform, &proxy ) ) overlapHits += 1; + } + double t4 = now_ms(); + + int moverPlanes = 0; + double moverSum = 0.0; + for ( int i = 0; i < MOVERS; ++i ) + { + float t = (float)i / (float)MOVERS; + float a = 6.2831853f * t; + b3Vec3 local = { 3.0f * cosf( 5.0f * a ), 1.0f + 0.6f * sinf( 3.0f * a ), 1.0f * sinf( 5.0f * a ) }; + b3Capsule mover = { b3TransformPoint( transform, local ), b3TransformPoint( transform, b3Add( local, (b3Vec3){ 0.0f, 1.0f, 0.0f } ) ), 0.3f }; + b3Capsule localMover = { b3InvTransformPoint( transform, mover.center1 ), b3InvTransformPoint( transform, mover.center2 ), mover.radius }; + b3PlaneResult planes[4]; + int count = b3CollideMoverAndCapsule( planes, &capsule, &localMover ); + for ( int k = 0; k < count; ++k ) + { + planes[k].plane.normal = b3RotateVector( q, planes[k].plane.normal ); + planes[k].point = b3TransformPoint( transform, planes[k].point ); + } + moverPlanes += count; + if ( count > 0 ) moverSum += planes[0].plane.offset; + } + double t5 = now_ms(); + + double massSum = 0.0; + for ( int i = 0; i < MASSES; ++i ) + { + float t = (float)i / (float)MASSES; + b3Capsule c = { { -1.0f - t, 0.0f, 0.0f }, { 1.0f, t, 0.0f }, 0.5f + 0.5f * t }; + b3MassData md = b3ComputeCapsuleMass( &c, 1.0f ); + massSum += md.mass + md.inertia.cx.x + md.inertia.cy.y + md.inertia.cz.z; + } + double t6 = now_ms(); + + printf( "box3d shape: %d sphere rays %.2f ms (%d hits, sum %.3f), %d capsule rays %.2f ms (%d hits, sum %.3f), " + "%d shape casts %.2f ms (%d hits, sum %.3f), %d overlaps %.2f ms (%d hits), %d movers %.2f ms (%d planes, sum %.3f), " + "%d masses %.2f ms (sum %.3f)\n", + RAYS, t1 - t0, sphereHits, sphereSum, RAYS, t2 - t1, capsuleHits, capsuleSum, CASTS, t3 - t2, castHits, castSum, + OVERLAPS, t4 - t3, overlapHits, MOVERS, t5 - t4, moverPlanes, moverSum, MASSES, t6 - t5, massSum ); + return 0; +} diff --git a/design.md b/design.md index 59d0c28..8eeda4e 100644 --- a/design.md +++ b/design.md @@ -88,20 +88,38 @@ started until its tests pass. query and the mover. The same results as the reference; the query at parity, the casts 1.4-2x. The heights, materials and flags are ints for want of 16- and 8-bit arrays, 3x the reference's bytes. -10. **shape**: shape.c (mass properties, ray and shape casts per shape, - compounds). Test: `test_shape`. -11. **dynamics**: `body`, `contact`, `constraint_graph` (graph colouring), +10. **shape** (done): sphere.c, capsule.c and the geometric half of + shape.c as `aephysics.shape`: the sphere's and capsule's mass, bounds, + ray casts (the closest-point forms that hold their precision far from + the origin), casts, overlap and mover planes, the hull's mover, and + the Shape of any kind with its dispatch under a transform (bounds, + swept and fat bounds, centroid, areas, mass, extent, ray and shape + casts, overlap, mover, proxy) and the collision filters. 440 checks: + test_shape.c's masses (sphere, analytic and transformed boxes, the + capsule bracketed by hulls), bounds, the sphere and capsule ray cases, + the overlap convention, the far-origin precision (our doubles sit + three orders under the reference's float floor and still hit at ten + million units), the cast through the dispatch; plus every kind under + one transform through every query, and the filters. Rays and masses + at parity; the GJK cast and overlap on proxies with radii 2-2.5x, + to profile (aephysics#8). The world-bound half of shape.c (creation + on a body, the broad-phase proxy, materials, events) comes with the + dynamics. +11. **compound**: compound.c (the baked compound: children under a + static tree, materials and hulls shared by content) with + test_compound.c, and the compound branch of the shape dispatch. +12. **dynamics**: `body`, `contact`, `constraint_graph` (graph colouring), `solver_set`, `island`, `solver` (the Soft Step: sub-stepping, relax iterations, restitution), `contact_solver` (scalar first; the wide SIMD path second, measured), the joints (revolute, prismatic, distance, motor, weld, wheel, spherical), `sensor`, `mover` (the character mover), `physics_world`. Tests: `test_body`, `test_joint`, `test_world`, `test_mover`, `test_determinism`, `test_large_world`. -12. **parallel**: `parallel_for` and the scheduler over Aether's actors; +13. **parallel**: `parallel_for` and the scheduler over Aether's actors; the benchmarks by thread count as the original records them. -13. **recording and replay**, `world_snapshot`: last, since they are the +14. **recording and replay**, `world_snapshot`: last, since they are the tooling and not the engine. -14. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run +15. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run against the C build on the same machine, recorded under `benchmark/`. ## Measures From cc7f1f4310088cf45a058196836a5d07f462cd5d Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Sat, 19 Sep 2026 22:00:37 -0300 Subject: [PATCH 2/2] shape: point the radius-cast note at issue #9 --- bench/RESULTS.md | 2 +- design.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/bench/RESULTS.md b/bench/RESULTS.md index e7dc9fe..5c6ccdd 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -273,4 +273,4 @@ against our double), the mover 1.45x. The sphere-against-capsule cast is 2.5x and the overlap 2x: both are one GJK call on proxies with radii, where the box-against-box work of the distance layer sat at 1.3-1.5x; the radius handling in `distance.shape_cast` and -`shape_distance` is the place to profile (aephysics#8). +`shape_distance` is the place to profile (aephysics#9). diff --git a/design.md b/design.md index 8eeda4e..1a14112 100644 --- a/design.md +++ b/design.md @@ -102,7 +102,7 @@ started until its tests pass. million units), the cast through the dispatch; plus every kind under one transform through every query, and the filters. Rays and masses at parity; the GJK cast and overlap on proxies with radii 2-2.5x, - to profile (aephysics#8). The world-bound half of shape.c (creation + to profile (aephysics#9). The world-bound half of shape.c (creation on a body, the broad-phase proxy, materials, events) comes with the dynamics. 11. **compound**: compound.c (the baked compound: children under a