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..05430fe 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#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/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..e425f67 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#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 + 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