From 36dc6d2999515d0b85e54a98823a9ada108f48bd Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Sat, 19 Sep 2026 22:41:07 -0300 Subject: [PATCH 1/2] compound: the baked compound, with the sphere, capsule and material split out aephysics.compound, Box3D's compound.c in Aether: capsules, hulls, meshes and spheres packed into one block under a tree rebuilt in full and carried in the block with its traversal stack; materials shared by value (deduplicated field by field through core's LongMap), hulls and meshes shared by content (their own hash, then their bytes); a mesh child's four material slots remapped into the compound's table; the bounds, overlap, ray and shape casts, box query and the mover's planes through the tree, each child in its own frame and its result carried back with the child and the material. So the shape dispatch can reach the compound without a cycle, sphere.c and capsule.c become aephysics.sphere and aephysics.capsule, the surface material aephysics.material, and the hull's mover moves beside the mesh's; aephysics.shape imports them all and gains the compound kind in every query. tree_validate accepts a baked tree. test_compound.ae: the reference's creation, material, sharing, child order, bounds, cast, remap, overlap, query and mover subtests (the byte roundtrip is not ported), plus the compound through the shape dispatch and a field of a thousand spheres. 159 checks. bench/compound.ae against bench/compound_box3d.c on 2,000 children: the same hits and sums on every phase; the build 0.7x, queries, overlaps and movers 1.3-1.6x, the shape cast 2.1x, the ray cast 2.9x with the time in the tree traversal itself, recorded to profile. --- README.md | 7 +- aephysics/capsule/module.ae | 197 ++++++++ aephysics/compound/module.ae | 780 +++++++++++++++++++++++++++++ aephysics/dynamic_tree/module.ae | 6 +- aephysics/material/module.ae | 30 ++ aephysics/mesh/module.ae | 22 +- aephysics/shape/module.ae | 443 ++--------------- aephysics/sphere/module.ae | 195 ++++++++ aephysics/test_compound.ae | 812 +++++++++++++++++++++++++++++++ aephysics/test_shape.ae | 114 ++--- bench/RESULTS.md | 32 ++ bench/compound.ae | 166 +++++++ bench/compound_box3d.c | 159 ++++++ design.md | 20 +- 14 files changed, 2525 insertions(+), 458 deletions(-) create mode 100644 aephysics/capsule/module.ae create mode 100644 aephysics/compound/module.ae create mode 100644 aephysics/material/module.ae create mode 100644 aephysics/sphere/module.ae create mode 100644 aephysics/test_compound.ae create mode 100644 bench/compound.ae create mode 100644 bench/compound_box3d.c diff --git a/README.md b/README.md index 9b297f5..d070cf2 100644 --- a/README.md +++ b/README.md @@ -34,8 +34,11 @@ 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` | 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.material` | a surface's material (friction, restitution, rolling resistance, tangent velocity, user id) and its default | done | +| `aephysics.sphere` | the sphere: mass, bounds, the ray cast in the closest-point form that keeps its precision far from the origin, the shape cast, overlap, the mover's plane | done, in `test_shape.ae` | +| `aephysics.capsule` | the capsule: mass by cylinder and caps, bounds, the ray cast by the closest points of the lines with the near-parallel fallback, the shape cast, overlap, the mover's plane | done, in `test_shape.ae` | +| `aephysics.compound` | the baked compound: capsules, hulls, meshes and spheres in one block under a static tree, hulls and meshes shared by content, materials by value; bounds, overlap, ray and shape casts, the box query, the mover's planes | done, `test_compound.ae` (159 checks); [same results as the reference, build 0.7x, queries 1.3-1.6x](bench/RESULTS.md#compound) | +| `aephysics.shape` | the shape of any kind (sphere, capsule, hull, mesh, height field, compound) with 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.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/capsule/module.ae b/aephysics/capsule/module.ae new file mode 100644 index 0000000..6a5a62e --- /dev/null +++ b/aephysics/capsule/module.ae @@ -0,0 +1,197 @@ +// aephysics.capsule -- the capsule: its mass (a cylinder and its two +// caps, the caps carried out by Steiner, turned onto the axis), bounds, +// the ray against it by the closest points of the ray's line and the +// axis with the near-parallel fallback through the axial circle (Real- +// Time Collision Detection 5.1.9), the shape cast, the overlap, and the +// character mover's plane against it. +// +// Box3D's capsule.c (Erin Catto, MIT), the reference this engine is +// measured against. Names are the reference's without its prefix, in +// snake case: b3RayCastCapsule is ray_cast_capsule. +import std.string +import aephysics.math +import aephysics.core +import aephysics.distance +import aephysics.manifold +import aephysics.mesh +import aephysics.sphere + +exports ( + compute_capsule_mass, compute_capsule_aabb, compute_swept_capsule_aabb, overlap_capsule, + ray_cast_capsule, shape_cast_capsule, collide_mover_and_capsule +) + +extern sqrt(x: float) -> float + +// 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: sphere.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 < sphere.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 sphere.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 sphere.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 = sphere.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 sphere.ray_cast_sphere(Sphere { center: c1, radius: r }, origin, translation, max_fraction) } + if length < tc { return sphere.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: sphere.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 = sphere.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 +} + diff --git a/aephysics/compound/module.ae b/aephysics/compound/module.ae new file mode 100644 index 0000000..34e9c2b --- /dev/null +++ b/aephysics/compound/module.ae @@ -0,0 +1,780 @@ +// aephysics.compound -- the baked compound: capsules, hulls, meshes and +// spheres packed into one block under a static bounding volume tree, +// the hulls and meshes shared by content, the materials shared by +// value; and the queries over it: bounds, overlap, ray cast, shape +// cast, the box query, the character mover's planes. +// +// The compound is Box3D's compound.c (Erin Catto, MIT), the reference +// this engine is measured against: the child order (capsules, hulls, +// meshes, spheres), the tree built once with a full rebuild and carried +// in the block, the one level of indirection from a hull or mesh +// instance to its shared data, a mesh child's four material slots +// remapped into the compound's table, and every query brought into the +// child's frame and its result carried back. Names are the reference's +// without its prefix, in snake case: b3CreateCompound is +// create_compound. +// +// Differences: the byte roundtrip (b3ConvertCompoundToBytes) is not +// ported, as with the tree's, the mesh's and the height field's; a +// child's material indices are four named fields; the shared-content +// maps are core's LongMap on the hull's and the mesh's own hash with a +// byte comparison behind it. +import std.string +import aephysics.math +import aephysics.core +import aephysics.dynamic_tree +import aephysics.hull +import aephysics.distance +import aephysics.manifold +import aephysics.mesh +import aephysics.material +import aephysics.sphere +import aephysics.capsule + +exports ( + CompoundCapsuleDef, CompoundHullDef, CompoundMeshDef, CompoundSphereDef, CompoundDef, + CompoundData, CompoundCapsule, CompoundHull, CompoundMesh, CompoundSphere, ChildShape, + COMPOUND_VERSION, MAX_COMPOUND_MESH_MATERIALS, MAX_CHILD_SHAPES, NULL_INDEX, + compound_def, compound_capsule_def, compound_hull_def, compound_mesh_def, compound_sphere_def, + create_compound, destroy_compound, + get_compound_materials, get_compound_material, get_compound_capsule, get_compound_hull, + get_compound_mesh, get_compound_sphere, get_compound_child, compound_child_count, + compute_compound_aabb, overlap_compound, ray_cast_compound, shape_cast_compound, + query_compound, collide_mover_and_compound, make_compound_child_sweep +) + +extern memcpy(dst: ptr, src: ptr, size: int) -> ptr +extern memset(block: ptr, value: int, size: int) -> ptr +extern memcmp(a: ptr, b: ptr, size: int) -> int + +const NULL_INDEX = 0 - 1 +const COMPOUND_VERSION = 0x5C0E7B9A +const MAX_COMPOUND_MESH_MATERIALS = 4 +// The child index shares a 64-bit key with two shape ids in the reference's tables. +const MAX_CHILD_SHAPES = 1 << 20 +// The shape module's kinds (it is above this one): sphere 0, capsule 1, hull 2, mesh 3. +const KIND_SPHERE = 0 +const KIND_CAPSULE = 1 +const KIND_HULL = 2 +const KIND_MESH = 3 + +// --- definitions ----------------------------------------------------------------------------- + +struct CompoundCapsuleDef { + capsule: Capsule + material: SurfaceMaterial +} + +struct CompoundHullDef { + hull: *HullData // shared; copied into the compound + transform: Transform // the hull into the compound's frame + material: SurfaceMaterial +} + +struct CompoundMeshDef { + mesh_data: *MeshData // shared; copied into the compound + transform: Transform + scale: Vec3 // may be negative on any axis + materials: ptr // SurfaceMaterial[], lined up with the triangles' material indices + material_count: int +} + +struct CompoundSphereDef { + sphere: Sphere + material: SurfaceMaterial +} + +// Everything here is copied into the compound; nothing is kept. +struct CompoundDef { + capsules: ptr // CompoundCapsuleDef[] + capsule_count: int + hulls: ptr // CompoundHullDef[] + hull_count: int + meshes: ptr // CompoundMeshDef[] + mesh_count: int + spheres: ptr // CompoundSphereDef[] + sphere_count: int +} + +compound_def() -> CompoundDef { + return CompoundDef { capsules: null, capsule_count: 0, hulls: null, hull_count: 0, meshes: null, mesh_count: 0, + spheres: null, sphere_count: 0 } +} + +compound_capsule_def(c: Capsule, material: SurfaceMaterial) -> CompoundCapsuleDef { + return CompoundCapsuleDef { capsule: c, material: material } +} + +compound_hull_def(h: *HullData, t: Transform, material: SurfaceMaterial) -> CompoundHullDef { + return CompoundHullDef { hull: h, transform: t, material: material } +} + +compound_mesh_def(m: *MeshData, t: Transform, scale: Vec3, materials: ptr, material_count: int) -> CompoundMeshDef { + return CompoundMeshDef { mesh_data: m, transform: t, scale: scale, materials: materials, material_count: material_count } +} + +compound_sphere_def(s: Sphere, material: SurfaceMaterial) -> CompoundSphereDef { + return CompoundSphereDef { sphere: s, material: material } +} + +// --- the block ---------------------------------------------------------------------------------- + +// One block: the tree's nodes, proxies and traversal stack, the material +// table, the capsules, the hull instances then the shared hulls, the +// mesh instances then the shared meshes, the spheres, at byte offsets. +struct CompoundData { + version: int + byte_count: int + node_offset: int + proxy_offset: int + stack_offset: int + tree: DynamicTree // its pointers into this block; never inserted into + material_offset: int + material_count: int + capsule_offset: int + capsule_count: int + hull_offset: int // the HullInstance array + hull_count: int + shared_hull_count: int + mesh_offset: int // the MeshInstance array + mesh_count: int + shared_mesh_count: int + sphere_offset: int + sphere_count: int +} + +// A capsule as stored and as returned. +struct CompoundCapsule { + capsule: Capsule + material_index: int +} + +// A hull instance as stored: the offset of its shared hull in the block. +struct HullInstance { + transform: Transform + hull_offset: int + material_index: int +} + +// A hull instance as returned. +struct CompoundHull { + hull: *HullData + transform: Transform + material_index: int +} + +// A mesh instance as stored: the triangle's material index, clamped to +// four, selects one of its slots in the compound's table. +struct MeshInstance { + transform: Transform + scale: Vec3 + mesh_offset: int + material_index0: int + material_index1: int + material_index2: int + material_index3: int +} + +struct CompoundMesh { + mesh_data: *MeshData + transform: Transform + scale: Vec3 + material_index0: int + material_index1: int + material_index2: int + material_index3: int +} + +struct CompoundSphere { + sphere: Sphere + material_index: int +} + +// A child of any kind: the fields of the other kinds are unused. The +// capsule's and sphere's transform is the identity, their place being +// in their centres; material index 0 serves the convex kinds. +struct ChildShape { + kind: int + capsule: Capsule + hull: *HullData + mesh: Mesh + sphere: Sphere + transform: Transform + material_index0: int + material_index1: int + material_index2: int + material_index3: int +} + +align8(x: int) -> int { return (x + 7) & (0 - 8) } + +get_compound_materials(c: *CompoundData) -> SurfaceMaterial[] { return ((c as ptr) + c.material_offset) as SurfaceMaterial[] } + +get_compound_material(c: *CompoundData, index: int) -> SurfaceMaterial { + materials = get_compound_materials(c) + return materials[index] +} + +get_compound_capsule(c: *CompoundData, index: int) -> CompoundCapsule { + capsules = ((c as ptr) + c.capsule_offset) as CompoundCapsule[] + return capsules[index] +} + +get_compound_hull(c: *CompoundData, index: int) -> CompoundHull { + instances = ((c as ptr) + c.hull_offset) as HullInstance[] + return CompoundHull { hull: ((c as ptr) + instances[index].hull_offset) as *HullData, transform: instances[index].transform, + material_index: instances[index].material_index } +} + +get_compound_mesh(c: *CompoundData, index: int) -> CompoundMesh { + instances = ((c as ptr) + c.mesh_offset) as MeshInstance[] + return CompoundMesh { mesh_data: ((c as ptr) + instances[index].mesh_offset) as *MeshData, transform: instances[index].transform, + scale: instances[index].scale, material_index0: instances[index].material_index0, + material_index1: instances[index].material_index1, material_index2: instances[index].material_index2, + material_index3: instances[index].material_index3 } +} + +get_compound_sphere(c: *CompoundData, index: int) -> CompoundSphere { + spheres = ((c as ptr) + c.sphere_offset) as CompoundSphere[] + return spheres[index] +} + +compound_child_count(c: *CompoundData) -> int { return c.capsule_count + c.hull_count + c.mesh_count + c.sphere_count } + +empty_child() -> ChildShape { + return ChildShape { kind: KIND_SPHERE, capsule: Capsule { center1: math.vec3_zero(), center2: math.vec3_zero(), radius: 0.0 }, + hull: null, mesh: Mesh { data: null, scale: math.vec3_one() }, sphere: Sphere { center: math.vec3_zero(), radius: 0.0 }, + transform: math.transform_identity(), material_index0: 0, material_index1: 0, material_index2: 0, material_index3: 0 } +} + +// The child at an index, in the order capsules, hulls, meshes, spheres. +get_compound_child(c: *CompoundData, child_index: int) -> ChildShape { + child = empty_child() + index = child_index + if index < c.capsule_count { + cc = get_compound_capsule(c, index) + child.kind = KIND_CAPSULE + child.capsule = cc.capsule + child.material_index0 = cc.material_index + return child + } + index = index - c.capsule_count + if index < c.hull_count { + ch = get_compound_hull(c, index) + child.kind = KIND_HULL + child.hull = ch.hull + child.transform = ch.transform + child.material_index0 = ch.material_index + return child + } + index = index - c.hull_count + if index < c.mesh_count { + cm = get_compound_mesh(c, index) + child.kind = KIND_MESH + child.mesh = Mesh { data: cm.mesh_data, scale: cm.scale } + child.transform = cm.transform + child.material_index0 = cm.material_index0 + child.material_index1 = cm.material_index1 + child.material_index2 = cm.material_index2 + child.material_index3 = cm.material_index3 + return child + } + index = index - c.mesh_count + cs = get_compound_sphere(c, index) + child.kind = KIND_SPHERE + child.sphere = cs.sphere + child.material_index0 = cs.material_index + return child +} + +child_material(child: ChildShape, slot: int) -> int { + if slot <= 0 { return child.material_index0 } + if slot == 1 { return child.material_index1 } + if slot == 2 { return child.material_index2 } + return child.material_index3 +} + +// --- creation -------------------------------------------------------------------------------------- + +// The material table under construction: the materials so far, and a +// map from a material's hash to its index. +struct MaterialTable { + materials: ptr // SurfaceMaterial[] + count: int + capacity: int + map: LongMap + scratch: ptr // the eight words a material hashes as +} + +// A material's hash over its fields, written as eight words so the +// struct's padding never enters. +material_hash(scratch: ptr, m: SurfaceMaterial) -> long { + words = scratch as float[] + words[0] = m.friction + words[1] = m.restitution + words[2] = m.rolling_resistance + words[3] = m.tangent_velocity.x + words[4] = m.tangent_velocity.y + words[5] = m.tangent_velocity.z + ids = scratch as long[] + ids[6] = m.user_material_id + ids[7] = m.custom_color as long + return core.hash_bytes(scratch, 64) +} + +// The index of a material in the table, added if new. +table_index(t: *MaterialTable, m: SurfaceMaterial) -> int { + h = material_hash(t.scratch, m) + materials = t.materials as SurfaceMaterial[] + found = core.map_get(&t.map, h, NULL_INDEX) + if found != NULL_INDEX && material.same_material(materials[found], m) { return found } + index = t.count + materials[index] = m + t.count = t.count + 1 + if found == NULL_INDEX { core.map_set(&t.map, h, index) } + return index +} + +// A shared block (a hull or a mesh) by its hash, verified by its bytes; +// the index of an equal one seen before, or the count (a new one). +shared_index(map: *LongMap, hash: long, block: ptr, byte_count: int, blocks: ptr[], counts: int[], count: int) -> int { + found = core.map_get(map, hash, NULL_INDEX) + if found != NULL_INDEX && counts[found] == byte_count && memcmp(blocks[found], block, byte_count) == 0 { return found } + blocks[count] = block + counts[count] = byte_count + if found == NULL_INDEX { core.map_set(map, hash, count) } + return count +} + +all_bits() -> long { return (0 as long) - (1 as long) } + +// The compound from its definition: the children's bounds go into a +// tree rebuilt in full, the materials into a shared table, the hulls and +// meshes deduplicated by content, then everything is packed into one +// block. Null when there are no children or too many. +create_compound(def: *CompoundDef) -> *CompoundData { + capsule_count = def.capsule_count + hull_count = def.hull_count + mesh_count = def.mesh_count + sphere_count = def.sphere_count + convex_count = capsule_count + hull_count + sphere_count + shape_count = convex_count + mesh_count + if shape_count == 0 || shape_count >= MAX_CHILD_SHAPES { return null } + + tree = dynamic_tree.tree_create(shape_count) + child_index = 0 + capsule_defs = def.capsules as CompoundCapsuleDef[] + hull_defs = def.hulls as CompoundHullDef[] + mesh_defs = def.meshes as CompoundMeshDef[] + sphere_defs = def.spheres as CompoundSphereDef[] + + capsule_instances_block = core.alloc(math.max_int(capsule_count, 1) * sizeof(CompoundCapsule)) + capsule_instances = capsule_instances_block as CompoundCapsule[] + hull_instances_block = core.alloc(math.max_int(hull_count, 1) * sizeof(HullInstance)) + hull_instances = hull_instances_block as HullInstance[] + mesh_instances_block = core.alloc(math.max_int(mesh_count, 1) * sizeof(MeshInstance)) + mesh_instances = mesh_instances_block as MeshInstance[] + sphere_instances_block = core.alloc(math.max_int(sphere_count, 1) * sizeof(CompoundSphere)) + sphere_instances = sphere_instances_block as CompoundSphere[] + + material_capacity = convex_count + i = 0 + while i < mesh_count { + material_capacity = material_capacity + math.max_int(mesh_defs[i].material_count, 1) + i = i + 1 + } + table = MaterialTable { materials: core.alloc(material_capacity * sizeof(SurfaceMaterial)), count: 0, capacity: material_capacity, + map: core.map_create(2 * material_capacity), scratch: core.alloc(64) } + + // Capsules. + i = 0 + while i < capsule_count { + capsule_instances[i] = CompoundCapsule { capsule: capsule_defs[i].capsule, material_index: table_index(&table, capsule_defs[i].material) } + box = capsule.compute_capsule_aabb(capsule_defs[i].capsule, math.transform_identity()) + dynamic_tree.tree_create_proxy(&tree, box, all_bits(), child_index as long) + child_index = child_index + 1 + i = i + 1 + } + + // Hulls: an instance each, the shared ones once. + shared_hulls_block = core.alloc(math.max_int(hull_count, 1) * 8) + shared_hulls = shared_hulls_block as ptr[] + shared_hull_sizes_block = core.alloc(math.max_int(hull_count, 1) * 4) + shared_hull_sizes = shared_hull_sizes_block as int[] + shared_hull_offsets_block = core.alloc(math.max_int(hull_count, 1) * 4) + shared_hull_offsets = shared_hull_offsets_block as int[] + shared_hull_count = 0 + hull_map = core.map_create(2 * math.max_int(hull_count, 1)) + i = 0 + while i < hull_count { + h = hull_defs[i].hull + box = hull.compute_hull_aabb(h, hull_defs[i].transform) + dynamic_tree.tree_create_proxy(&tree, box, all_bits(), child_index as long) + child_index = child_index + 1 + shared = shared_index(&hull_map, h.hash, h as ptr, h.byte_count, shared_hulls, shared_hull_sizes, shared_hull_count) + if shared == shared_hull_count { shared_hull_count = shared_hull_count + 1 } + // The offset is not known yet: the shared index stands in for it. + hull_instances[i] = HullInstance { transform: hull_defs[i].transform, hull_offset: shared, material_index: table_index(&table, hull_defs[i].material) } + i = i + 1 + } + core.map_destroy(&hull_map) + + // Meshes. + shared_meshes_block = core.alloc(math.max_int(mesh_count, 1) * 8) + shared_meshes = shared_meshes_block as ptr[] + shared_mesh_sizes_block = core.alloc(math.max_int(mesh_count, 1) * 4) + shared_mesh_sizes = shared_mesh_sizes_block as int[] + shared_mesh_offsets_block = core.alloc(math.max_int(mesh_count, 1) * 4) + shared_mesh_offsets = shared_mesh_offsets_block as int[] + shared_mesh_count = 0 + mesh_map = core.map_create(2 * math.max_int(mesh_count, 1)) + i = 0 + while i < mesh_count { + md = mesh_defs[i].mesh_data + box = mesh.compute_mesh_aabb(md, mesh_defs[i].transform, mesh_defs[i].scale) + dynamic_tree.tree_create_proxy(&tree, box, all_bits(), child_index as long) + child_index = child_index + 1 + // The mesh's materials go through the same table; only four slots travel. + slots = math.min_int(mesh_defs[i].material_count, MAX_COMPOUND_MESH_MATERIALS) + mesh_materials = mesh_defs[i].materials as SurfaceMaterial[] + instance = MeshInstance { transform: mesh_defs[i].transform, scale: mesh_defs[i].scale, mesh_offset: 0, + material_index0: 0, material_index1: 0, material_index2: 0, material_index3: 0 } + if slots > 0 { instance.material_index0 = table_index(&table, mesh_materials[0]) } + if slots > 1 { instance.material_index1 = table_index(&table, mesh_materials[1]) } + if slots > 2 { instance.material_index2 = table_index(&table, mesh_materials[2]) } + if slots > 3 { instance.material_index3 = table_index(&table, mesh_materials[3]) } + shared = shared_index(&mesh_map, md.hash, md as ptr, md.byte_count, shared_meshes, shared_mesh_sizes, shared_mesh_count) + if shared == shared_mesh_count { shared_mesh_count = shared_mesh_count + 1 } + instance.mesh_offset = shared + mesh_instances[i] = instance + i = i + 1 + } + core.map_destroy(&mesh_map) + + // Spheres. + i = 0 + while i < sphere_count { + sphere_instances[i] = CompoundSphere { sphere: sphere_defs[i].sphere, material_index: table_index(&table, sphere_defs[i].material) } + box = sphere.compute_sphere_aabb(sphere_defs[i].sphere, math.transform_identity()) + dynamic_tree.tree_create_proxy(&tree, box, all_bits(), child_index as long) + child_index = child_index + 1 + i = i + 1 + } + + dynamic_tree.tree_rebuild(&tree, true) + + // The layout. The rebuild is dense, so only the live nodes travel. + byte_count = align8(sizeof(CompoundData)) + node_offset = byte_count + byte_count = byte_count + align8(tree.node_end * sizeof(TreeNode)) + proxy_offset = byte_count + byte_count = byte_count + align8(tree.proxy_count * sizeof(TreeProxy)) + stack_offset = byte_count + byte_count = byte_count + align8(dynamic_tree.TREE_STACK_SIZE * 4) + material_offset = byte_count + byte_count = byte_count + align8(table.count * sizeof(SurfaceMaterial)) + capsule_offset = byte_count + byte_count = byte_count + align8(capsule_count * sizeof(CompoundCapsule)) + hull_offset = byte_count + byte_count = byte_count + align8(hull_count * sizeof(HullInstance)) + i = 0 + while i < shared_hull_count { + shared_hull_offsets[i] = byte_count + byte_count = byte_count + align8(shared_hull_sizes[i]) + i = i + 1 + } + mesh_offset = byte_count + byte_count = byte_count + align8(mesh_count * sizeof(MeshInstance)) + i = 0 + while i < shared_mesh_count { + shared_mesh_offsets[i] = byte_count + byte_count = byte_count + align8(shared_mesh_sizes[i]) + i = i + 1 + } + sphere_offset = byte_count + byte_count = byte_count + align8(sphere_count * sizeof(CompoundSphere)) + + block = core.alloc(byte_count) + memset(block, 0, byte_count) + c = block as *CompoundData + c.version = COMPOUND_VERSION + c.byte_count = byte_count + c.node_offset = node_offset + c.proxy_offset = proxy_offset + c.stack_offset = stack_offset + c.material_offset = material_offset + c.material_count = table.count + c.capsule_offset = capsule_offset + c.capsule_count = capsule_count + c.hull_offset = hull_offset + c.hull_count = hull_count + c.shared_hull_count = shared_hull_count + c.mesh_offset = mesh_offset + c.mesh_count = mesh_count + c.shared_mesh_count = shared_mesh_count + c.sphere_offset = sphere_offset + c.sphere_count = sphere_count + + // The baked tree: its nodes, proxies and stack in the block; nothing + // for insertions or rebuilds. + memcpy(block + node_offset, tree.nodes, tree.node_end * sizeof(TreeNode)) + memcpy(block + proxy_offset, tree.proxies, tree.proxy_count * sizeof(TreeProxy)) + c.tree = DynamicTree { nodes: block + node_offset, parents: null, proxies: block + proxy_offset, + node_end: tree.node_end, node_capacity: tree.node_end, pair_free_list: NULL_INDEX, + proxy_count: tree.proxy_count, proxy_capacity: tree.proxy_count, proxy_free_list: NULL_INDEX, + swap_nodes: null, leaf_indices: null, leaf_nodes: null, leaf_centers: null, + rebuild_capacity: 0, dfs_ordered: tree.dfs_ordered, + stack: block + stack_offset, stack_dist: null, copy_stack: null } + dynamic_tree.tree_destroy(&tree) + + memcpy(block + material_offset, table.materials, table.count * sizeof(SurfaceMaterial)) + if capsule_count > 0 { memcpy(block + capsule_offset, capsule_instances_block, capsule_count * sizeof(CompoundCapsule)) } + i = 0 + while i < hull_count { + hull_instances[i].hull_offset = shared_hull_offsets[hull_instances[i].hull_offset] + i = i + 1 + } + if hull_count > 0 { memcpy(block + hull_offset, hull_instances_block, hull_count * sizeof(HullInstance)) } + i = 0 + while i < shared_hull_count { + memcpy(block + shared_hull_offsets[i], shared_hulls[i], shared_hull_sizes[i]) + i = i + 1 + } + i = 0 + while i < mesh_count { + mesh_instances[i].mesh_offset = shared_mesh_offsets[mesh_instances[i].mesh_offset] + i = i + 1 + } + if mesh_count > 0 { memcpy(block + mesh_offset, mesh_instances_block, mesh_count * sizeof(MeshInstance)) } + i = 0 + while i < shared_mesh_count { + memcpy(block + shared_mesh_offsets[i], shared_meshes[i], shared_mesh_sizes[i]) + i = i + 1 + } + if sphere_count > 0 { memcpy(block + sphere_offset, sphere_instances_block, sphere_count * sizeof(CompoundSphere)) } + + core.map_destroy(&table.map) + core.free_bytes(table.scratch, 64) + core.free_bytes(table.materials, material_capacity * sizeof(SurfaceMaterial)) + core.free_bytes(shared_hulls_block, math.max_int(hull_count, 1) * 8) + core.free_bytes(shared_hull_sizes_block, math.max_int(hull_count, 1) * 4) + core.free_bytes(shared_hull_offsets_block, math.max_int(hull_count, 1) * 4) + core.free_bytes(shared_meshes_block, math.max_int(mesh_count, 1) * 8) + core.free_bytes(shared_mesh_sizes_block, math.max_int(mesh_count, 1) * 4) + core.free_bytes(shared_mesh_offsets_block, math.max_int(mesh_count, 1) * 4) + core.free_bytes(capsule_instances_block, math.max_int(capsule_count, 1) * sizeof(CompoundCapsule)) + core.free_bytes(hull_instances_block, math.max_int(hull_count, 1) * sizeof(HullInstance)) + core.free_bytes(mesh_instances_block, math.max_int(mesh_count, 1) * sizeof(MeshInstance)) + core.free_bytes(sphere_instances_block, math.max_int(sphere_count, 1) * sizeof(CompoundSphere)) + return c +} + +destroy_compound(c: *CompoundData) { core.free_bytes(c as ptr, c.byte_count) } + +// --- queries --------------------------------------------------------------------------------------- + +compute_compound_aabb(c: *CompoundData, t: Transform) -> AABB { + return math.aabb_transform(t, dynamic_tree.tree_get_root_bounds(&c.tree)) +} + +// The proxy in the compound's frame, one query at a time. +struct OverlapContext { + compound: *CompoundData + proxy: ShapeProxy + overlap: bool +} + +overlap_visitor(proxy_id: int, user_data: long, context: ptr) -> bool { + ctx = context as *OverlapContext + child = get_compound_child(ctx.compound, user_data as int) + hit = false + if child.kind == KIND_CAPSULE { hit = capsule.overlap_capsule(child.capsule, child.transform, ctx.proxy) } + else if child.kind == KIND_HULL { hit = hull.overlap_hull(child.hull, child.transform, ctx.proxy) } + else if child.kind == KIND_MESH { hit = mesh.overlap_mesh(child.mesh, child.transform, ctx.proxy) } + else if child.kind == KIND_SPHERE { hit = sphere.overlap_sphere(child.sphere, child.transform, ctx.proxy) } + if hit { + ctx.overlap = true + return false + } + return true +} + +var g_local: ptr = null // Vec3[MAX_SHAPE_CAST_POINTS]: the proxy in the compound's frame +var g_child: ptr = null // Vec3[MAX_SHAPE_CAST_POINTS]: the proxy in a child's frame + +scratch_ready() { + if g_local == null { + g_local = core.alloc(distance.MAX_SHAPE_CAST_POINTS * sizeof(Vec3)) + g_child = core.alloc(distance.MAX_SHAPE_CAST_POINTS * sizeof(Vec3)) + } +} + +// Whether a proxy in world space overlaps any child of the compound +// under its transform: the proxy is brought into the compound's frame, +// the tree culls, each child tests in its own frame. +overlap_compound(c: *CompoundData, shape_transform: Transform, proxy: ShapeProxy) -> bool { + scratch_ready() + ctx = OverlapContext { compound: c, proxy: mesh.make_local_proxy(proxy, shape_transform, g_local), overlap: false } + bounds = mesh.compute_proxy_aabb(ctx.proxy) + dynamic_tree.tree_query(&c.tree, bounds, all_bits(), false, overlap_visitor, (&ctx) as ptr) + return ctx.overlap +} + +// A cast in progress: the best hit so far, and the shape cast's proxy +// (the tree's box cast carries only the advancing fraction). +struct CastContext { + compound: *CompoundData + output: CastOutput + proxy: ShapeProxy + translation: Vec3 + can_encroach: bool +} + +// A child's material for a hit: slot 0 for the convex kinds, the +// triangle's (clamped to four) for a mesh. +hit_material(child: ChildShape, output: CastOutput) -> int { + if child.kind == KIND_MESH { return child_material(child, math.min_int(output.material_index, MAX_COMPOUND_MESH_MATERIALS - 1)) } + return child.material_index0 +} + +ray_visitor(input: *RayCastInput, proxy_id: int, user_data: long, context: ptr) -> float { + ctx = context as *CastContext + child_index = user_data as int + child = get_compound_child(ctx.compound, child_index) + origin = math.inv_transform_point(child.transform, input.origin) + translation = math.inv_rotate_vector(child.transform.q, input.translation) + output = distance.empty_cast_output() + if child.kind == KIND_CAPSULE { output = capsule.ray_cast_capsule(child.capsule, origin, translation, input.max_fraction) } + else if child.kind == KIND_HULL { output = hull.ray_cast_hull(child.hull, origin, translation, input.max_fraction) } + else if child.kind == KIND_MESH { output = mesh.ray_cast_mesh(child.mesh, origin, translation, input.max_fraction) } + else if child.kind == KIND_SPHERE { output = sphere.ray_cast_sphere(child.sphere, origin, translation, input.max_fraction) } + if output.hit { + output.material_index = hit_material(child, output) + output.point = math.transform_point(child.transform, output.point) + output.normal = math.rotate_vector(child.transform.q, output.normal) + output.child_index = child_index + ctx.output = output + return output.fraction + } + return input.max_fraction +} + +// A ray in the compound's frame against its children, nearest first +// through the tree, with the child and the material of the hit. +ray_cast_compound(c: *CompoundData, origin: Vec3, translation: Vec3, max_fraction: float) -> CastOutput { + ctx = CastContext { compound: c, output: distance.empty_cast_output(), proxy: distance.shape_proxy(null, 0, 0.0), + translation: translation, can_encroach: false } + input = RayCastInput { origin: origin, translation: translation, max_fraction: max_fraction } + dynamic_tree.tree_ray_cast(&c.tree, &input, all_bits(), false, ray_visitor, (&ctx) as ptr) + return ctx.output +} + +box_visitor(input: *BoxCastInput, proxy_id: int, user_data: long, context: ptr) -> float { + ctx = context as *CastContext + child_index = user_data as int + child = get_compound_child(ctx.compound, child_index) + local_proxy = mesh.make_local_proxy(ctx.proxy, child.transform, g_child) + translation = math.inv_rotate_vector(child.transform.q, ctx.translation) + output = distance.empty_cast_output() + if child.kind == KIND_CAPSULE { output = capsule.shape_cast_capsule(child.capsule, local_proxy, translation, input.max_fraction, ctx.can_encroach) } + else if child.kind == KIND_HULL { output = hull.shape_cast_hull(child.hull, local_proxy, translation, input.max_fraction, ctx.can_encroach) } + else if child.kind == KIND_MESH { output = mesh.shape_cast_mesh(child.mesh, local_proxy, translation, input.max_fraction, ctx.can_encroach) } + else if child.kind == KIND_SPHERE { output = sphere.shape_cast_sphere(child.sphere, local_proxy, translation, input.max_fraction, ctx.can_encroach) } + if output.hit { + output.material_index = hit_material(child, output) + output.point = math.transform_point(child.transform, output.point) + output.normal = math.rotate_vector(child.transform.q, output.normal) + output.child_index = child_index + ctx.output = output + return output.fraction + } + return input.max_fraction +} + +// A proxy in the compound's frame swept against its children: the tree +// casts the proxy's box, each child the proxy in its own frame. +shape_cast_compound(c: *CompoundData, proxy: ShapeProxy, translation: Vec3, max_fraction: float, can_encroach: bool) -> CastOutput { + if proxy.count == 0 { return distance.empty_cast_output() } + scratch_ready() + ctx = CastContext { compound: c, output: distance.empty_cast_output(), proxy: proxy, translation: translation, can_encroach: can_encroach } + input = BoxCastInput { box: mesh.compute_proxy_aabb(proxy), translation: translation, max_fraction: max_fraction } + dynamic_tree.tree_box_cast(&c.tree, &input, all_bits(), false, box_visitor, (&ctx) as ptr) + return ctx.output +} + +struct QueryContext { + compound: *CompoundData + visitor: fn(*CompoundData, int, ptr) -> bool + user_context: ptr +} + +query_visitor(proxy_id: int, user_data: long, context: ptr) -> bool { + ctx = context as *QueryContext + return ctx.visitor(ctx.compound, user_data as int, ctx.user_context) +} + +// Every child whose bounds overlap a box in the compound's frame, to the +// visitor, until it returns false. +query_compound(c: *CompoundData, bounds: AABB, visitor: fn(*CompoundData, int, ptr) -> bool, context: ptr) { + ctx = QueryContext { compound: c, visitor: visitor, user_context: context } + dynamic_tree.tree_query(&c.tree, bounds, dynamic_tree.default_mask_bits(), false, query_visitor, (&ctx) as ptr) +} + +struct MoverContext { + compound: *CompoundData + planes: PlaneResult[] + capacity: int + count: int + mover: Capsule +} + +const MOVER_SCRATCH = 64 +var g_planes: ptr = null // PlaneResult[MOVER_SCRATCH]: one child's planes before they are appended + +mover_visitor(proxy_id: int, user_data: long, context: ptr) -> bool { + ctx = context as *MoverContext + child_index = user_data as int + child = get_compound_child(ctx.compound, child_index) + local_mover = Capsule { center1: math.inv_transform_point(child.transform, ctx.mover.center1), + center2: math.inv_transform_point(child.transform, ctx.mover.center2), radius: ctx.mover.radius } + // A child writes into the scratch (a mesh up to its size), then its planes are appended. + capacity = math.min_int(ctx.capacity - ctx.count, MOVER_SCRATCH) + scratch = g_planes as PlaneResult[] + plane_count = 0 + if child.kind == KIND_CAPSULE { plane_count = capsule.collide_mover_and_capsule(scratch, child.capsule, local_mover) } + else if child.kind == KIND_HULL { plane_count = mesh.collide_mover_and_hull(scratch, child.hull, local_mover) } + else if child.kind == KIND_MESH { plane_count = mesh.collide_mover_and_mesh(scratch, capacity, child.mesh, local_mover) } + else if child.kind == KIND_SPHERE { plane_count = sphere.collide_mover_and_sphere(scratch, child.sphere, local_mover) } + planes = ctx.planes + i = 0 + while i < plane_count { + p = scratch[i] + p.plane.normal = math.rotate_vector(child.transform.q, p.plane.normal) + p.point = math.transform_point(child.transform, p.point) + p.child_index = child_index + p.material_index = child_material(child, math.min_int(p.material_index, MAX_COMPOUND_MESH_MATERIALS - 1)) + planes[ctx.count + i] = p + i = i + 1 + } + ctx.count = ctx.count + plane_count + return ctx.count < ctx.capacity +} + +// The planes of the children the mover's capsule (in the compound's +// frame) touches, up to the capacity, each carried back from its +// child's frame with the child and the material. +collide_mover_and_compound(planes: PlaneResult[], capacity: int, c: *CompoundData, mover: Capsule) -> int { + if capacity == 0 { return 0 } + if g_planes == null { g_planes = core.alloc(MOVER_SCRATCH * sizeof(PlaneResult)) } + ctx = MoverContext { compound: c, planes: planes, capacity: capacity, count: 0, mover: mover } + r = math.vec3(mover.radius, mover.radius, mover.radius) + bounds = AABB { lower: math.sub(math.min_vec3(mover.center1, mover.center2), r), upper: math.add(math.max_vec3(mover.center1, mover.center2), r) } + dynamic_tree.tree_query(&c.tree, bounds, all_bits(), false, mover_visitor, (&ctx) as ptr) + return ctx.count +} + +// A child's sweep at rest under the compound's transform (xf = compound * child). +make_compound_child_sweep(compound_transform: Transform, child_transform: Transform) -> Sweep { + xf = math.mul_transforms(compound_transform, child_transform) + return Sweep { local_center: math.vec3_zero(), c1: xf.p, c2: xf.p, q1: xf.q, q2: xf.q } +} diff --git a/aephysics/dynamic_tree/module.ae b/aephysics/dynamic_tree/module.ae index 57fd5b3..7cd0441 100644 --- a/aephysics/dynamic_tree/module.ae +++ b/aephysics/dynamic_tree/module.ae @@ -722,7 +722,8 @@ validate_subtree(t: *DynamicTree, node_index: int, leaf_count: int[]) -> int { pair = left_child(n.flag_index) if (pair & 1) != 0 || pair < 2 || pair >= t.node_end { return 0 - 1 } - if parents[pair] != node_index || parents[pair + 1] != node_index { return 0 - 1 } + // A baked tree (a compound's) carries no parents. + if t.parents != null && (parents[pair] != node_index || parents[pair + 1] != node_index) { return 0 - 1 } if t.dfs_ordered && node_index >= pair { return 0 - 1 } c1 = nodes[pair] c2 = nodes[pair + 1] @@ -746,7 +747,7 @@ tree_validate(t: *DynamicTree) -> bool { nodes = t.nodes as TreeNode[] parents = t.parents as int[] proxies = t.proxies as TreeProxy[] - if parents[ROOT_NODE] != NULL_INDEX { return false } + if t.parents != null && parents[ROOT_NODE] != NULL_INDEX { return false } if is_empty_node(nodes[ROOT_NODE + 1].flag_index) == false { return false } free_pair_count = 0 @@ -754,6 +755,7 @@ tree_validate(t: *DynamicTree) -> bool { while pair != NULL_INDEX { if (pair & 1) != 0 || pair < 2 || pair >= t.node_end { return false } if is_empty_node(nodes[pair].flag_index) == false || is_empty_node(nodes[pair + 1].flag_index) == false { return false } + if t.parents == null { return false } pair = parents[pair] free_pair_count = free_pair_count + 1 if 2 * free_pair_count >= t.node_end { return false } diff --git a/aephysics/material/module.ae b/aephysics/material/module.ae new file mode 100644 index 0000000..46c15d1 --- /dev/null +++ b/aephysics/material/module.ae @@ -0,0 +1,30 @@ +// aephysics.material -- a surface's material: what a contact takes from +// the shape it touches. Box3D's b3SurfaceMaterial (Erin Catto, MIT), +// with its default; the friction and restitution combining rules come +// with the contacts. +import std.string +import aephysics.math + +exports ( SurfaceMaterial, default_surface_material, same_material ) + +// A surface's material. +struct SurfaceMaterial { + friction: float // the Coulomb coefficient, usually in [0, 1] + restitution: float // the bounce, usually in [0, 1] + rolling_resistance: float // spheres and capsules only + tangent_velocity: Vec3 // a conveyor belt's, in the shape's frame + user_material_id: long // passed through query results and the combining functions + custom_color: int // a debug colour; 0 for none +} + +default_surface_material() -> SurfaceMaterial { + return SurfaceMaterial { friction: 0.6, restitution: 0.0, rolling_resistance: 0.0, tangent_velocity: math.vec3_zero(), + user_material_id: 0 as long, custom_color: 0 } +} + +// Field by field, so the struct's padding never enters. +same_material(a: SurfaceMaterial, b: SurfaceMaterial) -> bool { + return a.friction == b.friction && a.restitution == b.restitution && a.rolling_resistance == b.rolling_resistance && + a.tangent_velocity.x == b.tangent_velocity.x && a.tangent_velocity.y == b.tangent_velocity.y && + a.tangent_velocity.z == b.tangent_velocity.z && a.user_material_id == b.user_material_id && a.custom_color == b.custom_color +} diff --git a/aephysics/mesh/module.ae b/aephysics/mesh/module.ae index 366c6f0..3dd3397 100644 --- a/aephysics/mesh/module.ae +++ b/aephysics/mesh/module.ae @@ -34,7 +34,7 @@ exports ( create_grid_mesh, create_wave_mesh, create_torus_mesh, create_box_mesh, create_hollow_box_mesh, create_platform_mesh, overlap_mesh, compute_mesh_aabb, ray_cast_mesh, shape_cast_mesh, get_mesh_triangle, - collide_mover_and_mesh, query_mesh, + collide_mover_and_mesh, collide_mover_and_hull, query_mesh, test_bounds_triangle_overlap, intersect_ray_triangle, make_local_proxy, compute_proxy_aabb ) @@ -1484,6 +1484,26 @@ get_mesh_triangle(sh: Mesh, triangle_index: int) -> Triangle { i1: t.index1, i2: t.index2, i3: t.index3, flags: f } } +// 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 { + scratch_ready() + segment = g_local as Vec3[] + segment[0] = mover.center1 + segment[1] = mover.center2 + input = DistanceInput { proxy_a: hull.hull_proxy(h), proxy_b: distance.shape_proxy(g_local, 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 +} + // The planes of the front-facing triangles the mover's capsule is // within its radius of, up to the capacity; returns how many. collide_mover_and_mesh(planes: PlaneResult[], capacity: int, sh: Mesh, mover: Capsule) -> int { diff --git a/aephysics/shape/module.ae b/aephysics/shape/module.ae index 68286db..4a3d000 100644 --- a/aephysics/shape/module.ae +++ b/aephysics/shape/module.ae @@ -4,20 +4,14 @@ // 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. +// and the collision filters. The sphere's and the capsule's own geometry +// are the sphere and capsule modules. // -// 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. +// This is the geometric half of Box3D's shape.c (Erin Catto, MIT), the +// reference this engine is measured against. Names are the reference's +// without its prefix, in snake case: b3RayCastShape is ray_cast_shape. +// The world-bound half (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 @@ -28,26 +22,22 @@ import aephysics.distance import aephysics.manifold import aephysics.mesh import aephysics.height_field +import aephysics.material +import aephysics.sphere +import aephysics.capsule +import aephysics.compound 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, + sphere_shape, capsule_shape, hull_shape, mesh_shape, height_field_shape, compound_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 @@ -56,9 +46,6 @@ 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 @@ -69,7 +56,7 @@ struct Shape { hull: *HullData mesh: Mesh height_field: *HeightFieldData - compound: ptr // *CompoundData, the next layer + compound: *CompoundData } // Who collides with whom: a shape is in the categories of its bits and @@ -137,6 +124,15 @@ height_field_shape(h: *HeightFieldData, material_count: int) -> Shape { return shape } +compound_shape(c: *CompoundData) -> Shape { + shape = empty_shape() + shape.kind = SHAPE_COMPOUND + shape.density = 0.0 + shape.compound = c + shape.material_count = math.max_int(c.material_count, 1) + return shape +} + // --- filters ---------------------------------------------------------------------------- default_filter() -> Filter { return Filter { category_bits: 1 as long, mask_bits: all_bits(), group_index: 0 } } @@ -156,367 +152,16 @@ should_query_collide(shape_filter: Filter, query_filter: QueryFilter) -> bool { 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_CAPSULE { return capsule.compute_capsule_aabb(shape.capsule, t) } + if kind == SHAPE_COMPOUND { return compound.compute_compound_aabb(shape.compound, 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) } + if kind == SHAPE_SPHERE { return sphere.compute_sphere_aabb(shape.sphere, t) } return AABB { lower: t.p, upper: t.p } } @@ -537,15 +182,16 @@ 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_CAPSULE { return capsule.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) } + if kind == SHAPE_SPHERE { return sphere.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_COMPOUND { return math.aabb_center(compound.compute_compound_aabb(shape.compound, math.transform_identity())) } 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)) } @@ -578,9 +224,9 @@ get_shape_projected_area(shape: *Shape, plane_normal: Vec3) -> float { compute_shape_mass(shape: *Shape) -> MassData { kind = shape.kind - if kind == SHAPE_CAPSULE { return compute_capsule_mass(shape.capsule, shape.density) } + if kind == SHAPE_CAPSULE { return capsule.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) } + if kind == SHAPE_SPHERE { return sphere.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) } } @@ -620,6 +266,9 @@ compute_shape_extent(shape: *Shape, local_center: Vec3) -> ShapeExtent { if kind == SHAPE_HEIGHT_FIELD { return aabb_extent(height_field.compute_height_field_aabb(shape.height_field, math.transform_identity()), local_center) } + if kind == SHAPE_COMPOUND { + return aabb_extent(compound.compute_compound_aabb(shape.compound, math.transform_identity()), local_center) + } return ShapeExtent { min_extent: 0.0, max_extent: math.vec3_zero() } } @@ -636,8 +285,9 @@ ray_cast_shape(shape: *Shape, t: Transform, origin: Vec3, translation: Vec3, max 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) } + if kind == SHAPE_CAPSULE { output = capsule.ray_cast_capsule(shape.capsule, local_origin, local_translation, max_fraction) } + else if kind == SHAPE_COMPOUND { output = compound.ray_cast_compound(shape.compound, local_origin, local_translation, max_fraction) } + else if kind == SHAPE_SPHERE { output = sphere.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) } @@ -661,11 +311,12 @@ shape_cast_shape(shape: *Shape, t: Transform, proxy: ShapeProxy, translation: Ve 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) } + if kind == SHAPE_CAPSULE { output = capsule.shape_cast_capsule(shape.capsule, local_proxy, local_translation, max_fraction, can_encroach) } + else if kind == SHAPE_COMPOUND { output = compound.shape_cast_compound(shape.compound, 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 if kind == SHAPE_SPHERE { output = sphere.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) @@ -674,11 +325,12 @@ shape_cast_shape(shape: *Shape, t: Transform, proxy: ShapeProxy, translation: Ve 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_CAPSULE { return capsule.overlap_capsule(shape.capsule, t, proxy) } + if kind == SHAPE_COMPOUND { return compound.overlap_compound(shape.compound, 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) } + if kind == SHAPE_SPHERE { return sphere.overlap_sphere(shape.sphere, t, proxy) } return false } @@ -690,9 +342,10 @@ collide_mover(planes: PlaneResult[], capacity: int, shape: *Shape, t: Transform, 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) } + if kind == SHAPE_CAPSULE { plane_count = capsule.collide_mover_and_capsule(planes, shape.capsule, local_mover) } + else if kind == SHAPE_COMPOUND { plane_count = compound.collide_mover_and_compound(planes, capacity, shape.compound, local_mover) } + else if kind == SHAPE_SPHERE { plane_count = sphere.collide_mover_and_sphere(planes, shape.sphere, local_mover) } + else if kind == SHAPE_HULL { plane_count = mesh.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 @@ -709,8 +362,8 @@ collide_mover(planes: PlaneResult[], capacity: int, shape: *Shape, t: Transform, // 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_CAPSULE { return sphere.capsule_proxy(shape.capsule) } + if kind == SHAPE_SPHERE { return sphere.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/sphere/module.ae b/aephysics/sphere/module.ae new file mode 100644 index 0000000..f2012e8 --- /dev/null +++ b/aephysics/sphere/module.ae @@ -0,0 +1,195 @@ +// aephysics.sphere -- the sphere: its mass, bounds, the ray against it +// by the closest point on the ray to the centre (the Ray Tracing Gems +// 2019 form that keeps its precision far from the origin), the shape +// cast, the overlap, and the character mover's plane against it; with +// the proxy buffers the capsule shares. +// +// Box3D's sphere.c (Erin Catto, MIT), the reference this engine is +// measured against, including the initial-overlap convention: a ray +// starting inside, or of zero length inside, reports its origin at +// fraction zero with no normal. Names are the reference's without its +// prefix, in snake case: b3RayCastSphere is ray_cast_sphere. +import std.string +import aephysics.math +import aephysics.core +import aephysics.distance +import aephysics.manifold +import aephysics.mesh + +exports ( + OVERLAP_SLOP, + 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, + sphere_proxy, capsule_proxy, segment_proxy, length_scratch +) + +extern sqrt(x: float) -> float + +// Closer than this is an overlap (a tenth of the linear slop). +const OVERLAP_SLOP = 0.0005 + +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[] +} + +// A sphere's centre as a one-point proxy, in the module's buffer until the next call. +sphere_proxy(s: Sphere) -> ShapeProxy { + buffer = center_buffer() + buffer[0] = s.center + return distance.shape_proxy(g_center, 1, s.radius) +} + +// A capsule's two centres as a proxy, in the same buffer. +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) +} + +// Two centres as a proxy with a radius: the mover's core segment. +segment_proxy(a: Vec3, b: Vec3, radius: float) -> ShapeProxy { + buffer = center_buffer() + buffer[0] = a + buffer[1] = b + return distance.shape_proxy(g_center, 2, 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 +} + diff --git a/aephysics/test_compound.ae b/aephysics/test_compound.ae new file mode 100644 index 0000000..e17093f --- /dev/null +++ b/aephysics/test_compound.ae @@ -0,0 +1,812 @@ +// aephysics.compound against the reference's (Box3D's) test_compound.c: +// a mixed compound and one of each single kind, the materials shared, +// distinct, across kinds and through a mesh, the hulls and meshes +// shared by pointer and by content or kept distinct, the child order +// and transforms, the bounds, the ray cast (miss, nearest, a turned +// hull's normal), the shape cast (nearest, miss, the turned hull), the +// mesh material remap, the overlap (plain, transformed, every kind, a +// segment), the box query with its early exit, and the mover (two +// boxes, a turned child); and beyond it the compound through the shape +// dispatch and a field of a thousand spheres. The byte roundtrip is not +// ported. + +import std.string +import aephysics.math +import aephysics.core +import aephysics.dynamic_tree +import aephysics.hull +import aephysics.distance +import aephysics.manifold +import aephysics.mesh +import aephysics.height_field +import aephysics.material +import aephysics.sphere +import aephysics.capsule +import aephysics.compound +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 + +var failures = 0 +var checks = 0 + +ensure(name: string, ok: bool) { + checks = checks + 1 + if !ok { + println("compound: FAIL ${name}") + failures = failures + 1 + } +} + +small(name: string, value: float, tolerance: float) { + ensure("${name} (${value})", math.abs_float(value) < tolerance) +} + +make_material(friction: float, user_id: int) -> SurfaceMaterial { + m = material.default_surface_material() + m.friction = friction + m.user_material_id = user_id as long + return m +} + +// One block per kind of definition array; the def points at them. +struct Defs { + capsules: ptr + hulls: ptr + meshes: ptr + spheres: ptr + def: CompoundDef +} + +make_defs(capsule_count: int, hull_count: int, mesh_count: int, sphere_count: int) -> Defs { + d = Defs { capsules: calloc(math.max_int(capsule_count, 1), sizeof(CompoundCapsuleDef)), + hulls: calloc(math.max_int(hull_count, 1), sizeof(CompoundHullDef)), + meshes: calloc(math.max_int(mesh_count, 1), sizeof(CompoundMeshDef)), + spheres: calloc(math.max_int(sphere_count, 1), sizeof(CompoundSphereDef)), + def: compound.compound_def() } + d.def.capsules = d.capsules + d.def.capsule_count = capsule_count + d.def.hulls = d.hulls + d.def.hull_count = hull_count + d.def.meshes = d.meshes + d.def.mesh_count = mesh_count + d.def.spheres = d.spheres + d.def.sphere_count = sphere_count + return d +} + +free_defs(d: Defs) { + free(d.capsules) + free(d.hulls) + free(d.meshes) + free(d.spheres) +} + +point_proxy(block: ptr, p: Vec3, radius: float) -> ShapeProxy { + points = block as Vec3[] + points[0] = p + return distance.shape_proxy(block, 1, radius) +} + +test_create() { + mat = material.default_surface_material() + box = hull.make_box_hull(0.5, 0.5, 0.5) + md = mesh.create_box_mesh(math.vec3_zero(), math.vec3(0.5, 0.5, 0.5), false) + mats = calloc(1, sizeof(SurfaceMaterial)) + mat_array = mats as SurfaceMaterial[] + mat_array[0] = mat + + d = make_defs(1, 1, 1, 2) + capsules = d.capsules as CompoundCapsuleDef[] + capsules[0] = compound.compound_capsule_def(manifold.capsule(math.vec3(0.0 - 1.0, 0.0, 0.0), math.vec3(1.0, 0.0, 0.0), 0.25), mat) + hulls = d.hulls as CompoundHullDef[] + hulls[0] = compound.compound_hull_def(box, math.transform_identity(), mat) + meshes = d.meshes as CompoundMeshDef[] + meshes[0] = compound.compound_mesh_def(md, math.transform_identity(), math.vec3_one(), mats, 1) + spheres = d.spheres as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3(5.0, 0.0, 0.0), 0.5), mat) + spheres[1] = compound.compound_sphere_def(manifold.sphere(math.vec3(0.0 - 5.0, 0.0, 0.0), 0.5), mat) + c = compound.create_compound(&d.def) + ensure("mixed created", c != null) + ensure("mixed version", c.version == compound.COMPOUND_VERSION) + ensure("mixed bytes", c.byte_count > sizeof(CompoundData)) + ensure("mixed counts", c.capsule_count == 1 && c.hull_count == 1 && c.mesh_count == 1 && c.sphere_count == 2) + ensure("mixed one material", c.material_count == 1) + ensure("mixed shared", c.shared_hull_count == 1 && c.shared_mesh_count == 1) + ensure("mixed tree", c.tree.node_end >= 2 && c.tree.proxy_count == 5 && c.tree.nodes != null && c.tree.proxies != null) + ensure("mixed tree valid", dynamic_tree.tree_validate(&c.tree)) + ensure("mixed child count", compound.compound_child_count(c) == 5) + // The shared hull and mesh are copies: equal bytes at their own addresses. + ch = compound.get_compound_hull(c, 0) + ensure("hull copied", ch.hull != box && ch.hull.byte_count == box.byte_count && ch.hull.hash == box.hash) + ensure("hull copy valid", hull.is_valid_hull(ch.hull)) + cm = compound.get_compound_mesh(c, 0) + ensure("mesh copied", cm.mesh_data != md && cm.mesh_data.byte_count == md.byte_count && cm.mesh_data.hash == md.hash) + ensure("mesh copy valid", mesh.is_valid_mesh(cm.mesh_data)) + compound.destroy_compound(c) + free_defs(d) + + // One of each kind alone. + d = make_defs(1, 0, 0, 0) + capsules = d.capsules as CompoundCapsuleDef[] + capsules[0] = compound.compound_capsule_def(manifold.capsule(math.vec3_zero(), math.vec3(1.0, 0.0, 0.0), 0.5), mat) + c = compound.create_compound(&d.def) + ensure("capsule only", c != null && c.capsule_count == 1 && c.hull_count == 0 && c.mesh_count == 0 && c.sphere_count == 0) + compound.destroy_compound(c) + free_defs(d) + d = make_defs(0, 1, 0, 0) + hulls = d.hulls as CompoundHullDef[] + hulls[0] = compound.compound_hull_def(box, math.transform_identity(), mat) + c = compound.create_compound(&d.def) + ensure("hull only", c != null && c.hull_count == 1 && c.shared_hull_count == 1 && c.capsule_count == 0) + compound.destroy_compound(c) + free_defs(d) + d = make_defs(0, 0, 1, 0) + meshes = d.meshes as CompoundMeshDef[] + meshes[0] = compound.compound_mesh_def(md, math.transform_identity(), math.vec3_one(), mats, 1) + c = compound.create_compound(&d.def) + ensure("mesh only", c != null && c.mesh_count == 1 && c.shared_mesh_count == 1) + compound.destroy_compound(c) + free_defs(d) + d = make_defs(0, 0, 0, 1) + spheres = d.spheres as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3_zero(), 1.0), mat) + c = compound.create_compound(&d.def) + ensure("sphere only", c != null && c.sphere_count == 1) + compound.destroy_compound(c) + free_defs(d) + // Nothing at all. + d = make_defs(0, 0, 0, 0) + ensure("an empty compound is refused", compound.create_compound(&d.def) == null) + free_defs(d) + + mesh.destroy_mesh(md) + hull.destroy_hull(box) + free(mats) +} + +test_materials() { + // Three capsules with one material share one slot. + mat = make_material(0.4, 7) + d = make_defs(3, 0, 0, 0) + capsules = d.capsules as CompoundCapsuleDef[] + i = 0 + while i < 3 { + capsules[i] = compound.compound_capsule_def(manifold.capsule(math.vec3(i as float, 0.0, 0.0), math.vec3((i + 1) as float, 0.0, 0.0), 0.25), mat) + i = i + 1 + } + c = compound.create_compound(&d.def) + ensure("dedup count", c.material_count == 1) + ensure("dedup indices", compound.get_compound_capsule(c, 0).material_index == 0 && compound.get_compound_capsule(c, 1).material_index == 0 && + compound.get_compound_capsule(c, 2).material_index == 0) + small("dedup friction", compound.get_compound_material(c, 0).friction - 0.4, 0.000001) + compound.destroy_compound(c) + + // Three distinct materials keep three slots, each found by its id. + i = 0 + while i < 3 { + capsules[i].material = make_material(0.1 * ((i + 1) as float), i + 1) + i = i + 1 + } + c = compound.create_compound(&d.def) + ensure("distinct count", c.material_count == 3) + mats = compound.get_compound_materials(c) + i = 0 + while i < 3 { + cc = compound.get_compound_capsule(c, i) + ensure("distinct index ${i}", cc.material_index >= 0 && cc.material_index < 3) + ensure("distinct id ${i}", mats[cc.material_index].user_material_id == ((i + 1) as long)) + i = i + 1 + } + // Materials differing only in the id, the colour or the tangent velocity are distinct. + capsules[1].material = make_material(0.1, 1) + capsules[1].material.custom_color = 5 + capsules[2].material = make_material(0.1, 1) + capsules[2].material.tangent_velocity = math.vec3(0.0, 0.0, 1.0) + capsules[0].material = make_material(0.1, 1) + compound.destroy_compound(c) + c = compound.create_compound(&d.def) + ensure("every field distinguishes", c.material_count == 3) + compound.destroy_compound(c) + free_defs(d) + + // One material across a capsule, a hull and a sphere: one slot. + mat = make_material(0.5, 99) + box = hull.make_box_hull(0.5, 0.5, 0.5) + d = make_defs(1, 1, 0, 1) + capsules = d.capsules as CompoundCapsuleDef[] + capsules[0] = compound.compound_capsule_def(manifold.capsule(math.vec3_zero(), math.vec3(1.0, 0.0, 0.0), 0.25), mat) + hulls = d.hulls as CompoundHullDef[] + hulls[0] = compound.compound_hull_def(box, math.transform_identity(), mat) + spheres = d.spheres as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3(5.0, 0.0, 0.0), 0.5), mat) + c = compound.create_compound(&d.def) + ensure("cross-kind count", c.material_count == 1) + ensure("cross-kind indices", compound.get_compound_capsule(c, 0).material_index == 0 && compound.get_compound_hull(c, 0).material_index == 0 && + compound.get_compound_sphere(c, 0).material_index == 0) + compound.destroy_compound(c) + free_defs(d) + + // A mesh's materials go through the same table as the convex ones. + mat = make_material(0.3, 11) + md = mesh.create_box_mesh(math.vec3_zero(), math.vec3_one(), false) + ensure("box mesh has one material", md.material_count == 1) + mats_block = calloc(1, sizeof(SurfaceMaterial)) + mat_array = mats_block as SurfaceMaterial[] + mat_array[0] = mat + d = make_defs(0, 0, 1, 1) + meshes = d.meshes as CompoundMeshDef[] + meshes[0] = compound.compound_mesh_def(md, math.transform_identity(), math.vec3_one(), mats_block, 1) + spheres = d.spheres as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3(5.0, 0.0, 0.0), 0.5), mat) + c = compound.create_compound(&d.def) + ensure("mesh material shared", c.material_count == 1) + compound.destroy_compound(c) + free_defs(d) + free(mats_block) + mesh.destroy_mesh(md) + hull.destroy_hull(box) +} + +test_sharing() { + mat = material.default_surface_material() + // Three instances of one hull pointer: one shared hull. + box = hull.make_box_hull(1.0, 1.0, 1.0) + d = make_defs(0, 3, 0, 0) + hulls = d.hulls as CompoundHullDef[] + i = 0 + while i < 3 { + t = math.transform_identity() + t.p.x = (4 * i) as float + hulls[i] = compound.compound_hull_def(box, t, mat) + i = i + 1 + } + c = compound.create_compound(&d.def) + ensure("hull instances", c.hull_count == 3) + ensure("hull shared by pointer", c.shared_hull_count == 1) + ensure("hull instances share the copy", compound.get_compound_hull(c, 0).hull == compound.get_compound_hull(c, 2).hull) + small("hull instance transform", compound.get_compound_hull(c, 2).transform.p.x - 8.0, 0.000001) + compound.destroy_compound(c) + free_defs(d) + + // Two hulls built alike are byte-identical: one shared hull. + box_b = hull.make_box_hull(1.0, 1.0, 1.0) + ensure("two hull blocks", box != box_b) + d = make_defs(0, 2, 0, 0) + hulls = d.hulls as CompoundHullDef[] + hulls[0] = compound.compound_hull_def(box, math.transform_identity(), mat) + t = math.transform_identity() + t.p.x = 5.0 + hulls[1] = compound.compound_hull_def(box_b, t, mat) + c = compound.create_compound(&d.def) + ensure("hull shared by content", c.shared_hull_count == 1) + compound.destroy_compound(c) + // A different hull stays distinct. + box_c = hull.make_box_hull(2.0, 1.0, 1.0) + hulls[1] = compound.compound_hull_def(box_c, t, mat) + c = compound.create_compound(&d.def) + ensure("hulls distinct", c.shared_hull_count == 2) + compound.destroy_compound(c) + free_defs(d) + hull.destroy_hull(box_c) + hull.destroy_hull(box_b) + hull.destroy_hull(box) + + // The same for meshes. + md = mesh.create_box_mesh(math.vec3_zero(), math.vec3_one(), false) + mats_block = calloc(1, sizeof(SurfaceMaterial)) + mat_array = mats_block as SurfaceMaterial[] + mat_array[0] = mat + d = make_defs(0, 0, 3, 0) + meshes = d.meshes as CompoundMeshDef[] + i = 0 + while i < 3 { + t = math.transform_identity() + t.p.x = (4 * i) as float + meshes[i] = compound.compound_mesh_def(md, t, math.vec3_one(), mats_block, 1) + i = i + 1 + } + c = compound.create_compound(&d.def) + ensure("mesh instances", c.mesh_count == 3) + ensure("mesh shared by pointer", c.shared_mesh_count == 1) + compound.destroy_compound(c) + free_defs(d) + md_b = mesh.create_box_mesh(math.vec3_zero(), math.vec3_one(), false) + ensure("two mesh blocks", md != md_b) + d = make_defs(0, 0, 2, 0) + meshes = d.meshes as CompoundMeshDef[] + meshes[0] = compound.compound_mesh_def(md, math.transform_identity(), math.vec3_one(), mats_block, 1) + t = math.transform_identity() + t.p.x = 5.0 + meshes[1] = compound.compound_mesh_def(md_b, t, math.vec3_one(), mats_block, 1) + c = compound.create_compound(&d.def) + ensure("mesh shared by content", c.shared_mesh_count == 1) + compound.destroy_compound(c) + md_c = mesh.create_box_mesh(math.vec3_zero(), math.vec3(2.0, 1.0, 1.0), false) + meshes[1] = compound.compound_mesh_def(md_c, t, math.vec3_one(), mats_block, 1) + c = compound.create_compound(&d.def) + ensure("meshes distinct", c.shared_mesh_count == 2) + compound.destroy_compound(c) + free_defs(d) + free(mats_block) + mesh.destroy_mesh(md_c) + mesh.destroy_mesh(md_b) + mesh.destroy_mesh(md) +} + +test_children_and_bounds() { + mat = material.default_surface_material() + box = hull.make_box_hull(0.5, 0.5, 0.5) + md = mesh.create_box_mesh(math.vec3_zero(), math.vec3(0.5, 0.5, 0.5), false) + mats_block = calloc(1, sizeof(SurfaceMaterial)) + mat_array = mats_block as SurfaceMaterial[] + mat_array[0] = mat + d = make_defs(2, 1, 1, 1) + capsules = d.capsules as CompoundCapsuleDef[] + capsules[0] = compound.compound_capsule_def(manifold.capsule(math.vec3_zero(), math.vec3(1.0, 0.0, 0.0), 0.2), mat) + capsules[1] = compound.compound_capsule_def(manifold.capsule(math.vec3(0.0, 2.0, 0.0), math.vec3(1.0, 2.0, 0.0), 0.2), mat) + hulls = d.hulls as CompoundHullDef[] + t = math.transform_identity() + t.p = math.vec3(5.0, 0.0, 0.0) + hulls[0] = compound.compound_hull_def(box, t, mat) + meshes = d.meshes as CompoundMeshDef[] + t = math.transform_identity() + t.p = math.vec3(0.0, 0.0, 5.0) + meshes[0] = compound.compound_mesh_def(md, t, math.vec3_one(), mats_block, 1) + spheres = d.spheres as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3(0.0 - 5.0, 0.0, 0.0), 0.5), mat) + c = compound.create_compound(&d.def) + // The order is capsules, hulls, meshes, spheres. + ensure("child kinds", compound.get_compound_child(c, 0).kind == shape.SHAPE_CAPSULE && compound.get_compound_child(c, 1).kind == shape.SHAPE_CAPSULE && + compound.get_compound_child(c, 2).kind == shape.SHAPE_HULL && compound.get_compound_child(c, 3).kind == shape.SHAPE_MESH && + compound.get_compound_child(c, 4).kind == shape.SHAPE_SPHERE) + // A capsule's and a sphere's transform is the identity: their place is in their centres. + cap0 = compound.get_compound_child(c, 0) + small("capsule child transform x", cap0.transform.p.x, math.EPSILON) + small("capsule child transform y", cap0.transform.p.y, math.EPSILON) + small("capsule child transform z", cap0.transform.p.z, math.EPSILON) + small("capsule child centre", cap0.capsule.center2.x - 1.0, math.EPSILON) + sph = compound.get_compound_child(c, 4) + small("sphere child transform", sph.transform.p.x, math.EPSILON) + small("sphere child centre", sph.sphere.center.x + 5.0, math.EPSILON) + // A hull's and a mesh's carry their stored transform. + h = compound.get_compound_child(c, 2) + small("hull child transform", h.transform.p.x - 5.0, math.EPSILON) + m = compound.get_compound_child(c, 3) + small("mesh child transform", m.transform.p.z - 5.0, math.EPSILON) + ensure("mesh child data", m.mesh.data != null) + compound.destroy_compound(c) + free_defs(d) + + // The bounds contain the children and move with the transform. + d = make_defs(0, 0, 0, 2) + spheres = d.spheres as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3(0.0 - 3.0, 0.0, 0.0), 1.0), mat) + spheres[1] = compound.compound_sphere_def(manifold.sphere(math.vec3(4.0, 0.0, 0.0), 0.5), mat) + c = compound.create_compound(&d.def) + local = compound.compute_compound_aabb(c, math.transform_identity()) + ensure("bounds lower x", local.lower.x <= 0.0 - 4.0 + 0.00001) + ensure("bounds upper x", local.upper.x >= 4.5 - 0.00001) + ensure("bounds lower y", local.lower.y <= 0.0 - 1.0 + 0.00001) + ensure("bounds upper y", local.upper.y >= 1.0 - 0.00001) + xf = Transform { p: math.vec3(10.0, 20.0, 30.0), q: math.quat_identity() } + world = compound.compute_compound_aabb(c, xf) + small("moved bounds x", world.lower.x - (local.lower.x + 10.0), 0.0001) + small("moved bounds y", world.upper.y - (local.upper.y + 20.0), 0.0001) + small("moved bounds z", world.lower.z - (local.lower.z + 30.0), 0.0001) + compound.destroy_compound(c) + free_defs(d) + free(mats_block) + mesh.destroy_mesh(md) + hull.destroy_hull(box) +} + +test_casts() { + mat = material.default_surface_material() + point_block = calloc(1, sizeof(Vec3)) + + // A ray above a sphere misses. + d = make_defs(0, 0, 0, 1) + spheres = d.spheres as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3_zero(), 0.5), mat) + c = compound.create_compound(&d.def) + ensure("ray miss", compound.ray_cast_compound(c, math.vec3(0.0 - 5.0, 5.0, 0.0), math.vec3(10.0, 0.0, 0.0), 1.0).hit == false) + compound.destroy_compound(c) + free_defs(d) + + // Two spheres along x: the ray hits the nearer, with its material. + d = make_defs(0, 0, 0, 2) + spheres = d.spheres as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3(5.0, 0.0, 0.0), 1.0), make_material(0.4, 100)) + spheres[1] = compound.compound_sphere_def(manifold.sphere(math.vec3(10.0, 0.0, 0.0), 1.0), make_material(0.4, 200)) + c = compound.create_compound(&d.def) + out = compound.ray_cast_compound(c, math.vec3_zero(), math.vec3(20.0, 0.0, 0.0), 1.0) + ensure("ray nearest hit", out.hit) + small("ray nearest fraction", out.fraction - 0.2, 0.0001) + small("ray nearest normal", out.normal.x + 1.0, 0.0001) + ensure("ray nearest child", out.child_index == 0) + ensure("ray nearest material", compound.get_compound_material(c, out.material_index).user_material_id == (100 as long)) + // From the far side, the other one. + out = compound.ray_cast_compound(c, math.vec3(15.0, 0.0, 0.0), math.vec3(0.0 - 20.0, 0.0, 0.0), 1.0) + ensure("ray from the far side", out.hit && out.child_index == 1 && compound.get_compound_material(c, out.material_index).user_material_id == (200 as long)) + small("ray from the far side fraction", out.fraction - 0.2, 0.0001) + // The shape cast: a sphere of radius 0.25 stops at x = 3.75. + out = compound.shape_cast_compound(c, point_proxy(point_block, math.vec3_zero(), 0.25), math.vec3(20.0, 0.0, 0.0), 1.0, false) + ensure("shape cast nearest hit", out.hit) + small("shape cast nearest fraction", out.fraction - 3.75 / 20.0, 0.001) + ensure("shape cast nearest child", out.child_index == 0) + compound.destroy_compound(c) + free_defs(d) + + // A unit box turned a quarter about z at x 5: the normal comes back in the compound's frame. + box = hull.make_box_hull(1.0, 1.0, 1.0) + d = make_defs(0, 1, 0, 0) + hulls = d.hulls as CompoundHullDef[] + hulls[0] = compound.compound_hull_def(box, Transform { p: math.vec3(5.0, 0.0, 0.0), q: math.make_quat_from_axis_angle(math.vec3_axis_z(), 0.5 * math.PI) }, mat) + c = compound.create_compound(&d.def) + out = compound.ray_cast_compound(c, math.vec3_zero(), math.vec3(20.0, 0.0, 0.0), 1.0) + ensure("turned hull ray hit", out.hit) + small("turned hull ray fraction", out.fraction - 0.2, 0.0001) + small("turned hull ray normal x", out.normal.x + 1.0, 0.001) + small("turned hull ray normal y", out.normal.y, 0.001) + small("turned hull ray normal z", out.normal.z, 0.001) + out = compound.shape_cast_compound(c, point_proxy(point_block, math.vec3_zero(), 0.25), math.vec3(20.0, 0.0, 0.0), 1.0, false) + ensure("turned hull shape cast hit", out.hit) + small("turned hull shape cast fraction", out.fraction - 3.75 / 20.0, 0.001) + small("turned hull shape cast normal x", out.normal.x + 1.0, 0.001) + small("turned hull shape cast normal y", out.normal.y, 0.001) + small("turned hull shape cast normal z", out.normal.z, 0.001) + small("turned hull shape cast point x", out.point.x - 4.0, 0.001) + small("turned hull shape cast point y", out.point.y, 0.001) + small("turned hull shape cast point z", out.point.z, 0.001) + ensure("turned hull shape cast child", out.child_index == 0) + compound.destroy_compound(c) + free_defs(d) + hull.destroy_hull(box) + + // A shape cast that misses, and an empty proxy. + d = make_defs(0, 0, 0, 1) + spheres = d.spheres as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3(5.0, 0.0, 0.0), 1.0), mat) + c = compound.create_compound(&d.def) + ensure("shape cast miss", compound.shape_cast_compound(c, point_proxy(point_block, math.vec3(0.0, 5.0, 0.0), 0.25), math.vec3(20.0, 0.0, 0.0), 1.0, false).hit == false) + ensure("empty proxy", compound.shape_cast_compound(c, distance.shape_proxy(point_block, 0, 0.25), math.vec3(20.0, 0.0, 0.0), 1.0, false).hit == false) + compound.destroy_compound(c) + free_defs(d) + + // A mesh of two triangles with a material each: the hit's material is remapped into the table. + vertices_block = calloc(6, sizeof(Vec3)) + vertices = vertices_block as Vec3[] + vertices[0] = math.vec3(0.0 - 3.0, 0.0, 0.0 - 1.0) + vertices[1] = math.vec3(0.0 - 2.0, 0.0, 1.0) + vertices[2] = math.vec3(0.0 - 1.0, 0.0, 0.0 - 1.0) + vertices[3] = math.vec3(1.0, 0.0, 0.0 - 1.0) + vertices[4] = math.vec3(2.0, 0.0, 1.0) + vertices[5] = math.vec3(3.0, 0.0, 0.0 - 1.0) + indices_block = calloc(6, 4) + indices = indices_block as int[] + i = 0 + while i < 6 { + indices[i] = i + i = i + 1 + } + material_indices_block = calloc(2, 4) + material_indices = material_indices_block as int[] + material_indices[1] = 1 + mdef = mesh.mesh_def() + mdef.vertices = vertices_block + mdef.indices = indices_block + mdef.material_indices = material_indices_block + mdef.vertex_count = 6 + mdef.triangle_count = 2 + md = mesh.create_mesh(&mdef, null, 0) + ensure("two-material mesh", md.material_count == 2) + mats_block = calloc(2, sizeof(SurfaceMaterial)) + mat_array = mats_block as SurfaceMaterial[] + mat_array[0] = make_material(0.3, 100) + mat_array[1] = make_material(0.7, 200) + d = make_defs(0, 0, 1, 0) + meshes = d.meshes as CompoundMeshDef[] + meshes[0] = compound.compound_mesh_def(md, math.transform_identity(), math.vec3_one(), mats_block, 2) + c = compound.create_compound(&d.def) + ensure("remap table", c.material_count == 2) + out = compound.ray_cast_compound(c, math.vec3(0.0 - 2.0, 5.0, 0.0), math.vec3(0.0, 0.0 - 10.0, 0.0), 1.0) + ensure("remap ray a", out.hit && compound.get_compound_material(c, out.material_index).user_material_id == (100 as long)) + out = compound.ray_cast_compound(c, math.vec3(2.0, 5.0, 0.0), math.vec3(0.0, 0.0 - 10.0, 0.0), 1.0) + ensure("remap ray b", out.hit && compound.get_compound_material(c, out.material_index).user_material_id == (200 as long)) + out = compound.shape_cast_compound(c, point_proxy(point_block, math.vec3(2.0, 5.0, 0.0), 0.1), math.vec3(0.0, 0.0 - 10.0, 0.0), 1.0, false) + ensure("remap shape cast", out.hit && compound.get_compound_material(c, out.material_index).user_material_id == (200 as long)) + // The mover's plane on the second triangle carries the same material. + planes_block = calloc(8, sizeof(PlaneResult)) + planes = planes_block as PlaneResult[] + count = compound.collide_mover_and_compound(planes, 8, c, manifold.capsule(math.vec3(2.0, 0.2, 0.0), math.vec3(2.0, 1.2, 0.0), 0.3)) + ensure("remap mover (${count})", count == 1) + if count == 1 { ensure("remap mover material", compound.get_compound_material(c, planes[0].material_index).user_material_id == (200 as long)) } + free(planes_block) + compound.destroy_compound(c) + free_defs(d) + free(mats_block) + mesh.destroy_mesh(md) + free(material_indices_block) + free(indices_block) + free(vertices_block) + free(point_block) +} + +test_overlap() { + mat = material.default_surface_material() + point_block = calloc(2, sizeof(Vec3)) + points = point_block as Vec3[] + d = make_defs(0, 0, 0, 2) + spheres = d.spheres as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3(0.0 - 3.0, 0.0, 0.0), 0.5), mat) + spheres[1] = compound.compound_sphere_def(manifold.sphere(math.vec3(3.0, 0.0, 0.0), 0.5), mat) + c = compound.create_compound(&d.def) + ensure("gap is clear", compound.overlap_compound(c, math.transform_identity(), point_proxy(point_block, math.vec3_zero(), 0.25)) == false) + ensure("second sphere overlaps", compound.overlap_compound(c, math.transform_identity(), point_proxy(point_block, math.vec3(3.0, 0.0, 0.0), 0.1))) + // Under a transform the proxy arrives in world space. + t = Transform { p: math.vec3(10.0, 20.0, 30.0), q: math.make_quat_from_axis_angle(math.vec3_axis_z(), 0.5 * math.PI) } + ensure("moved gap is clear", compound.overlap_compound(c, t, point_proxy(point_block, math.transform_point(t, math.vec3_zero()), 0.25)) == false) + ensure("moved sphere overlaps", compound.overlap_compound(c, t, point_proxy(point_block, math.transform_point(t, math.vec3(3.0, 0.0, 0.0)), 0.1))) + ensure("a stale local point misses", compound.overlap_compound(c, t, point_proxy(point_block, math.vec3(3.0, 0.0, 0.0), 0.1)) == false) + // A segment proxy, point by point. + t = Transform { p: math.vec3(0.0 - 40.0, 15.0, 7.0), q: math.make_quat_from_axis_angle(math.vec3_axis_y(), 0.5 * math.PI) } + points[0] = math.transform_point(t, math.vec3(0.0 - 1.0, 0.0, 0.0)) + points[1] = math.transform_point(t, math.vec3(1.0, 0.0, 0.0)) + ensure("segment in the gap", compound.overlap_compound(c, t, distance.shape_proxy(point_block, 2, 0.25)) == false) + points[1] = math.transform_point(t, math.vec3(2.9, 0.0, 0.0)) + ensure("segment reaching the sphere", compound.overlap_compound(c, t, distance.shape_proxy(point_block, 2, 0.25))) + compound.destroy_compound(c) + free_defs(d) + + // One child of every kind, far apart, under a transform. + box = hull.make_box_hull(0.5, 0.5, 0.5) + md = mesh.create_box_mesh(math.vec3_zero(), math.vec3(0.5, 0.5, 0.5), false) + mats_block = calloc(1, sizeof(SurfaceMaterial)) + mat_array = mats_block as SurfaceMaterial[] + mat_array[0] = mat + d = make_defs(1, 1, 1, 1) + capsules = d.capsules as CompoundCapsuleDef[] + capsules[0] = compound.compound_capsule_def(manifold.capsule(math.vec3(0.0 - 10.0, 0.0, 0.0), math.vec3(0.0 - 9.0, 0.0, 0.0), 0.25), mat) + hulls = d.hulls as CompoundHullDef[] + hulls[0] = compound.compound_hull_def(box, Transform { p: math.vec3_zero(), q: math.make_quat_from_axis_angle(math.vec3_axis_z(), 0.25 * math.PI) }, mat) + meshes = d.meshes as CompoundMeshDef[] + meshes[0] = compound.compound_mesh_def(md, Transform { p: math.vec3(10.0, 0.0, 0.0), q: math.make_quat_from_axis_angle(math.vec3_axis_y(), 0.5 * math.PI) }, math.vec3_one(), mats_block, 1) + spheres = d.spheres as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3(20.0, 0.0, 0.0), 0.5), mat) + c = compound.create_compound(&d.def) + t = Transform { p: math.vec3(100.0, 200.0, 300.0), q: math.make_quat_from_axis_angle(math.vec3_axis_z(), 0.5 * math.PI) } + ensure("capsule child overlaps", compound.overlap_compound(c, t, point_proxy(point_block, math.transform_point(t, math.vec3(0.0 - 9.5, 0.0, 0.0)), 0.1))) + ensure("hull child overlaps", compound.overlap_compound(c, t, point_proxy(point_block, math.transform_point(t, math.vec3_zero()), 0.1))) + ensure("mesh child overlaps", compound.overlap_compound(c, t, point_proxy(point_block, math.transform_point(t, math.vec3(10.0, 0.45, 0.0)), 0.1))) + ensure("sphere child overlaps", compound.overlap_compound(c, t, point_proxy(point_block, math.transform_point(t, math.vec3(20.0, 0.0, 0.0)), 0.1))) + ensure("gap between hull and mesh", compound.overlap_compound(c, t, point_proxy(point_block, math.transform_point(t, math.vec3(5.0, 0.0, 0.0)), 0.1)) == false) + compound.destroy_compound(c) + free_defs(d) + free(mats_block) + mesh.destroy_mesh(md) + hull.destroy_hull(box) + free(point_block) +} + +struct Accumulator { + indices: ptr // int[8] + count: int + stop_after: int // -1 never +} + +accumulate(c: *CompoundData, child_index: int, context: ptr) -> bool { + acc = context as *Accumulator + indices = acc.indices as int[] + if acc.count < 8 { + indices[acc.count] = child_index + acc.count = acc.count + 1 + } + if acc.stop_after >= 0 && acc.count >= acc.stop_after { return false } + return true +} + +test_query_and_mover() { + mat = material.default_surface_material() + d = make_defs(0, 0, 0, 3) + spheres = d.spheres as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3(0.0 - 10.0, 0.0, 0.0), 0.5), mat) + spheres[1] = compound.compound_sphere_def(manifold.sphere(math.vec3_zero(), 0.5), mat) + spheres[2] = compound.compound_sphere_def(manifold.sphere(math.vec3(10.0, 0.0, 0.0), 0.5), mat) + c = compound.create_compound(&d.def) + indices_block = calloc(8, 4) + indices = indices_block as int[] + acc = Accumulator { indices: indices_block, count: 0, stop_after: 0 - 1 } + compound.query_compound(c, AABB { lower: math.vec3(0.0 - 1.0, 0.0 - 1.0, 0.0 - 1.0), upper: math.vec3(1.0, 1.0, 1.0) }, accumulate, (&acc) as ptr) + ensure("query the middle", acc.count == 1 && indices[0] == 1) + acc = Accumulator { indices: indices_block, count: 0, stop_after: 1 } + wide = AABB { lower: math.vec3(0.0 - 20.0, 0.0 - 1.0, 0.0 - 1.0), upper: math.vec3(20.0, 1.0, 1.0) } + compound.query_compound(c, wide, accumulate, (&acc) as ptr) + ensure("query stops early", acc.count == 1) + acc = Accumulator { indices: indices_block, count: 0, stop_after: 0 - 1 } + compound.query_compound(c, wide, accumulate, (&acc) as ptr) + ensure("query all three", acc.count == 3) + ensure("query each once", indices[0] + indices[1] + indices[2] == 3 && indices[0] != indices[1] && indices[1] != indices[2] && indices[0] != indices[2]) + compound.destroy_compound(c) + free_defs(d) + + // Two boxes side by side with a gap; a capsule lying across both + // gets a plane from each pointing up. + box = hull.make_box_hull(0.5, 0.5, 0.5) + d = make_defs(0, 2, 0, 0) + hulls = d.hulls as CompoundHullDef[] + hulls[0] = compound.compound_hull_def(box, Transform { p: math.vec3(0.0 - 1.0, 0.0, 0.0), q: math.quat_identity() }, mat) + hulls[1] = compound.compound_hull_def(box, Transform { p: math.vec3(1.0, 0.0, 0.0), q: math.quat_identity() }, mat) + c = compound.create_compound(&d.def) + mover = manifold.capsule(math.vec3(0.0 - 1.0, 0.6, 0.0), math.vec3(1.0, 0.6, 0.0), 0.2) + planes_block = calloc(8, sizeof(PlaneResult)) + planes = planes_block as PlaneResult[] + count = compound.collide_mover_and_compound(planes, 8, c, mover) + ensure("mover two planes (${count})", count >= 2) + up = 0 + i = 0 + while i < count { + if planes[i].plane.normal.y > 0.9 { up = up + 1 } + i = i + 1 + } + ensure("mover planes point up", up >= 2) + ensure("mover children", count == 2 && planes[0].child_index != planes[1].child_index) + ensure("mover capacity", compound.collide_mover_and_compound(planes, 1, c, mover) <= 1) + ensure("mover no capacity", compound.collide_mover_and_compound(planes, 0, c, mover) == 0) + compound.destroy_compound(c) + free_defs(d) + + // A child turned a quarter about z: its plane comes back in the compound's frame. + d = make_defs(0, 1, 0, 0) + hulls = d.hulls as CompoundHullDef[] + hulls[0] = compound.compound_hull_def(box, Transform { p: math.vec3_zero(), q: math.make_quat_from_axis_angle(math.vec3_axis_z(), 0.5 * math.PI) }, make_material(0.25, 77)) + c = compound.create_compound(&d.def) + mover = manifold.capsule(math.vec3(0.0 - 0.1, 0.6, 0.0), math.vec3(0.1, 0.6, 0.0), 0.2) + count = compound.collide_mover_and_compound(planes, 8, c, mover) + ensure("turned child plane", count >= 1) + if count >= 1 { + small("turned child normal x", planes[0].plane.normal.x, 0.001) + small("turned child normal y", planes[0].plane.normal.y - 1.0, 0.001) + small("turned child normal z", planes[0].plane.normal.z, 0.001) + small("turned child point", planes[0].point.y - 0.5, 0.001) + ensure("turned child index", planes[0].child_index == 0) + ensure("turned child material", compound.get_compound_material(c, planes[0].material_index).user_material_id == (77 as long)) + } + compound.destroy_compound(c) + free_defs(d) + free(planes_block) + free(indices_block) + hull.destroy_hull(box) +} + +test_through_shape() { + // The compound as a Shape under a transform: every query goes through the dispatch. + _mat = material.default_surface_material() + box = hull.make_box_hull(1.0, 1.0, 1.0) + d = make_defs(0, 1, 0, 1) + hulls = d.hulls as CompoundHullDef[] + hulls[0] = compound.compound_hull_def(box, Transform { p: math.vec3(5.0, 0.0, 0.0), q: math.quat_identity() }, make_material(0.3, 5)) + spheres = d.spheres as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3(0.0 - 5.0, 0.0, 0.0), 1.0), make_material(0.4, 6)) + c = compound.create_compound(&d.def) + sh = shape.compound_shape(c) + ensure("compound shape kind", sh.kind == shape.SHAPE_COMPOUND && sh.material_count == 2) + t = Transform { p: math.vec3(10.0, 20.0, 30.0), q: math.make_quat_from_axis_angle(math.vec3_axis_z(), 0.5 * math.PI) } + // Local x is world y: the hull sits at world (10, 25, 30), the sphere at (10, 15, 30). + bounds = shape.compute_shape_aabb(&sh, t) + small("compound bounds upper y", bounds.upper.y - 26.0, 0.0001) + small("compound bounds lower y", bounds.lower.y - 14.0, 0.0001) + small("compound centroid", math.length(shape.get_shape_centroid(&sh)), 0.0001) + extent = shape.compute_shape_extent(&sh, math.vec3_zero()) + small("compound extent", extent.max_extent.x - 6.0, 0.0001) + ensure("compound has no mass", shape.compute_shape_mass(&sh).mass == 0.0) + // A world ray down onto the hull's top (world y 26) from above. + out = shape.ray_cast_shape(&sh, t, math.vec3(10.0, 30.0, 30.0), math.vec3(0.0, 0.0 - 8.0, 0.0), 1.0) + ensure("compound ray hit", out.hit) + small("compound ray fraction", out.fraction - 0.5, 0.0001) + small("compound ray normal", out.normal.y - 1.0, 0.0001) + ensure("compound ray child", out.child_index == 0) + ensure("compound ray material", compound.get_compound_material(c, out.material_index).user_material_id == (5 as long)) + // A world ray up onto the sphere's underside (world y 14). + out = shape.ray_cast_shape(&sh, t, math.vec3(10.0, 10.0, 30.0), math.vec3(0.0, 8.0, 0.0), 1.0) + ensure("compound ray sphere", out.hit && out.child_index == 1) + small("compound ray sphere fraction", out.fraction - 0.5, 0.0001) + ensure("compound ray sphere material", compound.get_compound_material(c, out.material_index).user_material_id == (6 as long)) + // A shape cast of a small sphere down onto the hull. + point_block = calloc(1, sizeof(Vec3)) + out = shape.shape_cast_shape(&sh, t, point_proxy(point_block, math.vec3(10.0, 30.0, 30.0), 0.5), math.vec3(0.0, 0.0 - 8.0, 0.0), 1.0, false) + ensure("compound shape cast hit", out.hit && out.child_index == 0) + small("compound shape cast fraction", out.fraction - 3.5 / 8.0, 0.001) + small("compound shape cast normal", out.normal.y - 1.0, 0.001) + // The overlap. + ensure("compound overlap", shape.overlap_shape(&sh, t, point_proxy(point_block, math.vec3(10.0, 26.4, 30.0), 0.5))) + ensure("compound clear", shape.overlap_shape(&sh, t, point_proxy(point_block, math.vec3(10.0, 26.6, 30.0), 0.5)) == false) + // The mover standing on the hull. + planes_block = calloc(8, sizeof(PlaneResult)) + planes = planes_block as PlaneResult[] + mover = manifold.capsule(math.vec3(10.0, 26.2, 30.0), math.vec3(10.0, 27.2, 30.0), 0.3) + count = shape.collide_mover(planes, 8, &sh, t, mover) + ensure("compound mover (${count})", count == 1) + if count == 1 { + small("compound mover normal", planes[0].plane.normal.y - 1.0, 0.0001) + small("compound mover offset", planes[0].plane.offset - 0.1, 0.0001) + ensure("compound mover child and material", planes[0].child_index == 0 && planes[0].material_index == 0) + } + free(planes_block) + free(point_block) + compound.destroy_compound(c) + free_defs(d) + hull.destroy_hull(box) +} + +var field_visits = 0 + +count_child(c: *CompoundData, child_index: int, context: ptr) -> bool { + field_visits = field_visits + 1 + return true +} + +test_field() { + // A thousand spheres on a grid: the tree finds the few in a box, the + // ray the nearest, the cast through them all. + mat = material.default_surface_material() + d = make_defs(0, 0, 0, 1000) + spheres = d.spheres as CompoundSphereDef[] + i = 0 + while i < 1000 { + x = (i % 10) as float + y = ((i / 10) % 10) as float + z = (i / 100) as float + spheres[i] = compound.compound_sphere_def(manifold.sphere(math.vec3(2.0 * x, 2.0 * y, 2.0 * z), 0.5), mat) + i = i + 1 + } + c = compound.create_compound(&d.def) + ensure("field created", c != null && c.sphere_count == 1000 && c.tree.proxy_count == 1000) + ensure("field tree valid", dynamic_tree.tree_validate(&c.tree)) + ensure("field tree height", dynamic_tree.tree_get_height(&c.tree) <= 16) + field_visits = 0 + // Centres 4 and 6 on each axis: two per axis, eight spheres. + compound.query_compound(c, AABB { lower: math.vec3(3.0, 3.0, 3.0), upper: math.vec3(7.0, 7.0, 7.0) }, count_child, null) + ensure("field query (${field_visits})", field_visits == 8) + out = compound.ray_cast_compound(c, math.vec3(0.0 - 5.0, 4.0, 4.0), math.vec3(30.0, 0.0, 0.0), 1.0) + ensure("field ray", out.hit && out.child_index == 0 * 1 + 2 * 10 + 2 * 100) + small("field ray fraction", out.fraction - 4.5 / 30.0, 0.0001) + out = compound.ray_cast_compound(c, math.vec3(25.0, 4.0, 4.0), math.vec3(0.0 - 30.0, 0.0, 0.0), 1.0) + ensure("field ray back", out.hit && out.child_index == 9 + 2 * 10 + 2 * 100) + point_block = calloc(1, sizeof(Vec3)) + out = compound.shape_cast_compound(c, point_proxy(point_block, math.vec3(4.0, 30.0, 4.0), 0.25), math.vec3(0.0, 0.0 - 40.0, 0.0), 1.0, false) + ensure("field shape cast", out.hit && out.child_index == 2 + 9 * 10 + 2 * 100) + small("field shape cast fraction", out.fraction - (30.0 - 18.75) / 40.0, 0.001) + ensure("field overlap", compound.overlap_compound(c, math.transform_identity(), point_proxy(point_block, math.vec3(18.0, 18.0, 18.0), 0.1))) + ensure("field clear", compound.overlap_compound(c, math.transform_identity(), point_proxy(point_block, math.vec3(1.0, 1.0, 1.0), 0.1)) == false) + free(point_block) + compound.destroy_compound(c) + free_defs(d) +} + +main() { + before = core.alloc_count() + test_create() + test_materials() + test_sharing() + test_children_and_bounds() + test_casts() + test_overlap() + test_query_and_mover() + test_through_shape() + test_field() + // The scratch stays allocated: compound's three blocks, mesh's three, sphere's two, shape's one. + ensure("every other counted allocation was freed (${core.alloc_count() - before})", core.alloc_count() == before + 9) + + println("compound: ${checks} checks") + if failures == 0 { + println("compound: all checks passed") + } else { + println("compound: ${failures} failure(s)") + exit(1) + } +} diff --git a/aephysics/test_shape.ae b/aephysics/test_shape.ae index 3ef824f..365ebf0 100644 --- a/aephysics/test_shape.ae +++ b/aephysics/test_shape.ae @@ -18,6 +18,10 @@ import aephysics.distance import aephysics.manifold import aephysics.mesh import aephysics.height_field +import aephysics.material +import aephysics.sphere +import aephysics.capsule +import aephysics.compound import aephysics.shape extern calloc(count: int, size: int) -> ptr @@ -87,7 +91,7 @@ check_inertia_equal(name: string, a: Matrix3, b: Matrix3, tol: float) { 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) + md = sphere.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) @@ -157,7 +161,7 @@ test_mass() { c = test_capsule() radius = c.radius length = math.distance(c.center1, c.center2) - md = shape.compute_capsule_mass(c, 1.0) + md = capsule.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 @@ -210,7 +214,7 @@ test_mass() { // 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) + md = capsule.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) @@ -219,7 +223,7 @@ test_mass() { 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) + mx = capsule.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) @@ -234,14 +238,14 @@ test_mass() { } test_aabb() { - b = shape.compute_sphere_aabb(test_sphere(), math.transform_identity()) + b = sphere.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()) + b = capsule.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) @@ -312,54 +316,54 @@ test_ray_cast_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) + check_hit("sphere -x", sphere.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) + check_hit("sphere +y", sphere.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) + check_hit("sphere -z", sphere.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) + check_hit("sphere offset", sphere.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) + check_hit("sphere diagonal", sphere.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) + ensure("sphere away", sphere.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", sphere.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", sphere.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 clipped", sphere.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 = sphere.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) + out = sphere.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) + out = sphere.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) + ensure("sphere zero ray outside", sphere.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) + ensure("sphere graze hits", sphere.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", sphere.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) + out = sphere.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) + out = sphere.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) @@ -370,57 +374,57 @@ test_ray_cast_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) + check_hit("capsule side y", capsule.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) + check_hit("capsule side z", capsule.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) + check_hit("capsule side near c1", capsule.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) + check_hit("capsule oblique", capsule.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) + check_hit("capsule cap collinear", capsule.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) + check_hit("capsule cap c2", capsule.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) + check_hit("capsule cap c1", capsule.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) + ensure("capsule away", capsule.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", capsule.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", capsule.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", capsule.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", capsule.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) + out = capsule.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) + out = capsule.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) + out = capsule.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) + ensure("capsule zero ray outside", capsule.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) + check_hit("capsule degenerate", capsule.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 clipped", capsule.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 = capsule.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) @@ -428,18 +432,18 @@ test_ray_cast_capsule() { 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) + out = capsule.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) + out = capsule.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) + ensure("exactly parallel outside misses", capsule.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() { @@ -447,14 +451,14 @@ test_overlap_convention() { 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) + check_initial_overlap("sphere moving", sphere.ray_cast_sphere(s, inside, ray, 1.0), inside) + check_initial_overlap("sphere point", sphere.ray_cast_sphere(s, inside, zero, 1.0), inside) + ensure("sphere point outside", sphere.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) + check_initial_overlap("capsule moving", capsule.ray_cast_capsule(c, inside, ray, 1.0), inside) + check_initial_overlap("capsule point", capsule.ray_cast_capsule(c, inside, zero, 1.0), inside) + ensure("capsule point outside", capsule.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) @@ -523,8 +527,8 @@ test_far_origin() { 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) + os = sphere.ray_cast_sphere(s, origin, translation, 1.0) + oc = capsule.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 @@ -558,13 +562,13 @@ 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) + out = sphere.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) + out = capsule.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) diff --git a/bench/RESULTS.md b/bench/RESULTS.md index 5c6ccdd..647b0d0 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -274,3 +274,35 @@ 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#9). + +## compound + +`bench/compound.ae` and `bench/compound_box3d.c`: a compound of 2,000 +children (1,000 spheres, 500 capsules and 500 instances of one box hull +on a 20 x 10 x 10 grid, two materials) built ten times; 100,000 rays +through it; 100,000 box queries over it; 10,000 sphere shape casts down +through it; 10,000 overlaps and 10,000 mover planes under a transform. + +| phase | aephysics | Box3D | +|---|---|---| +| 10 builds (2,000 children) | **9.9 ms** | 14.3 | +| 100,000 ray casts | 75.4 | **25.7** | +| 100,000 box queries | 8.7 | **6.5** | +| 10,000 shape casts | 10.7 | **5.0** | +| 10,000 overlaps | 1.5 | **0.9** | +| 10,000 mover planes | 1.2 | **0.9** | + +The same answers: 50,450 ray hits with equal sums of fraction and child +index, 777,892 query visits, 5,098 cast hits, 3,278 overlaps, 5,090 +mover planes with equal offset sums. The build is faster here (0.7x; +the content maps are core's LongMap on the hull's and the mesh's own +hash, the reference rehashes every block through a generic table); the +queries, overlaps and movers 1.3-1.6x; the shape cast 2.1x (issue #9's +radius GJK). The ray cast is 2.9x: a probe with a visitor that does +nothing shows the time in `dynamic_tree.tree_ray_cast` itself (77 ms +for 520,000 leaf visits), not in the children -- the tree layer showed +the same cast at 1.9x on a sparser scene, the reference's SIMD slab +test against scalar doubles. The traversal is the place to profile +(aephysics#10). The compound is 379 KB here against 231 KB there: the +tree's nodes and proxies are our wider doubles and longs, and the +block carries the traversal stack. diff --git a/bench/compound.ae b/bench/compound.ae new file mode 100644 index 0000000..3aa9b8d --- /dev/null +++ b/bench/compound.ae @@ -0,0 +1,166 @@ +// The compound on the same scenes as bench/compound_box3d.c: a compound +// of 2,000 children (1,000 spheres, 500 capsules and 500 instances of +// one box hull on a 20 x 10 x 10 grid, two materials) built ten times; +// 100,000 rays through it; 100,000 box queries over it; 10,000 sphere +// shape casts; 10,000 overlaps; 10,000 mover planes. 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.hull +import aephysics.distance +import aephysics.manifold +import aephysics.mesh +import aephysics.material +import aephysics.compound + +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 = 100000 +const QUERIES = 100000 +const CASTS = 10000 +const OVERLAPS = 10000 +const MOVERS = 10000 + +var query_hits = 0 + +count_child(c: *CompoundData, child_index: int, context: ptr) -> bool { + query_hits = query_hits + 1 + return true +} + +main() { + mat_a = material.default_surface_material() + mat_b = material.default_surface_material() + mat_b.friction = 0.3 + box = hull.make_box_hull(0.4, 0.4, 0.4) + spheres_block = calloc(1000, sizeof(CompoundSphereDef)) + spheres = spheres_block as CompoundSphereDef[] + capsules_block = calloc(500, sizeof(CompoundCapsuleDef)) + capsules = capsules_block as CompoundCapsuleDef[] + hulls_block = calloc(500, sizeof(CompoundHullDef)) + hulls = hulls_block as CompoundHullDef[] + i = 0 + while i < 2000 { + c = math.vec3((2 * (i % 20)) as float, (2 * ((i / 20) % 10)) as float, (2 * (i / 200)) as float) + mat = mat_a + if (i & 1) != 0 { mat = mat_b } + if i < 1000 { + spheres[i] = compound.compound_sphere_def(manifold.sphere(c, 0.5), mat) + } else if i < 1500 { + capsules[i - 1000] = compound.compound_capsule_def(manifold.capsule(math.vec3(c.x - 0.4, c.y, c.z), math.vec3(c.x + 0.4, c.y, c.z), 0.3), mat) + } else { + hulls[i - 1500] = compound.compound_hull_def(box, Transform { p: c, q: math.make_quat_from_axis_angle(math.vec3_axis_y(), 0.3) }, mat) + } + i = i + 1 + } + def = compound.compound_def() + def.capsules = capsules_block + def.capsule_count = 500 + def.hulls = hulls_block + def.hull_count = 500 + def.spheres = spheres_block + def.sphere_count = 1000 + + t0 = clock() + data = compound.create_compound(&def) + i = 1 + while i < 10 { + compound.destroy_compound(data) + data = compound.create_compound(&def) + i = i + 1 + } + t1 = clock() + + ray_hits = 0 + ray_sum = 0.0 + i = 0 + while i < RAYS { + t = (i as float) / (RAYS as float) + origin = math.vec3(0.0 - 3.0, 1.0 + 17.0 * t, 9.0 + 8.0 * sin(40.0 * t)) + translation = math.vec3(45.0, 2.0 * sin(7.0 * t), 2.0 * cos(9.0 * t)) + out = compound.ray_cast_compound(data, origin, translation, 1.0) + if out.hit { + ray_hits = ray_hits + 1 + ray_sum = ray_sum + out.fraction + (out.child_index as float) + } + i = i + 1 + } + t2 = clock() + + query_hits = 0 + i = 0 + while i < QUERIES { + t = (i as float) / (QUERIES as float) + c = math.vec3(1.0 + 37.0 * t, 9.0 + 8.0 * sin(3.0 * t), 9.0 + 8.0 * cos(30.0 * t)) + h = math.vec3(1.5, 1.5, 1.5) + bounds = AABB { lower: math.sub(c, h), upper: math.add(c, h) } + compound.query_compound(data, bounds, count_child, null) + i = i + 1 + } + t3 = clock() + + cast_hits = 0 + cast_sum = 0.0 + start_block = calloc(1, sizeof(Vec3)) + start = start_block as Vec3[] + i = 0 + while i < CASTS { + t = (i as float) / (CASTS as float) + start[0] = math.vec3(1.0 + 37.0 * t, 25.0, 9.0 + 8.0 * sin(50.0 * t)) + out = compound.shape_cast_compound(data, distance.shape_proxy(start_block, 1, 0.3), math.vec3(0.5 * sin(11.0 * t), 0.0 - 30.0, 0.0), 1.0, false) + if out.hit { + cast_hits = cast_hits + 1 + cast_sum = cast_sum + out.fraction + (out.child_index as float) + } + i = i + 1 + } + t4 = clock() + + transform = Transform { p: math.vec3(10.0, 20.0, 30.0), q: math.make_quat_from_axis_angle(math.vec3_axis_z(), 0.5 * math.PI) } + overlap_hits = 0 + i = 0 + while i < OVERLAPS { + t = (i as float) / (OVERLAPS as float) + local = math.vec3(1.0 + 37.0 * t, 9.0 + 8.0 * sin(17.0 * t), 9.0 + 8.0 * cos(23.0 * t)) + start[0] = math.transform_point(transform, local) + if compound.overlap_compound(data, transform, distance.shape_proxy(start_block, 1, 0.4)) { overlap_hits = overlap_hits + 1 } + i = i + 1 + } + t5 = clock() + + planes_block = calloc(8, 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 = math.vec3(1.0 + 37.0 * t, 9.5 + 8.0 * sin(17.0 * t), 9.0 + 8.0 * cos(23.0 * t)) + mover = manifold.capsule(a, math.vec3(a.x, a.y + 1.0, a.z), 0.35) + count = compound.collide_mover_and_compound(planes, 8, data, mover) + mover_planes = mover_planes + count + k = 0 + while k < count { + mover_sum = mover_sum + planes[k].plane.offset + k = k + 1 + } + i = i + 1 + } + t6 = clock() + + println("aephysics compound: 10 builds ${ms(t1 - t0)} ms (${data.tree.proxy_count} children, ${data.byte_count} bytes, ${data.material_count} materials), ${RAYS} rays ${ms(t2 - t1)} ms (${ray_hits} hits, sum ${ray_sum}), ${QUERIES} queries ${ms(t3 - t2)} ms (${query_hits} hits), ${CASTS} casts ${ms(t4 - t3)} ms (${cast_hits} hits, sum ${cast_sum}), ${OVERLAPS} overlaps ${ms(t5 - t4)} ms (${overlap_hits} hits), ${MOVERS} movers ${ms(t6 - t5)} ms (${mover_planes} planes, sum ${mover_sum})") + free(planes_block) + free(start_block) + compound.destroy_compound(data) + free(hulls_block) + free(capsules_block) + free(spheres_block) + hull.destroy_hull(box) +} diff --git a/bench/compound_box3d.c b/bench/compound_box3d.c new file mode 100644 index 0000000..d2de14f --- /dev/null +++ b/bench/compound_box3d.c @@ -0,0 +1,159 @@ +// The compound of the reference on the same scenes as bench/compound.ae: +// a compound of 2,000 children (1,000 spheres, 500 capsules and 500 +// instances of one box hull on a 20 x 10 x 10 grid, two materials) +// built ten times; 100,000 rays through it; 100,000 box queries over +// it; 10,000 sphere shape casts; 10,000 overlaps; 10,000 mover planes. +// 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" + +#include +#include +#include +#include + +// Internal to the reference (compound.h in src) but linked from its library. +int b3CollideMoverAndCompound( b3PlaneResult* planes, int capacity, const b3CompoundData* shape, const b3Capsule* mover ); + +static double now_ms( void ) +{ + struct timespec ts; + timespec_get( &ts, TIME_UTC ); + return ts.tv_sec * 1000.0 + ts.tv_nsec / 1.0e6; +} + +static int g_queryHits; +static bool count_child( const b3CompoundData* compound, int childIndex, void* context ) +{ + (void)compound; (void)childIndex; (void)context; + g_queryHits += 1; + return true; +} + +#define RAYS 100000 +#define QUERIES 100000 +#define CASTS 10000 +#define OVERLAPS 10000 +#define MOVERS 10000 + +int main( void ) +{ + b3SurfaceMaterial matA = b3DefaultSurfaceMaterial(); + b3SurfaceMaterial matB = b3DefaultSurfaceMaterial(); + matB.friction = 0.3f; + b3BoxHull box = b3MakeBoxHull( 0.4f, 0.4f, 0.4f ); + + b3CompoundSphereDef* spheres = calloc( 1000, sizeof( b3CompoundSphereDef ) ); + b3CompoundCapsuleDef* capsules = calloc( 500, sizeof( b3CompoundCapsuleDef ) ); + b3CompoundHullDef* hulls = calloc( 500, sizeof( b3CompoundHullDef ) ); + for ( int i = 0; i < 2000; ++i ) + { + int x = i % 20, y = ( i / 20 ) % 10, z = i / 200; + b3Vec3 c = { 2.0f * x, 2.0f * y, 2.0f * z }; + b3SurfaceMaterial mat = ( i & 1 ) ? matB : matA; + if ( i < 1000 ) + { + spheres[i] = (b3CompoundSphereDef){ .sphere = { c, 0.5f }, .material = mat }; + } + else if ( i < 1500 ) + { + capsules[i - 1000] = (b3CompoundCapsuleDef){ .capsule = { { c.x - 0.4f, c.y, c.z }, { c.x + 0.4f, c.y, c.z }, 0.3f }, .material = mat }; + } + else + { + hulls[i - 1500] = (b3CompoundHullDef){ .hull = &box.base, .transform = { c, b3MakeQuatFromAxisAngle( b3Vec3_axisY, 0.3f ) }, .material = mat }; + } + } + b3CompoundDef def = { .capsules = capsules, .capsuleCount = 500, .hulls = hulls, .hullCount = 500, .spheres = spheres, .sphereCount = 1000 }; + + double t0 = now_ms(); + b3CompoundData* compound = NULL; + for ( int i = 0; i < 10; ++i ) + { + if ( compound ) b3DestroyCompound( compound ); + compound = b3CreateCompound( &def ); + } + double t1 = now_ms(); + + int rayHits = 0; + double raySum = 0.0; + for ( int i = 0; i < RAYS; ++i ) + { + float t = (float)i / (float)RAYS; + b3Vec3 origin = { -3.0f, 1.0f + 17.0f * t, 9.0f + 8.0f * sinf( 40.0f * t ) }; + b3Vec3 translation = { 45.0f, 2.0f * sinf( 7.0f * t ), 2.0f * cosf( 9.0f * t ) }; + b3RayCastInput input = { origin, translation, 1.0f }; + b3CastOutput out = b3RayCastCompound( compound, &input ); + if ( out.hit ) + { + rayHits += 1; + raySum += out.fraction + out.childIndex; + } + } + double t2 = now_ms(); + + g_queryHits = 0; + for ( int i = 0; i < QUERIES; ++i ) + { + float t = (float)i / (float)QUERIES; + b3Vec3 c = { 1.0f + 37.0f * t, 9.0f + 8.0f * sinf( 3.0f * t ), 9.0f + 8.0f * cosf( 30.0f * t ) }; + b3Vec3 h = { 1.5f, 1.5f, 1.5f }; + b3AABB bounds = { b3Sub( c, h ), b3Add( c, h ) }; + b3QueryCompound( compound, bounds, count_child, NULL ); + } + double t3 = now_ms(); + + int castHits = 0; + double castSum = 0.0; + for ( int i = 0; i < CASTS; ++i ) + { + float t = (float)i / (float)CASTS; + b3Vec3 start = { 1.0f + 37.0f * t, 25.0f, 9.0f + 8.0f * sinf( 50.0f * t ) }; + b3ShapeCastInput input = { { &start, 1, 0.3f }, { 0.5f * sinf( 11.0f * t ), -30.0f, 0.0f }, 1.0f, false }; + b3CastOutput out = b3ShapeCastCompound( compound, &input ); + if ( out.hit ) + { + castHits += 1; + castSum += out.fraction + out.childIndex; + } + } + double t4 = now_ms(); + + b3Transform transform = { { 10.0f, 20.0f, 30.0f }, b3MakeQuatFromAxisAngle( b3Vec3_axisZ, 0.5f * B3_PI ) }; + int overlapHits = 0; + for ( int i = 0; i < OVERLAPS; ++i ) + { + float t = (float)i / (float)OVERLAPS; + b3Vec3 local = { 1.0f + 37.0f * t, 9.0f + 8.0f * sinf( 17.0f * t ), 9.0f + 8.0f * cosf( 23.0f * t ) }; + b3Vec3 center = b3TransformPoint( transform, local ); + b3ShapeProxy proxy = { ¢er, 1, 0.4f }; + if ( b3OverlapCompound( compound, transform, &proxy ) ) overlapHits += 1; + } + double t5 = now_ms(); + + int moverPlanes = 0; + double moverSum = 0.0; + for ( int i = 0; i < MOVERS; ++i ) + { + float t = (float)i / (float)MOVERS; + b3Vec3 a = { 1.0f + 37.0f * t, 9.5f + 8.0f * sinf( 17.0f * t ), 9.0f + 8.0f * cosf( 23.0f * t ) }; + b3Capsule mover = { a, { a.x, a.y + 1.0f, a.z }, 0.35f }; + b3PlaneResult planes[8]; + int count = b3CollideMoverAndCompound( planes, 8, compound, &mover ); + moverPlanes += count; + for ( int k = 0; k < count; ++k ) moverSum += planes[k].plane.offset; + } + double t6 = now_ms(); + + printf( "box3d compound: 10 builds %.1f ms (%d children, %d bytes, %d materials), %d rays %.2f ms (%d hits, sum %.3f), " + "%d queries %.2f ms (%d hits), %d casts %.2f ms (%d hits, sum %.3f), %d overlaps %.2f ms (%d hits), %d movers %.2f ms (%d planes, sum %.3f)\n", + t1 - t0, compound->tree.proxyCount, compound->byteCount, compound->materialCount, RAYS, t2 - t1, rayHits, raySum, QUERIES, t3 - t2, + g_queryHits, CASTS, t4 - t3, castHits, castSum, OVERLAPS, t5 - t4, overlapHits, MOVERS, t6 - t5, moverPlanes, moverSum ); + b3DestroyCompound( compound ); + free( hulls ); + free( capsules ); + free( spheres ); + return 0; +} diff --git a/design.md b/design.md index 1a14112..f894d61 100644 --- a/design.md +++ b/design.md @@ -105,9 +105,23 @@ started until its tests pass. 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 - static tree, materials and hulls shared by content) with - test_compound.c, and the compound branch of the shape dispatch. +11. **compound** (done): compound.c as `aephysics.compound`: the + children's bounds into a tree rebuilt in full and carried in the + block with its traversal stack, materials deduplicated field by field + through core's LongMap, hulls and meshes by their own hash and bytes, + a mesh child's four material slots remapped; overlap, ray and shape + casts, the box query and the mover through the tree, each child in + its own frame. 159 checks: test_compound.c's creation, materials, + sharing, child order, bounds, casts, remap, overlap, query and mover + subtests (the byte roundtrip is not ported), plus the compound + through the shape dispatch and a field of a thousand spheres. The + same results as the reference; the build 0.7x, the queries 1.3-1.6x, + the ray cast 2.9x with the time in the tree's traversal itself + (aephysics#10). To make the dispatch reach the compound without a + cycle, sphere.c and capsule.c became `aephysics.sphere` and + `aephysics.capsule`, the material `aephysics.material`, and the + hull's mover moved beside the mesh's; `aephysics.shape` imports them + all. `tree_validate` accepts a baked tree (no parents). 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 From 09eba1dbcbe1c854db8edf097ea377059dff4d91 Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Sat, 19 Sep 2026 22:41:33 -0300 Subject: [PATCH 2/2] compound: point the ray cast note at issue #11 --- 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 647b0d0..05430fe 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -303,6 +303,6 @@ nothing shows the time in `dynamic_tree.tree_ray_cast` itself (77 ms for 520,000 leaf visits), not in the children -- the tree layer showed the same cast at 1.9x on a sparser scene, the reference's SIMD slab test against scalar doubles. The traversal is the place to profile -(aephysics#10). The compound is 379 KB here against 231 KB there: the +(aephysics#11). The compound is 379 KB here against 231 KB there: the tree's nodes and proxies are our wider doubles and longs, and the block carries the traversal stack. diff --git a/design.md b/design.md index f894d61..e425f67 100644 --- a/design.md +++ b/design.md @@ -117,7 +117,7 @@ started until its tests pass. through the shape dispatch and a field of a thousand spheres. The same results as the reference; the build 0.7x, the queries 1.3-1.6x, the ray cast 2.9x with the time in the tree's traversal itself - (aephysics#10). To make the dispatch reach the compound without a + (aephysics#11). To make the dispatch reach the compound without a cycle, sphere.c and capsule.c became `aephysics.sphere` and `aephysics.capsule`, the material `aephysics.material`, and the hull's mover moved beside the mesh's; `aephysics.shape` imports them