From b34450b8e258c630cb2fb8395ec78c9eff4a4df8 Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Sat, 19 Sep 2026 16:47:21 -0300 Subject: [PATCH] aephysics.hull: quickhull and the half-edge hull Box3D's hull.c in Aether: the quickhull builder with per-face conflict lists and cached farthest points, the iterative horizon walk, the cone from the apex, the merging of flipped, concave and coplanar cone faces, and the packing into one block with the arrays at offsets. The builder's pointers are indices into its pools, with the intrusive lists chained through prev/next and their sentinels in extra slots. Half-edge indices are ints; the SOA mirrors for the SIMD support function are not kept; the box hull is a heap block destroyed like any hull; the hash is this engine's own. Also: the tessellated cylinder, cone and rock, clone and clone-and-transform (with the winding reversed under a mirroring scale), the analytic box hull with scale_box, support vertex and face, mass, bounds, the ray cast against the planes, extent, projected area, and the 2D hull (monotone chain) with weld and simplification. math gains the plane helpers, rotate_inertia, max_element_index, aabb_add_point and LINEAR_SLOP; core gains lsr32. test_hull.ae carries test_hull.c: 438 checks at the reference's tolerances, including the 512-point sphere stress at three caps and the 4,096-point merge churn. bench/hull.ae against bench/hull_box3d.c: the same hulls (6,554 vertices, 12,106 faces) at 1.6-2x the reference's time, recorded in bench/RESULTS.md. --- README.md | 3 +- aephysics/core/module.ae | 5 +- aephysics/hull/module.ae | 2082 ++++++++++++++++++++++++++++++++++++++ aephysics/math/module.ae | 64 +- aephysics/test_hull.ae | 731 +++++++++++++ bench/RESULTS.md | 21 + bench/hull.ae | 98 ++ bench/hull_box3d.c | 88 ++ design.md | 16 +- 9 files changed, 3100 insertions(+), 8 deletions(-) create mode 100644 aephysics/hull/module.ae create mode 100644 aephysics/test_hull.ae create mode 100644 bench/hull.ae create mode 100644 bench/hull_box3d.c diff --git a/README.md b/README.md index 44fd8e8..5e6b311 100644 --- a/README.md +++ b/README.md @@ -28,7 +28,8 @@ so a test written against the reference reads the same here. | `aephysics.math` | vectors, quaternions, transforms, 3x3 matrices, bounding boxes, segment distances, inertia helpers, the deterministic atan2/cos/sin | done, `test_math.ae` (6M checks) | | `aephysics.core` | bit set, id pool, hash set, arrays, the stack and arena allocators | done, `test_core.ae` (100k checks) | | `aephysics.dynamic_tree` | the bounding volume hierarchy under the broad phase: SAH insertion, rotations, enlarge, sweep refit, partial rebuild in depth-first order, box / closest / ray / swept-box queries | done, `test_dynamic_tree.ae` (12k checks); [same tree as the reference, ray cast 1.9x its time](bench/RESULTS.md#dynamic_tree) | -| `aephysics.collision` | hull (quickhull), GJK distance and shape cast, contact manifolds, triangle mesh, height field, shapes with mass properties, ray and shape casts | next | +| `aephysics.hull` | quickhull with face merging, the half-edge hull with its mass properties, box / cylinder / cone / rock hulls, clone-and-transform with mirroring, support functions, ray cast, the 2D hull | done, `test_hull.ae` (438 checks); [same hulls as the reference, 1.6-2x its time](bench/RESULTS.md#hull) | +| `aephysics.collision` | GJK distance and shape cast, contact manifolds, triangle mesh, height field, shapes with mass properties, ray and shape casts | next | | `aephysics.dynamics` | bodies, contacts, the constraint graph, islands, the Soft Step solver, joints (spherical, revolute, prismatic, distance, motor, weld, wheel), sensors, the character mover, the world | | | `aephysics` | the public API | | diff --git a/aephysics/core/module.ae b/aephysics/core/module.ae index 2b25801..12ff1fc 100644 --- a/aephysics/core/module.ae +++ b/aephysics/core/module.ae @@ -23,7 +23,7 @@ exports ( BitSet, IdPool, HashSet, IntArray, Buffer, Stack, Arena, alloc, free_bytes, alloc_count, bytes_in_use, is_power_of_2, bounding_power_of_2, round_up_power_of_2, lower_power_of_2_exponent, - clz32, ctz32, ctz64, pop_count64, lsr, + clz32, ctz32, ctz64, pop_count64, lsr, lsr32, bitset_create, bitset_destroy, bitset_set_count_and_clear, bitset_grow, bitset_union, bitset_set, bitset_set_grow, bitset_clear, bitset_get, bitset_count, bitset_bytes, idpool_create, idpool_destroy, idpool_alloc, idpool_free, idpool_count, idpool_capacity, @@ -125,6 +125,9 @@ pop_count64(block: long) -> int { // A logical shift right of a long by 1..63: what an unsigned shift does, // where the language's shift is arithmetic. +// The logical shift right of an int, for 0 < n < 32. +lsr32(x: int, n: int) -> int { return (x >> n) & ((1 << (32 - n)) - 1) } + lsr(value: long, shift: int) -> long { if shift <= 0 { return value } if shift >= 64 { return 0 } diff --git a/aephysics/hull/module.ae b/aephysics/hull/module.ae new file mode 100644 index 0000000..72668f7 --- /dev/null +++ b/aephysics/hull/module.ae @@ -0,0 +1,2082 @@ +// aephysics.hull -- convex hulls: quickhull over a point cloud, the +// half-edge hull that comes out, its mass properties, the box hull, the +// tessellated cylinder, cone and rock, ray casts and support functions +// on a hull, and the 2D hull the contact reduction uses. +// +// The shape is Box3D's hull.c (Erin Catto, with portions by Dirk +// Gregorius, MIT), the reference this engine is measured against: the +// builder keeps every face's conflict list with its farthest point +// cached, walks the horizon iteratively, builds a cone from the apex, +// merges the cone faces that came out flipped, concave or coplanar, and +// packs the result as one block with the arrays hanging off the end. +// Names are the reference's without its prefix, in snake case: +// b3CreateHull is create_hull. +// +// Differences: the builder's pointers are indices into its pools, with +// the intrusive lists chained through prev/next indices and their +// sentinels in extra slots; the hull's half-edge indices are ints, not +// bytes (Aether has no byte type); the structure-of-arrays mirrors for +// the SIMD support function are not kept (the wide path is native, later); +// the box hull is a heap block like any other hull and is destroyed like +// one; and the hull hash is this engine's own, not the reference's. +import std.string +import aephysics.math +import aephysics.core + +exports ( + HullData, HalfEdge, MassData, CastOutput, ShapeExtent, Point2D, + MAX_HULL_VERTICES, MAX_HULL_FACES, MAX_HULL_EDGES, HULL_VERSION, NULL_INDEX, + hull_vertices, hull_points, hull_edges, hull_planes, hull_faces, + create_hull, destroy_hull, clone_hull, clone_and_transform_hull, is_valid_hull, + hash_hull_data, compare_hull_data, + create_cylinder, create_cone, create_rock, + make_box_hull, make_cube_hull, make_offset_box_hull, make_transformed_box_hull, + make_scaled_box_hull, scale_box, + find_hull_support_vertex, find_hull_support_face, + compute_hull_mass, compute_hull_aabb, compute_swept_hull_aabb, ray_cast_hull, + compute_hull_extent, compute_hull_projected_area, + hull_2d, simplify_hull_2d +) + +extern memcpy(dst: ptr, src: ptr, size: int) -> ptr +extern memmove(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 +extern sqrt(x: float) -> float + +const NULL_INDEX = 0 - 1 +const MAX_HULL_VERTICES = 128 +const MAX_HULL_FACES = 128 +const MAX_HULL_EDGES = 128 +// The final hull's counts are capped at 256 as in the reference, whose indices are bytes. +const HULL_MAX_COUNT = 256 +const HULL_VERSION = 0x4A4C9587 + +const MARK_VISIBLE = 0 +const MARK_DELETE = 1 + +// A half-edge of a hull: the next edge counter-clockwise around the face, +// the twin across the edge, the origin vertex, the face to the left. +struct HalfEdge { + next: int + twin: int + origin: int + face: int +} + +// A convex hull: one block, with the arrays at byte offsets from its +// start. vertices are the index of one half-edge leaving each vertex; +// faces the index of one half-edge on each face. Cannot be copied by +// value: clone it. +struct HullData { + version: int + hash: long + aabb: AABB + surface_area: float + volume: float + inner_radius: float + center: Vec3 + central_inertia: Matrix3 + vertex_count: int + vertex_offset: int + point_offset: int + edge_count: int + edge_offset: int + face_count: int + plane_offset: int + face_offset: int + byte_count: int +} + +struct MassData { + mass: float + center: Vec3 + inertia: Matrix3 +} + +// A ray or shape cast's result. +struct CastOutput { + normal: Vec3 + point: Vec3 + fraction: float + iterations: int + triangle_index: int + hit: bool +} + +// The smallest sphere at the origin inside the shape and the largest box around it. +struct ShapeExtent { + min_extent: float + max_extent: Vec3 +} + +// A point of the 2D hull, with where it came from. +struct Point2D { + p: Vec2 + separation: float + original_index: int +} + +// --- the hull's arrays -------------------------------------------------------- + +hull_vertices(h: *HullData) -> int[] { return ((h as ptr) + h.vertex_offset) as int[] } +hull_points(h: *HullData) -> Vec3[] { return ((h as ptr) + h.point_offset) as Vec3[] } +hull_edges(h: *HullData) -> HalfEdge[] { return ((h as ptr) + h.edge_offset) as HalfEdge[] } +hull_planes(h: *HullData) -> Plane[] { return ((h as ptr) + h.plane_offset) as Plane[] } +hull_faces(h: *HullData) -> int[] { return ((h as ptr) + h.face_offset) as int[] } + +align8(x: int) -> int { return (x + 7) & (0 - 8) } + +// --- the builder --------------------------------------------------------------- + +struct QHVertex { + prev: int // intrusive list link, NULL_INDEX when not in a list + next: int + conflict_face: int + position: Vec3 + final_index: int + reachable: bool +} + +struct QHHalfEdge { + prev: int // the ring around the owning face + next: int + origin: int + face: int + twin: int + final_index: int +} + +struct QHFace { + prev: int // intrusive list link + next: int + edge: int + mark: int + area: float + plane: Plane + centroid: Vec3 + max_conflict_distance: float + max_conflict: int // the farthest conflict above min_outside, or NULL_INDEX + final_index: int + flipped: bool +} + +// One frame of the iterative horizon walk: four ints. +const FRAME_INTS = 4 + +// All working memory for one build. Vertices, edges and faces are pools; +// the vertex list, the orphaned list and every face's conflict list are +// chained through the vertices' prev/next with a sentinel slot each, +// past the pool: vertex_list_head, orphaned_head, then one per face. +struct HullBuilder { + tolerance: float + min_radius: float + min_outside: float + interior_point: Vec3 + + vertices: ptr // QHVertex[]: vertex_capacity + 2 + face_capacity + vertex_capacity: int + vertex_count: int + vertex_list_head: int + orphaned_head: int + conflict_head_base: int + + edges: ptr // QHHalfEdge[] + edge_capacity: int + edge_count: int + edge_free_head: int // LIFO through next + + faces: ptr // QHFace[]: face_capacity + 1 + face_capacity: int + face_count: int + face_free_head: int // LIFO through next + face_list_head: int + + horizon: ptr // int[] of edges + horizon_capacity: int + horizon_count: int + cone: ptr // int[] of faces + cone_capacity: int + cone_count: int + merged_faces: ptr // int[] of faces + merged_capacity: int + merged_count: int + horizon_stack: ptr // int[]: FRAME_INTS per frame + horizon_stack_capacity: int + + final_vertex_count: int + final_half_edge_count: int + final_face_count: int +} + +conflict_head(b: *HullBuilder, face: int) -> int { return b.conflict_head_base + face } + +// The vertex lists: doubly linked through the vertices, a sentinel per list. +vlist_init(vertices: QHVertex[], head: int) { + vertices[head].prev = head + vertices[head].next = head +} + +vlist_contains(vertices: QHVertex[], node: int) -> bool { + return vertices[node].prev != NULL_INDEX && vertices[node].next != NULL_INDEX +} + +// Insert node before `where`. +vlist_insert(vertices: QHVertex[], node: int, where: int) { + before = vertices[where].prev + vertices[node].prev = before + vertices[node].next = where + vertices[before].next = node + vertices[where].prev = node +} + +vlist_remove(vertices: QHVertex[], node: int) { + before = vertices[node].prev + after = vertices[node].next + vertices[before].next = after + vertices[after].prev = before + vertices[node].prev = NULL_INDEX + vertices[node].next = NULL_INDEX +} + +vlist_push_back(vertices: QHVertex[], head: int, node: int) { vlist_insert(vertices, node, head) } +vlist_empty(vertices: QHVertex[], head: int) -> bool { return vertices[head].next == head } + +// The face list, the same way through the faces. +flist_init(faces: QHFace[], head: int) { + faces[head].prev = head + faces[head].next = head +} + +flist_contains(faces: QHFace[], node: int) -> bool { + return faces[node].prev != NULL_INDEX && faces[node].next != NULL_INDEX +} + +flist_push_back(faces: QHFace[], head: int, node: int) { + before = faces[head].prev + faces[node].prev = before + faces[node].next = head + faces[before].next = node + faces[head].prev = node +} + +flist_remove(faces: QHFace[], node: int) { + before = faces[node].prev + after = faces[node].next + faces[before].next = after + faces[after].prev = before + faces[node].prev = NULL_INDEX + faces[node].next = NULL_INDEX +} + +new_vertex(b: *HullBuilder, position: Vec3) -> int { + vertices = b.vertices as QHVertex[] + v = b.vertex_count + b.vertex_count = v + 1 + vertices[v] = QHVertex { prev: NULL_INDEX, next: NULL_INDEX, conflict_face: NULL_INDEX, + position: position, final_index: NULL_INDEX, reachable: false } + return v +} + +new_edge(b: *HullBuilder) -> int { + edges = b.edges as QHHalfEdge[] + e = b.edge_free_head + if e != NULL_INDEX { + b.edge_free_head = edges[e].next + } else { + e = b.edge_count + b.edge_count = e + 1 + } + // The rest is written by new_face right after. + edges[e].final_index = NULL_INDEX + return e +} + +retire_edge(b: *HullBuilder, e: int) { + edges = b.edges as QHHalfEdge[] + edges[e].next = b.edge_free_head + b.edge_free_head = e +} + +new_face(b: *HullBuilder, v1: int, v2: int, v3: int) -> int { + faces = b.faces as QHFace[] + f = b.face_free_head + if f != NULL_INDEX { + // next was the free-list pointer; recover the head before it is clobbered. + b.face_free_head = faces[f].next + } else { + f = b.face_count + b.face_count = f + 1 + } + faces[f].prev = NULL_INDEX + faces[f].next = NULL_INDEX + faces[f].max_conflict = NULL_INDEX + faces[f].max_conflict_distance = 0.0 + faces[f].final_index = NULL_INDEX + + edge1 = new_edge(b) + edge2 = new_edge(b) + edge3 = new_edge(b) + + vertices = b.vertices as QHVertex[] + p1 = vertices[v1].position + p2 = vertices[v2].position + p3 = vertices[v3].position + + n = math.cross(math.sub(p2, p1), math.sub(p3, p1)) + length = math.length(n) + normal = math.vec3_zero() + if length > 0.0 { normal = math.mul_sv(1.0 / length, n) } + plane = Plane { normal: normal, offset: math.dot(normal, p1) } + area = 0.5 * length + + faces[f].edge = edge1 + faces[f].mark = MARK_VISIBLE + faces[f].area = area + faces[f].centroid = math.mul_sv(1.0 / 3.0, math.add(p1, math.add(p2, p3))) + faces[f].plane = plane + faces[f].flipped = math.plane_separation(plane, b.interior_point) > 0.0 + vlist_init(vertices, conflict_head(b, f)) + + edges = b.edges as QHHalfEdge[] + edges[edge1] = QHHalfEdge { prev: edge3, next: edge2, origin: v1, face: f, twin: NULL_INDEX, final_index: NULL_INDEX } + edges[edge2] = QHHalfEdge { prev: edge1, next: edge3, origin: v2, face: f, twin: NULL_INDEX, final_index: NULL_INDEX } + edges[edge3] = QHHalfEdge { prev: edge2, next: edge1, origin: v3, face: f, twin: NULL_INDEX, final_index: NULL_INDEX } + return f +} + +// Remove the face from the list and put it on the free list. +retire_face(b: *HullBuilder, f: int) { + faces = b.faces as QHFace[] + // A cone face can be merged before it was ever added to the list. + if flist_contains(faces, f) { flist_remove(faces, f) } + faces[f].edge = NULL_INDEX + faces[f].next = b.face_free_head + b.face_free_head = f +} + +build_bounds(count: int, points: Vec3[]) -> AABB { + bounds = math.aabb_empty() + i = 0 + while i < count { + bounds = math.aabb_add_point(bounds, points[i]) + i = i + 1 + } + return bounds +} + +// The two points farthest apart along the cardinal axis they spread most +// on: out[0] and out[1], or NULL_INDEX when the spread is within tolerance. +find_farthest_points_along_cardinal_axes(out: int[], tolerance: float, count: int, points: Vec3[]) { + out[0] = NULL_INDEX + out[1] = NULL_INDEX + v0 = points[0] + min_x = v0.x + max_x = v0.x + min_y = v0.y + max_y = v0.y + min_z = v0.z + max_z = v0.z + min_ix = 0 + max_ix = 0 + min_iy = 0 + max_iy = 0 + min_iz = 0 + max_iz = 0 + i = 1 + while i < count { + v = points[i] + if v.x < min_x { + min_x = v.x + min_ix = i + } else if v.x > max_x { + max_x = v.x + max_ix = i + } + if v.y < min_y { + min_y = v.y + min_iy = i + } else if v.y > max_y { + max_y = v.y + max_iy = i + } + if v.z < min_z { + min_z = v.z + min_iz = i + } else if v.z > max_z { + max_z = v.z + max_iz = i + } + i = i + 1 + } + d = math.vec3(max_x - min_x, max_y - min_y, max_z - min_z) + axis = math.max_element_index(d) + spread = d.z + first = min_iz + second = max_iz + if axis == 0 { + spread = d.x + first = min_ix + second = max_ix + } else if axis == 1 { + spread = d.y + first = min_iy + second = max_iy + } + if spread > 2.0 * tolerance { + out[0] = first + out[1] = second + } +} + +find_farthest_point_from_line(index1: int, index2: int, tolerance: float, count: int, points: Vec3[]) -> int { + a = points[index1] + ab = math.sub(points[index2], a) + ab_length_sqr = math.dot(ab, ab) + inv = 1.0 / ab_length_sqr + max_distance_sqr = 4.0 * tolerance * tolerance + max_index = NULL_INDEX + i = 0 + while i < count { + if i != index1 && i != index2 { + ap = math.sub(points[i], a) + c = math.cross(ap, ab) + distance_sqr = math.dot(c, c) * inv + if distance_sqr > max_distance_sqr { + max_distance_sqr = distance_sqr + max_index = i + } + } + i = i + 1 + } + return max_index +} + +find_farthest_point_from_plane(index1: int, index2: int, index3: int, tolerance: float, count: int, points: Vec3[]) -> int { + plane = math.make_plane_from_points(points[index1], points[index2], points[index3]) + max_distance = 2.0 * tolerance + max_index = NULL_INDEX + i = 0 + while i < count { + if i != index1 && i != index2 && i != index3 { + distance = math.abs_float(math.plane_separation(plane, points[i])) + if distance > max_distance { + max_distance = distance + max_index = i + } + } + i = i + 1 + } + return max_index +} + +is_edge_convex(b: *HullBuilder, e: int, tolerance: float) -> bool { + edges = b.edges as QHHalfEdge[] + faces = b.faces as QHFace[] + distance = math.plane_separation(faces[edges[e].face].plane, faces[edges[edges[e].twin].face].centroid) + return distance < 0.0 - tolerance +} + +is_edge_concave(b: *HullBuilder, e: int, tolerance: float) -> bool { + edges = b.edges as QHHalfEdge[] + faces = b.faces as QHFace[] + distance = math.plane_separation(faces[edges[e].face].plane, faces[edges[edges[e].twin].face].centroid) + return distance > tolerance +} + +vertex_count_of_face(b: *HullBuilder, f: int) -> int { + edges = b.edges as QHHalfEdge[] + faces = b.faces as QHFace[] + count = 0 + start = faces[f].edge + e = start + while true { + count = count + 1 + e = edges[e].next + if e == start { break } + } + return count +} + +// The index-th edge around the face. +face_edge_at(b: *HullBuilder, f: int, index: int) -> int { + edges = b.edges as QHHalfEdge[] + faces = b.faces as QHFace[] + e = faces[f].edge + while index > 0 { + e = edges[e].next + index = index - 1 + } + return e +} + +link_face(b: *HullBuilder, f: int, index: int, twin: int) { + edges = b.edges as QHHalfEdge[] + e = face_edge_at(b, f, index) + edges[e].twin = twin + edges[twin].twin = e +} + +link_faces(b: *HullBuilder, f1: int, index1: int, f2: int, index2: int) { + edges = b.edges as QHHalfEdge[] + e1 = face_edge_at(b, f1, index1) + e2 = face_edge_at(b, f2, index2) + edges[e1].twin = e2 + edges[e2].twin = e1 +} + +// Newell's plane of a polygon face, its centroid and area. +newell_plane(b: *HullBuilder, f: int) { + edges = b.edges as QHHalfEdge[] + faces = b.faces as QHFace[] + vertices = b.vertices as QHVertex[] + count = 0 + centroid = math.vec3_zero() + nx = 0.0 + ny = 0.0 + nz = 0.0 + start = faces[f].edge + // The first vertex is the origin, to reduce round-off. + origin = vertices[edges[start].origin].position + e = start + while true { + twin = edges[e].twin + v1 = math.sub(vertices[edges[e].origin].position, origin) + v2 = math.sub(vertices[edges[twin].origin].position, origin) + count = count + 1 + centroid = math.add(centroid, v1) + nx = nx + (v1.y - v2.y) * (v1.z + v2.z) + ny = ny + (v1.z - v2.z) * (v1.x + v2.x) + nz = nz + (v1.x - v2.x) * (v1.y + v2.y) + e = edges[e].next + if e == start { break } + } + centroid = math.add(math.mul_sv(1.0 / (count as float), centroid), origin) + normal = math.vec3(nx, ny, nz) + length = math.length(normal) + normal = math.mul_sv(1.0 / length, normal) + faces[f].centroid = centroid + faces[f].plane = math.make_plane_from_normal_and_point(normal, centroid) + faces[f].area = 0.5 * length +} + +compute_tolerance(b: *HullBuilder, count: int, points: Vec3[]) { + bounds = build_bounds(count, points) + max_abs = math.max_vec3(math.abs_vec3(bounds.lower), math.abs_vec3(bounds.upper)) + max_sum = max_abs.x + max_abs.y + max_abs.z + max_coord = math.max_element(max_abs) + max_distance = math.min_float(math.SQRT3 * max_coord, max_sum) + // The reference's tolerance is in its float's epsilon; the hull's + // decisions (what is coplanar, what is outside) keep that scale. + b.tolerance = (3.0 * max_distance * 1.01 + max_coord) * FLOAT_EPSILON + b.min_radius = 4.0 * b.tolerance + b.min_outside = 2.0 * b.min_radius +} + +// The reference's single-precision epsilon, which its tolerances are in. +const FLOAT_EPSILON = 0.00000011920929 + +build_initial_hull(b: *HullBuilder, count: int, points: Vec3[]) -> bool { + pair_block = core.alloc(8) + pair = pair_block as int[] + find_farthest_points_along_cardinal_axes(pair, b.tolerance, count, points) + index1 = pair[0] + index2 = pair[1] + core.free_bytes(pair_block, 8) + if index1 < 0 || index2 < 0 { return false } + index3 = find_farthest_point_from_line(index1, index2, b.tolerance, count, points) + if index3 < 0 { return false } + index4 = find_farthest_point_from_plane(index1, index2, index3, b.tolerance, count, points) + if index4 < 0 { return false } + + v1 = math.sub(points[index1], points[index4]) + v2 = math.sub(points[index2], points[index4]) + v3 = math.sub(points[index3], points[index4]) + if math.scalar_triple_product(v1, v2, v3) < 0.0 { + swap = index2 + index2 = index3 + index3 = swap + } + + b.interior_point = math.mul_sv(0.25, math.add(math.add(points[index1], points[index2]), math.add(points[index3], points[index4]))) + + vertices = b.vertices as QHVertex[] + faces = b.faces as QHFace[] + vertex1 = new_vertex(b, points[index1]) + vlist_push_back(vertices, b.vertex_list_head, vertex1) + vertex2 = new_vertex(b, points[index2]) + vlist_push_back(vertices, b.vertex_list_head, vertex2) + vertex3 = new_vertex(b, points[index3]) + vlist_push_back(vertices, b.vertex_list_head, vertex3) + vertex4 = new_vertex(b, points[index4]) + vlist_push_back(vertices, b.vertex_list_head, vertex4) + + face1 = new_face(b, vertex1, vertex2, vertex3) + flist_push_back(faces, b.face_list_head, face1) + face2 = new_face(b, vertex4, vertex2, vertex1) + flist_push_back(faces, b.face_list_head, face2) + face3 = new_face(b, vertex4, vertex3, vertex2) + flist_push_back(faces, b.face_list_head, face3) + face4 = new_face(b, vertex4, vertex1, vertex3) + flist_push_back(faces, b.face_list_head, face4) + + link_faces(b, face1, 0, face2, 1) + link_faces(b, face1, 1, face3, 1) + link_faces(b, face1, 2, face4, 1) + link_faces(b, face2, 0, face3, 2) + link_faces(b, face3, 0, face4, 2) + link_faces(b, face4, 0, face2, 2) + + // Every other point goes on the conflict list of the face it is farthest above. + index = 0 + while index < count { + if index != index1 && index != index2 && index != index3 && index != index4 { + point = points[index] + max_distance = b.min_outside + max_face = NULL_INDEX + f = faces[b.face_list_head].next + while f != b.face_list_head { + distance = math.plane_separation(faces[f].plane, point) + if distance > max_distance { + max_distance = distance + max_face = f + } + f = faces[f].next + } + if max_face != NULL_INDEX { + v = new_vertex(b, point) + vertices[v].conflict_face = max_face + vlist_push_back(vertices, conflict_head(b, max_face), v) + if max_distance > faces[max_face].max_conflict_distance { + faces[max_face].max_conflict_distance = max_distance + faces[max_face].max_conflict = v + } + } + } + index = index + 1 + } + return true +} + +// Recompute a face's farthest conflict after its plane changed; bounded +// by its own list. +recache_conflicts(b: *HullBuilder, f: int) { + vertices = b.vertices as QHVertex[] + faces = b.faces as QHFace[] + max_vertex = NULL_INDEX + max_distance = b.min_outside + head = conflict_head(b, f) + v = vertices[head].next + while v != head { + distance = math.plane_separation(faces[f].plane, vertices[v].position) + if distance > max_distance { + max_distance = distance + max_vertex = v + } + v = vertices[v].next + } + faces[f].max_conflict = max_vertex + faces[f].max_conflict_distance = max_distance +} + +next_conflict_vertex(b: *HullBuilder) -> int { + faces = b.faces as QHFace[] + max_vertex = NULL_INDEX + max_distance = b.min_outside + f = faces[b.face_list_head].next + while f != b.face_list_head { + if faces[f].max_conflict != NULL_INDEX && faces[f].max_conflict_distance > max_distance { + max_distance = faces[f].max_conflict_distance + max_vertex = faces[f].max_conflict + } + f = faces[f].next + } + return max_vertex +} + +// Every conflict vertex of the face onto the orphaned list. +drain_conflict_list(b: *HullBuilder, f: int) { + vertices = b.vertices as QHVertex[] + head = conflict_head(b, f) + v = vertices[head].next + while v != head { + orphan = v + v = vertices[v].next + vertices[orphan].conflict_face = NULL_INDEX + vlist_remove(vertices, orphan) + vlist_push_back(vertices, b.orphaned_head, orphan) + } +} + +// Mark a face for deletion, drain its conflicts and open a frame for it +// on the horizon stack. entry_edge is the half-edge of the face whose +// twin is in the parent just deleted, or NULL_INDEX for the seed. +enter_horizon_face(b: *HullBuilder, f: int, entry_edge: int, frame: int) { + faces = b.faces as QHFace[] + edges = b.edges as QHHalfEdge[] + stack = b.horizon_stack as int[] + faces[f].mark = MARK_DELETE + drain_conflict_list(b, f) + base = frame * FRAME_INTS + stack[base] = f + stack[base + 3] = 0 + if entry_edge != NULL_INDEX { + stack[base + 1] = entry_edge + stack[base + 2] = edges[entry_edge].next + } else { + stack[base + 1] = faces[f].edge + stack[base + 2] = faces[f].edge + } +} + +// The horizon of the faces the apex sees, as the half-edges of the +// visible faces along it, by an explicit depth-first walk. +build_horizon(b: *HullBuilder, apex: int, seed: int) { + faces = b.faces as QHFace[] + edges = b.edges as QHHalfEdge[] + vertices = b.vertices as QHVertex[] + stack = b.horizon_stack as int[] + horizon = b.horizon as int[] + apex_position = vertices[apex].position + top = 0 + enter_horizon_face(b, seed, NULL_INDEX, top) + top = top + 1 + while top > 0 { + base = (top - 1) * FRAME_INTS + if stack[base + 3] != 0 && stack[base + 2] == stack[base + 1] { + top = top - 1 + continue + } + stack[base + 3] = 1 + e = stack[base + 2] + twin = edges[e].twin + stack[base + 2] = edges[e].next + twin_face = edges[twin].face + if faces[twin_face].mark != MARK_VISIBLE { continue } + distance = math.plane_separation(faces[twin_face].plane, apex_position) + if distance > b.min_radius { + enter_horizon_face(b, twin_face, twin, top) + top = top + 1 + } else { + horizon[b.horizon_count] = e + b.horizon_count = b.horizon_count + 1 + } + } +} + +// New faces from the apex to each horizon edge, linked to the outside +// and to each other. +build_cone(b: *HullBuilder, apex: int) { + edges = b.edges as QHHalfEdge[] + horizon = b.horizon as int[] + cone = b.cone as int[] + i = 0 + while i < b.horizon_count { + e = horizon[i] + f = new_face(b, apex, edges[e].origin, edges[edges[e].twin].origin) + cone[b.cone_count] = f + b.cone_count = b.cone_count + 1 + link_face(b, f, 1, edges[e].twin) + i = i + 1 + } + face1 = cone[b.cone_count - 1] + i = 0 + while i < b.cone_count { + face2 = cone[i] + link_faces(b, face1, 2, face2, 0) + face1 = face2 + i = i + 1 + } +} + +// Retire the half-edges in [begin, end) around a ring. +destroy_edges(b: *HullBuilder, begin: int, end: int) { + edges = b.edges as QHHalfEdge[] + e = begin + while e != end { + next = edges[e].next + retire_edge(b, e) + e = next + } +} + +connect_edges(b: *HullBuilder, prev: int, next: int) { + edges = b.edges as QHHalfEdge[] + faces = b.faces as QHFace[] + vertices = b.vertices as QHVertex[] + merged = b.merged_faces as int[] + prev_twin = edges[prev].twin + next_twin = edges[next].twin + if edges[prev_twin].face == edges[next_twin].face { + // The same face on both sides: next is redundant and the shared + // neighbour would be orphaned. + face = edges[next].face + if faces[face].edge == next { faces[face].edge = prev } + twin = NULL_INDEX + opposing = edges[prev_twin].face + if vertex_count_of_face(b, opposing) == 3 { + // The dead triangle's three half-edges, captured before the rewire. + dead0 = prev_twin + dead1 = next_twin + dead2 = edges[next_twin].prev + twin = edges[dead2].twin + faces[opposing].mark = MARK_DELETE + merged[b.merged_count] = opposing + b.merged_count = b.merged_count + 1 + + edges[prev].next = edges[next].next + edges[edges[prev].next].prev = prev + edges[prev].twin = twin + edges[twin].twin = prev + // The redundant vertex leaves the list; its slot is abandoned. + vlist_remove(vertices, edges[next].origin) + retire_edge(b, dead0) + retire_edge(b, dead1) + retire_edge(b, dead2) + } else { + twin = next_twin + twin_face = edges[twin].face + if faces[twin_face].edge == prev_twin { faces[twin_face].edge = twin } + edges[twin].next = edges[prev_twin].next + edges[edges[twin].next].prev = twin + retire_edge(b, prev_twin) + + edges[prev].next = edges[next].next + edges[edges[prev].next].prev = prev + edges[prev].twin = twin + edges[twin].twin = prev + vlist_remove(vertices, edges[next].origin) + } + // The twin's face changed shape. + newell_plane(b, edges[twin].face) + recache_conflicts(b, edges[twin].face) + } else { + edges[prev].next = next + edges[next].prev = prev + } +} + +// The conflicts of the merged faces go to the face that absorbed them, +// or to the orphaned list; the merged faces are retired. +absorb_faces(b: *HullBuilder, f: int) { + vertices = b.vertices as QHVertex[] + faces = b.faces as QHFace[] + merged = b.merged_faces as int[] + i = 0 + while i < b.merged_count { + head = conflict_head(b, merged[i]) + v = vertices[head].next + while v != head { + vertex = v + v = vertices[v].next + vlist_remove(vertices, vertex) + distance = math.plane_separation(faces[f].plane, vertices[vertex].position) + if distance > b.min_outside { + vlist_push_back(vertices, conflict_head(b, f), vertex) + vertices[vertex].conflict_face = f + if distance > faces[f].max_conflict_distance { + faces[f].max_conflict_distance = distance + faces[f].max_conflict = vertex + } + } else { + vlist_push_back(vertices, b.orphaned_head, vertex) + vertices[vertex].conflict_face = NULL_INDEX + } + } + retire_face(b, merged[i]) + i = i + 1 + } +} + +// Merge the face across the edge into the edge's face. +connect_faces(b: *HullBuilder, e: int) { + edges = b.edges as QHHalfEdge[] + faces = b.faces as QHFace[] + merged = b.merged_faces as int[] + face = edges[e].face + twin = edges[e].twin + twin_face = edges[twin].face + + edge_prev = edges[e].prev + edge_next = edges[e].next + twin_prev = edges[twin].prev + twin_next = edges[twin].next + + while edges[edges[edge_prev].twin].face == twin_face { + edge_prev = edges[edge_prev].prev + twin_next = edges[twin_next].next + } + while edges[edges[edge_next].twin].face == twin_face { + edge_next = edges[edge_next].next + twin_prev = edges[twin_prev].prev + } + + faces[face].edge = edge_prev + + // The opposing face goes; merged_faces is single-buffered, connect_faces does not nest. + b.merged_count = 0 + merged[0] = twin_face + b.merged_count = 1 + faces[twin_face].mark = MARK_DELETE + faces[twin_face].edge = NULL_INDEX + + absorbed = twin_next + stop = edges[twin_prev].next + while absorbed != stop { + edges[absorbed].face = face + absorbed = edges[absorbed].next + } + + destroy_edges(b, edges[edge_prev].next, edge_next) + destroy_edges(b, edges[twin_prev].next, twin_next) + + connect_edges(b, edge_prev, twin_next) + connect_edges(b, twin_prev, edge_next) + + newell_plane(b, face) + recache_conflicts(b, face) + absorb_faces(b, face) +} + +merge_concave(b: *HullBuilder, f: int) -> bool { + edges = b.edges as QHHalfEdge[] + faces = b.faces as QHFace[] + start = faces[f].edge + e = start + while true { + twin = edges[e].twin + if is_edge_concave(b, e, b.min_radius) || is_edge_concave(b, twin, b.min_radius) { + connect_faces(b, e) + return true + } + e = edges[e].next + if e == start { break } + } + return false +} + +merge_coplanar(b: *HullBuilder, f: int) -> bool { + edges = b.edges as QHHalfEdge[] + faces = b.faces as QHFace[] + start = faces[f].edge + e = start + while true { + twin = edges[e].twin + if is_edge_convex(b, e, b.min_radius) == false || is_edge_convex(b, twin, b.min_radius) == false { + connect_faces(b, e) + return true + } + e = edges[e].next + if e == start { break } + } + return false +} + +// The cone faces that came out flipped merge into their largest +// neighbour; then concave edges, then coplanar ones. +merge_faces(b: *HullBuilder) { + edges = b.edges as QHHalfEdge[] + faces = b.faces as QHFace[] + cone = b.cone as int[] + i = 0 + while i < b.cone_count { + f = cone[i] + if faces[f].mark == MARK_VISIBLE && faces[f].flipped { + faces[f].flipped = false + best_area = 0.0 + best_edge = NULL_INDEX + start = faces[f].edge + e = start + while true { + area = faces[edges[edges[e].twin].face].area + if area > best_area { + best_area = area + best_edge = e + } + e = edges[e].next + if e == start { break } + } + connect_faces(b, best_edge) + } + i = i + 1 + } + i = 0 + while i < b.cone_count { + f = cone[i] + if faces[f].mark == MARK_VISIBLE { + while merge_concave(b, f) { } + } + i = i + 1 + } + i = 0 + while i < b.cone_count { + f = cone[i] + if faces[f].mark == MARK_VISIBLE { + while merge_coplanar(b, f) { } + } + i = i + 1 + } +} + +// The orphaned vertices find the cone face they are farthest above, or +// are interior and abandoned. +resolve_vertices(b: *HullBuilder) { + vertices = b.vertices as QHVertex[] + faces = b.faces as QHFace[] + cone = b.cone as int[] + v = vertices[b.orphaned_head].next + while v != b.orphaned_head { + vertex = v + v = vertices[v].next + vlist_remove(vertices, vertex) + max_distance = b.min_outside + max_face = NULL_INDEX + i = 0 + while i < b.cone_count { + f = cone[i] + if faces[f].mark == MARK_VISIBLE { + distance = math.plane_separation(faces[f].plane, vertices[vertex].position) + if distance > max_distance { + max_distance = distance + max_face = f + } + } + i = i + 1 + } + if max_face != NULL_INDEX { + vlist_push_back(vertices, conflict_head(b, max_face), vertex) + vertices[vertex].conflict_face = max_face + if max_distance > faces[max_face].max_conflict_distance { + faces[max_face].max_conflict_distance = max_distance + faces[max_face].max_conflict = vertex + } + } + } +} + +// The deleted faces leave the list with their edges; the surviving cone +// faces join it. +resolve_faces(b: *HullBuilder) { + edges = b.edges as QHHalfEdge[] + faces = b.faces as QHFace[] + cone = b.cone as int[] + f = faces[b.face_list_head].next + while f != b.face_list_head { + face = f + f = faces[f].next + if faces[face].mark == MARK_DELETE && flist_contains(faces, face) { + // Each half-edge is owned by one face, so the rings of the dead + // region retire every interior edge once. + start = faces[face].edge + e = start + while true { + next = edges[e].next + retire_edge(b, e) + e = next + if e == start { break } + } + retire_face(b, face) + } + } + i = 0 + while i < b.cone_count { + if faces[cone[i]].mark != MARK_DELETE { flist_push_back(faces, b.face_list_head, cone[i]) } + i = i + 1 + } +} + +add_vertex_to_hull(b: *HullBuilder, vertex: int) { + vertices = b.vertices as QHVertex[] + f = vertices[vertex].conflict_face + vertices[vertex].conflict_face = NULL_INDEX + vlist_remove(vertices, vertex) + vlist_push_back(vertices, b.vertex_list_head, vertex) + b.horizon_count = 0 + build_horizon(b, vertex, f) + b.cone_count = 0 + build_cone(b, vertex) + merge_faces(b) + resolve_vertices(b) + resolve_faces(b) +} + +// Drop the vertices no face reaches, shift everything back by the origin +// and count what is left. +clean_hull(b: *HullBuilder, origin: Vec3) { + vertices = b.vertices as QHVertex[] + edges = b.edges as QHHalfEdge[] + faces = b.faces as QHFace[] + face_count = 0 + half_edge_count = 0 + f = faces[b.face_list_head].next + while f != b.face_list_head { + start = faces[f].edge + e = start + while true { + vertices[edges[e].origin].reachable = true + e = edges[e].next + half_edge_count = half_edge_count + 1 + if e == start { break } + } + faces[f].plane.offset = faces[f].plane.offset + math.dot(faces[f].plane.normal, origin) + faces[f].centroid = math.add(faces[f].centroid, origin) + face_count = face_count + 1 + f = faces[f].next + } + vertex_count = 0 + v = vertices[b.vertex_list_head].next + while v != b.vertex_list_head { + vertex = v + v = vertices[v].next + if vertices[vertex].reachable == false { + vlist_remove(vertices, vertex) + } else { + vertices[vertex].position = math.add(vertices[vertex].position, origin) + vertex_count = vertex_count + 1 + } + } + b.interior_point = math.add(b.interior_point, origin) + b.final_vertex_count = vertex_count + b.final_half_edge_count = half_edge_count + b.final_face_count = face_count +} + +has_hull(b: *HullBuilder) -> bool { + v = b.final_vertex_count + e = b.final_half_edge_count / 2 + f = b.final_face_count + return v - e + f == 2 && f >= 4 +} + +// The whole build. True iff the result satisfies Euler's identity. +construct(b: *HullBuilder, points: Vec3[], count: int, max_vertex_count: int, origin: Vec3, shifted: Vec3[]) -> bool { + if count < 4 { return false } + i = 0 + while i < count { + shifted[i] = math.sub(points[i], origin) + i = i + 1 + } + compute_tolerance(b, count, shifted) + if build_initial_hull(b, count, shifted) == false { return false } + budget = math.clamp_int(max_vertex_count - 4, 0, HULL_MAX_COUNT - 4) + vertex = next_conflict_vertex(b) + while vertex != NULL_INDEX && budget > 0 { + add_vertex_to_hull(b, vertex) + vertex = next_conflict_vertex(b) + budget = budget - 1 + } + clean_hull(b, origin) + return has_hull(b) +} + +// The builder's pools, sized from the input count and the vertex limit. +builder_create(count: int, clamped_max: int) -> HullBuilder { + m = clamped_max + b = HullBuilder { + tolerance: 0.0, min_radius: 0.0, min_outside: 0.0, interior_point: math.vec3_zero(), + vertices: null, vertex_capacity: count + 4, vertex_count: 0, + vertex_list_head: 0, orphaned_head: 0, conflict_head_base: 0, + edges: null, edge_capacity: math.max_int(48, 24 * m - 48), edge_count: 0, edge_free_head: NULL_INDEX, + faces: null, face_capacity: math.max_int(16, 5 * m - 10), face_count: 0, face_free_head: NULL_INDEX, + face_list_head: 0, + horizon: null, horizon_capacity: math.max_int(6, 3 * m - 6), horizon_count: 0, + cone: null, cone_capacity: math.max_int(6, 3 * m - 6), cone_count: 0, + merged_faces: null, merged_capacity: math.max_int(4, 2 * m - 4), merged_count: 0, + horizon_stack: null, horizon_stack_capacity: math.max_int(4, 2 * m - 4), + final_vertex_count: 0, final_half_edge_count: 0, final_face_count: 0 + } + vertex_slots = b.vertex_capacity + 2 + b.face_capacity + b.vertices = core.alloc(vertex_slots * sizeof(QHVertex)) + b.vertex_list_head = b.vertex_capacity + b.orphaned_head = b.vertex_capacity + 1 + b.conflict_head_base = b.vertex_capacity + 2 + b.edges = core.alloc(b.edge_capacity * sizeof(QHHalfEdge)) + b.faces = core.alloc((b.face_capacity + 1) * sizeof(QHFace)) + b.face_list_head = b.face_capacity + b.horizon = core.alloc(b.horizon_capacity * 4) + b.cone = core.alloc(b.cone_capacity * 4) + b.merged_faces = core.alloc(b.merged_capacity * 4) + b.horizon_stack = core.alloc(b.horizon_stack_capacity * FRAME_INTS * 4) + vertices = b.vertices as QHVertex[] + vlist_init(vertices, b.vertex_list_head) + vlist_init(vertices, b.orphaned_head) + faces = b.faces as QHFace[] + flist_init(faces, b.face_list_head) + return b +} + +builder_destroy(b: *HullBuilder) { + core.free_bytes(b.vertices, (b.vertex_capacity + 2 + b.face_capacity) * sizeof(QHVertex)) + core.free_bytes(b.edges, b.edge_capacity * sizeof(QHHalfEdge)) + core.free_bytes(b.faces, (b.face_capacity + 1) * sizeof(QHFace)) + core.free_bytes(b.horizon, b.horizon_capacity * 4) + core.free_bytes(b.cone, b.cone_capacity * 4) + core.free_bytes(b.merged_faces, b.merged_capacity * 4) + core.free_bytes(b.horizon_stack, b.horizon_stack_capacity * FRAME_INTS * 4) +} + +// --- the hull ------------------------------------------------------------------- + +find_hull_support_vertex(h: *HullData, direction: Vec3) -> int { + points = hull_points(h) + best_index = NULL_INDEX + best_dot = 0.0 - math.MAX_FLOAT + i = 0 + while i < h.vertex_count { + d = math.dot(direction, points[i]) + if d > best_dot { + best_index = i + best_dot = d + } + i = i + 1 + } + return best_index +} + +find_hull_support_face(h: *HullData, direction: Vec3) -> int { + planes = hull_planes(h) + best_index = NULL_INDEX + best_dot = 0.0 - math.MAX_FLOAT + i = 0 + while i < h.face_count { + d = math.dot(planes[i].normal, direction) + if d > best_dot { + best_index = i + best_dot = d + } + i = i + 1 + } + return best_index +} + +// Every invariant of a hull: Euler, the vertex-edge links, the twin +// pairing, the face rings, the centre below every plane, the bulk +// properties positive. +is_valid_hull(h: *HullData) -> bool { + if h == null { return false } + if h.version != HULL_VERSION { return false } + v = h.vertex_count + e = h.edge_count / 2 + f = h.face_count + if v - e + f != 2 { return false } + vertices = hull_vertices(h) + edges = hull_edges(h) + i = 0 + while i < h.vertex_count { + if edges[vertices[i]].origin != i { return false } + i = i + 1 + } + i = 0 + while i < h.edge_count { + if edges[i].twin != i + 1 || edges[i + 1].twin != i { return false } + i = i + 2 + } + faces = hull_faces(h) + planes = hull_planes(h) + face_index = 0 + while face_index < h.face_count { + if math.plane_separation(planes[face_index], h.center) >= 0.0 { return false } + base = faces[face_index] + edge_index = base + while true { + next = edges[edge_index].next + twin = edges[edge_index].twin + if edges[edge_index].face != face_index { return false } + if edges[twin].twin != edge_index { return false } + if edges[next].origin != edges[twin].origin { return false } + edge_index = next + if edge_index == base { break } + } + face_index = face_index + 1 + } + if h.volume <= 0.0 || h.surface_area <= 0.0 || h.inner_radius <= 0.0 { return false } + return true +} + +update_hull_bounds(h: *HullData) { + points = hull_points(h) + bounds = AABB { lower: points[0], upper: points[0] } + i = 1 + while i < h.vertex_count { + bounds = math.aabb_add_point(bounds, points[i]) + i = i + 1 + } + h.aabb = bounds +} + +// Volume, centroid, inertia about it, surface area and the inner radius +// (M. Kallay, "Computing the Moment of Inertia of a Solid Defined by a +// Triangle Mesh"). False when the hull has no volume. +update_hull_bulk_properties(h: *HullData) -> bool { + points = hull_points(h) + faces = hull_faces(h) + edges = hull_edges(h) + planes = hull_planes(h) + area = 0.0 + volume = 0.0 + center = math.vec3_zero() + // The first vertex as the origin, to reduce round-off. + origin = points[0] + xx = 0.0 + xy = 0.0 + yy = 0.0 + xz = 0.0 + zz = 0.0 + yz = 0.0 + face_index = 0 + while face_index < h.face_count { + edge1 = faces[face_index] + edge2 = edges[edge1].next + edge3 = edges[edge2].next + v1 = math.sub(points[edges[edge1].origin], origin) + while true { + v2 = math.sub(points[edges[edge2].origin], origin) + v3 = math.sub(points[edges[edge3].origin], origin) + area = area + math.length(math.cross(math.sub(v2, v1), math.sub(v3, v1))) + det = math.scalar_triple_product(v1, v2, v3) + volume = volume + det + v4 = math.add(v1, math.add(v2, v3)) + center = math.add(center, math.mul_sv(det, v4)) + xx = xx + det * (v1.x * v1.x + v2.x * v2.x + v3.x * v3.x + v4.x * v4.x) + yy = yy + det * (v1.y * v1.y + v2.y * v2.y + v3.y * v3.y + v4.y * v4.y) + zz = zz + det * (v1.z * v1.z + v2.z * v2.z + v3.z * v3.z + v4.z * v4.z) + xy = xy + det * (v1.x * v1.y + v2.x * v2.y + v3.x * v3.y + v4.x * v4.y) + xz = xz + det * (v1.x * v1.z + v2.x * v2.z + v3.x * v3.z + v4.x * v4.z) + yz = yz + det * (v1.y * v1.z + v2.y * v2.z + v3.y * v3.z + v4.y * v4.z) + edge2 = edge3 + edge3 = edges[edge3].next + if edge1 == edge3 { break } + } + face_index = face_index + 1 + } + + local_center = math.vec3_zero() + if volume > 0.0 { local_center = math.mul_sv(0.25 / volume, center) } + center = math.add(local_center, origin) + + radius = math.MAX_FLOAT + face_index = 0 + while face_index < h.face_count { + distance = math.plane_separation(planes[face_index], center) + radius = math.min_float(radius, 0.0 - distance) + face_index = face_index + 1 + } + + inertia = Matrix3 { + cx: math.vec3(yy + zz, 0.0 - xy, 0.0 - xz), + cy: math.vec3(0.0 - xy, xx + zz, 0.0 - yz), + cz: math.vec3(0.0 - xz, 0.0 - yz, xx + yy) + } + mass = volume / 6.0 + central = math.sub_mm(math.mul_sm(1.0 / 120.0, inertia), math.steiner(mass, local_center)) + + h.center = center + h.central_inertia = central + h.volume = mass + h.surface_area = 0.5 * area + h.inner_radius = radius + return mass > 0.0 && volume > 0.0 && area > 0.0 && radius > 0.0 +} + +// This engine's hash of the block, with the hash field itself zero; +// never zero. +hash_block(block: ptr, byte_count: int) -> long { + words = block as long[] + count = byte_count / 8 + h = 0x9E3779B9 as long + i = 0 + while i < count { + h = core.key_hash(h ^ words[i]) + (i as long) + i = i + 1 + } + h = core.key_hash(h) + if h == (0 as long) { return 1 as long } + return h +} + +stamp_hash(h: *HullData) { + h.hash = 0 as long + h.hash = hash_block(h as ptr, h.byte_count) +} + +hash_hull_data(h: *HullData) -> long { return h.hash } + +compare_hull_data(a: *HullData, b: *HullData) -> bool { + if a == b { return true } + if a.byte_count != b.byte_count { return false } + return memcmp(a as ptr, b as ptr, a.byte_count) == 0 +} + +// The block for a hull of these counts, with the offsets set. +allocate_hull(vertex_count: int, edge_count: int, face_count: int) -> *HullData { + byte_count = align8(sizeof(HullData)) + vertex_offset = byte_count + byte_count = byte_count + align8(vertex_count * 4) + point_offset = byte_count + byte_count = byte_count + align8(vertex_count * sizeof(Vec3)) + edge_offset = byte_count + byte_count = byte_count + align8(edge_count * sizeof(HalfEdge)) + plane_offset = byte_count + byte_count = byte_count + align8(face_count * sizeof(Plane)) + face_offset = byte_count + byte_count = byte_count + align8(face_count * 4) + h = core.alloc(byte_count) as *HullData + h.version = HULL_VERSION + h.vertex_offset = vertex_offset + h.point_offset = point_offset + h.edge_offset = edge_offset + h.plane_offset = plane_offset + h.face_offset = face_offset + h.vertex_count = vertex_count + h.edge_count = edge_count + h.face_count = face_count + h.byte_count = byte_count + return h +} + +// The convex hull of the points, with at most max_vertex_count vertices +// (clamped to [4, MAX_HULL_VERTICES]). Null when the points are fewer +// than four, coincident, collinear or coplanar, or the hull would exceed +// the limits. +create_hull(points: Vec3[], count: int, max_vertex_count: int) -> *HullData { + if count < 4 { return null } + origin = points[0] + clamped_max = math.clamp_int(max_vertex_count, 4, MAX_HULL_VERTICES) + b = builder_create(count, clamped_max) + shifted_block = core.alloc(count * sizeof(Vec3)) + shifted = shifted_block as Vec3[] + ok = construct(&b, points, count, clamped_max, origin, shifted) + core.free_bytes(shifted_block, count * sizeof(Vec3)) + if ok == false { + builder_destroy(&b) + return null + } + if b.final_vertex_count > MAX_HULL_VERTICES || b.final_face_count > MAX_HULL_FACES || b.final_half_edge_count > 2 * MAX_HULL_EDGES { + builder_destroy(&b) + return null + } + + vertices = b.vertices as QHVertex[] + edges = b.edges as QHHalfEdge[] + faces = b.faces as QHFace[] + + // Walk the lists into temp arrays, stamping the final index on each + // node so the resolution below is linear. + temp_vertices_block = core.alloc(HULL_MAX_COUNT * 4) + temp_vertices = temp_vertices_block as int[] + vertex_count = 0 + v = vertices[b.vertex_list_head].next + while v != b.vertex_list_head { + vertices[v].final_index = vertex_count + temp_vertices[vertex_count] = v + vertex_count = vertex_count + 1 + v = vertices[v].next + } + + // Edges in twin-paired order (i, i + 1). + temp_faces_block = core.alloc(HULL_MAX_COUNT * 4) + temp_faces = temp_faces_block as int[] + temp_edges_block = core.alloc(HULL_MAX_COUNT * 4) + temp_edges = temp_edges_block as int[] + face_count = 0 + edge_count = 0 + f = faces[b.face_list_head].next + while f != b.face_list_head { + faces[f].final_index = face_count + temp_faces[face_count] = f + face_count = face_count + 1 + start = faces[f].edge + e = start + while true { + if edges[e].final_index < 0 { + edges[e].final_index = edge_count + temp_edges[edge_count] = e + edge_count = edge_count + 1 + twin = edges[e].twin + edges[twin].final_index = edge_count + temp_edges[edge_count] = twin + edge_count = edge_count + 1 + } + e = edges[e].next + if e == start { break } + } + f = faces[f].next + } + + h = allocate_hull(vertex_count, edge_count, face_count) + hull_vertex = hull_vertices(h) + hull_edge = hull_edges(h) + hull_face = hull_faces(h) + final_points = hull_points(h) + planes = hull_planes(h) + + i = 0 + while i < vertex_count { + hull_vertex[i] = 0 + final_points[i] = vertices[temp_vertices[i]].position + i = i + 1 + } + i = 0 + while i < edge_count { + e = temp_edges[i] + hull_edge[i] = HalfEdge { next: edges[edges[e].next].final_index, twin: edges[edges[e].twin].final_index, + origin: vertices[edges[e].origin].final_index, face: faces[edges[e].face].final_index } + hull_vertex[vertices[edges[e].origin].final_index] = i + i = i + 1 + } + i = 0 + while i < face_count { + f = temp_faces[i] + hull_face[i] = edges[faces[f].edge].final_index + planes[i] = faces[f].plane + i = i + 1 + } + + core.free_bytes(temp_vertices_block, HULL_MAX_COUNT * 4) + core.free_bytes(temp_faces_block, HULL_MAX_COUNT * 4) + core.free_bytes(temp_edges_block, HULL_MAX_COUNT * 4) + builder_destroy(&b) + + update_hull_bounds(h) + if update_hull_bulk_properties(h) == false { + destroy_hull(h) + return null + } + if is_valid_hull(h) == false { + destroy_hull(h) + return null + } + stamp_hash(h) + return h +} + +destroy_hull(h: *HullData) { core.free_bytes(h as ptr, h.byte_count) } + +clone_hull(h: *HullData) -> *HullData { + if h == null || is_valid_hull(h) == false { return null } + clone = core.alloc(h.byte_count) + memcpy(clone, h as ptr, h.byte_count) + return clone as *HullData +} + +// A copy, scaled (non-uniformly, or mirrored) then transformed, with the +// planes and bulk properties recomputed. Null if the result has no volume. +clone_and_transform_hull(original: *HullData, t: Transform, scale: Vec3) -> *HullData { + if original == null || is_valid_hull(original) == false { return null } + h = clone_hull(original) + safe = math.safe_scale(scale) + edges = hull_edges(h) + faces = hull_faces(h) + face_count = h.face_count + vertex_count = h.vertex_count + + if safe.x * safe.y * safe.z < 0.0 { + // Reflected: reverse the winding of every face. + i = 0 + while i < face_count { + start = faces[i] + current = start + prev = NULL_INDEX + while true { + if edges[current].next == start { + prev = current + break + } + current = edges[current].next + if current == start { break } + } + current = start + while true { + next = edges[current].next + edges[current].next = prev + if current < edges[current].twin { + twin = edges[current].twin + swap = edges[current].origin + edges[current].origin = edges[twin].origin + edges[twin].origin = swap + } + prev = current + current = next + if current == start { break } + } + i = i + 1 + } + vertices = hull_vertices(h) + i = 0 + while i < vertex_count { + vertices[i] = edges[vertices[i]].twin + i = i + 1 + } + } + + m = math.make_matrix_from_quat(t.q) + points = hull_points(h) + i = 0 + while i < vertex_count { + points[i] = math.add(math.mul_mv(m, math.mul(safe, points[i])), t.p) + i = i + 1 + } + + planes = hull_planes(h) + i = 0 + while i < face_count { + count = 0 + centroid = math.vec3_zero() + nx = 0.0 + ny = 0.0 + nz = 0.0 + start = faces[i] + current = start + origin = points[edges[start].origin] + while true { + twin = edges[current].twin + v1 = math.sub(points[edges[current].origin], origin) + v2 = math.sub(points[edges[twin].origin], origin) + count = count + 1 + centroid = math.add(centroid, v1) + nx = nx + (v1.y - v2.y) * (v1.z + v2.z) + ny = ny + (v1.z - v2.z) * (v1.x + v2.x) + nz = nz + (v1.x - v2.x) * (v1.y + v2.y) + current = edges[current].next + if current == start { break } + } + centroid = math.add(math.mul_sv(1.0 / (count as float), centroid), origin) + normal = math.vec3(nx, ny, nz) + area = math.length(normal) + normal = math.mul_sv(1.0 / area, normal) + planes[i] = math.make_plane_from_normal_and_point(normal, centroid) + i = i + 1 + } + + update_hull_bounds(h) + if update_hull_bulk_properties(h) == false { + destroy_hull(h) + return null + } + stamp_hash(h) + return h +} + +// --- tessellated hulls ----------------------------------------------------------- + +// A cylinder of `sides` sides along y, from y_offset up by height. +create_cylinder(height: float, radius: float, y_offset: float, sides: int) -> *HullData { + count = 2 * sides + block = core.alloc(count * sizeof(Vec3)) + points = block as Vec3[] + alpha = 0.0 + delta = 2.0 * math.PI / (sides as float) + i = 0 + while i < sides { + cs = math.compute_cos_sin(alpha) + points[2 * i] = math.vec3(radius * cs.cosine, y_offset, radius * cs.sine) + points[2 * i + 1] = math.vec3(radius * cs.cosine, y_offset + height, radius * cs.sine) + alpha = alpha + delta + i = i + 1 + } + h = create_hull(points, count, count) + core.free_bytes(block, count * sizeof(Vec3)) + return h +} + +// A truncated cone along y: radius1 at the bottom, radius2 at height. +create_cone(height: float, radius1: float, radius2: float, slices: int) -> *HullData { + count = 2 * slices + block = core.alloc(count * sizeof(Vec3)) + points = block as Vec3[] + alpha = 0.0 + delta = 2.0 * math.PI / (slices as float) + i = 0 + while i < slices { + cs = math.compute_cos_sin(alpha) + points[2 * i] = math.vec3(radius1 * cs.cosine, 0.0, radius1 * cs.sine) + points[2 * i + 1] = math.vec3(radius2 * cs.cosine, height, radius2 * cs.sine) + alpha = alpha + delta + i = i + 1 + } + h = create_hull(points, count, count) + core.free_bytes(block, count * sizeof(Vec3)) + return h +} + +// Ten points of a Fibonacci lattice on a sphere: a rock. +create_rock(radius: float) -> *HullData { + count = 10 + block = core.alloc(count * sizeof(Vec3)) + points = block as Vec3[] + phi = (1.0 + sqrt(5.0)) / 2.0 + theta = 2.0 * math.PI / phi + cosine = 1.0 + sine = 0.0 + delta = math.compute_cos_sin(theta) + i = 0 + while i < count { + z = 1.0 - (2.0 * (i as float) + 1.0) / (count as float) + radius_xy = sqrt(1.0 - z * z) + points[i] = math.vec3(radius * radius_xy * cosine, radius * radius_xy * sine, radius * z) + c0 = cosine + s0 = sine + cosine = delta.cosine * c0 - delta.sine * s0 + sine = delta.sine * c0 + delta.cosine * s0 + i = i + 1 + } + h = create_hull(points, count, count) + core.free_bytes(block, count * sizeof(Vec3)) + return h +} + +// --- the box hull ---------------------------------------------------------------- + +// The box's topology: 8 vertices, 24 half-edges, 6 faces, as the +// reference bakes it; the geometry is filled in per box. +box_edge_table(index: int, field: int) -> int { + // next, twin, origin, face per half-edge + table = [ 2, 1, 2, 0, 17, 0, 1, 5, 4, 3, 1, 0, 20, 2, 5, 3, 6, 5, 5, 0, 23, 4, 6, 4, 0, 7, 6, 0, 18, 6, 2, 2, + 10, 9, 0, 1, 21, 8, 3, 5, 12, 11, 3, 1, 16, 10, 7, 2, 14, 13, 7, 1, 19, 12, 4, 4, 8, 15, 4, 1, 22, 14, 0, 3, + 7, 17, 3, 2, 9, 16, 2, 5, 11, 19, 6, 2, 5, 18, 7, 4, 15, 21, 1, 3, 1, 20, 0, 5, 3, 23, 4, 3, 13, 22, 5, 4 ] + return table[index * 4 + field] +} + +box_vertex_edge(index: int) -> int { + table = [ 8, 1, 0, 9, 13, 3, 5, 11 ] + return table[index] +} + +box_face_edge(index: int) -> int { + table = [ 0, 8, 16, 20, 19, 21 ] + return table[index] +} + +box_corner_sign(index: int) -> Vec3 { + if index == 0 { return math.vec3(1.0, 1.0, 1.0) } + if index == 1 { return math.vec3(0.0 - 1.0, 1.0, 1.0) } + if index == 2 { return math.vec3(0.0 - 1.0, 0.0 - 1.0, 1.0) } + if index == 3 { return math.vec3(1.0, 0.0 - 1.0, 1.0) } + if index == 4 { return math.vec3(1.0, 1.0, 0.0 - 1.0) } + if index == 5 { return math.vec3(0.0 - 1.0, 1.0, 0.0 - 1.0) } + if index == 6 { return math.vec3(0.0 - 1.0, 0.0 - 1.0, 0.0 - 1.0) } + return math.vec3(1.0, 0.0 - 1.0, 0.0 - 1.0) +} + +// A box of half widths hx, hy, hz under a local transform, as a hull +// with analytic mass properties. Destroy it like any hull. +make_transformed_box_hull(hx: float, hy: float, hz: float, t: Transform) -> *HullData { + min_h = 0.2 * math.LINEAR_SLOP + h = math.max_vec3(math.vec3(min_h, min_h, min_h), math.vec3(hx, hy, hz)) + box = allocate_hull(8, 24, 6) + box.aabb = math.aabb_transform(t, AABB { lower: math.neg(h), upper: h }) + box.surface_area = 8.0 * (h.x * h.y + h.x * h.z + h.y * h.z) + box.volume = 8.0 * h.x * h.y * h.z + box.inner_radius = math.min_float(h.x, math.min_float(h.y, h.z)) + box.center = t.p + box.central_inertia = math.rotate_inertia(t.q, math.box_inertia(box.volume, math.neg(h), h)) + + vertices = hull_vertices(box) + edges = hull_edges(box) + faces = hull_faces(box) + points = hull_points(box) + planes = hull_planes(box) + i = 0 + while i < 8 { + vertices[i] = box_vertex_edge(i) + points[i] = math.transform_point(t, math.mul(box_corner_sign(i), h)) + i = i + 1 + } + i = 0 + while i < 24 { + edges[i] = HalfEdge { next: box_edge_table(i, 0), twin: box_edge_table(i, 1), + origin: box_edge_table(i, 2), face: box_edge_table(i, 3) } + i = i + 1 + } + i = 0 + while i < 6 { + faces[i] = box_face_edge(i) + i = i + 1 + } + lower = math.neg(h) + planes[0] = math.transform_plane(t, math.make_plane_from_normal_and_point(math.neg(math.vec3_axis_x()), lower)) + planes[1] = math.transform_plane(t, math.make_plane_from_normal_and_point(math.vec3_axis_x(), h)) + planes[2] = math.transform_plane(t, math.make_plane_from_normal_and_point(math.neg(math.vec3_axis_y()), lower)) + planes[3] = math.transform_plane(t, math.make_plane_from_normal_and_point(math.vec3_axis_y(), h)) + planes[4] = math.transform_plane(t, math.make_plane_from_normal_and_point(math.neg(math.vec3_axis_z()), lower)) + planes[5] = math.transform_plane(t, math.make_plane_from_normal_and_point(math.vec3_axis_z(), h)) + stamp_hash(box) + return box +} + +make_cube_hull(half_width: float) -> *HullData { return make_box_hull(half_width, half_width, half_width) } + +make_offset_box_hull(hx: float, hy: float, hz: float, offset: Vec3) -> *HullData { + return make_transformed_box_hull(hx, hy, hz, Transform { p: offset, q: math.quat_identity() }) +} + +make_box_hull(hx: float, hy: float, hz: float) -> *HullData { + return make_transformed_box_hull(hx, hy, hz, math.transform_identity()) +} + +// Resolve a post scale (non-uniform, possibly negative) into new half +// widths and transform; approximate under shear. half_widths[0..2] and +// the transform are in/out. +scale_box(half_widths: float[], t: *Transform, post_scale: Vec3, min_half_width: float) { + q = t.q + if post_scale.x < 0.0 || post_scale.y < 0.0 || post_scale.z < 0.0 { + m = math.make_matrix_from_quat(q) + m.cx.x = m.cx.x * post_scale.x + m.cy.x = m.cy.x * post_scale.x + m.cz.x = m.cz.x * post_scale.x + m.cx.y = m.cx.y * post_scale.y + m.cy.y = m.cy.y * post_scale.y + m.cz.y = m.cz.y * post_scale.y + m.cx.z = m.cx.z * post_scale.z + m.cy.z = m.cy.z * post_scale.z + m.cz.z = m.cz.z * post_scale.z + m.cx = math.normalize(m.cx) + m.cy = math.normalize(m.cy) + m.cz = math.normalize(m.cz) + if post_scale.x < 0.0 { m.cx = math.neg(m.cx) } + if post_scale.y < 0.0 { m.cy = math.neg(m.cy) } + if post_scale.z < 0.0 { m.cz = math.neg(m.cz) } + q = math.make_quat_from_matrix(m) + } + abs_scale = math.abs_vec3(post_scale) + h = math.vec3(half_widths[0], half_widths[1], half_widths[2]) + p1 = math.mul(abs_scale, math.rotate_vector(q, math.neg(h))) + p2 = math.mul(abs_scale, math.rotate_vector(q, h)) + local1 = math.inv_rotate_vector(q, p1) + local2 = math.inv_rotate_vector(q, p2) + lower = math.min_vec3(local1, local2) + upper = math.max_vec3(local1, local2) + scaled = math.mul_sv(0.5, math.sub(upper, lower)) + limit = math.vec3(min_half_width, min_half_width, min_half_width) + result = math.max_vec3(scaled, limit) + half_widths[0] = result.x + half_widths[1] = result.y + half_widths[2] = result.z + t.p = math.mul(post_scale, t.p) + t.q = q +} + +make_scaled_box_hull(half_widths: Vec3, t: Transform, post_scale: Vec3) -> *HullData { + block = core.alloc(24) + h = block as float[] + h[0] = half_widths.x + h[1] = half_widths.y + h[2] = half_widths.z + xf = t + scale_box(h, &xf, post_scale, 4.0 * math.LINEAR_SLOP) + box = make_transformed_box_hull(h[0], h[1], h[2], xf) + core.free_bytes(block, 24) + return box +} + +// --- shape functions ------------------------------------------------------------- + +compute_hull_mass(h: *HullData, density: float) -> MassData { + return MassData { mass: density * h.volume, center: h.center, inertia: math.mul_sm(density, h.central_inertia) } +} + +compute_hull_aabb(h: *HullData, t: Transform) -> AABB { return math.aabb_transform(t, h.aabb) } + +compute_swept_hull_aabb(h: *HullData, xf1: Transform, xf2: Transform) -> AABB { + return math.aabb_union(math.aabb_transform(xf1, h.aabb), math.aabb_transform(xf2, h.aabb)) +} + +// A ray from origin along translation, to max_fraction of it, against +// the hull's planes: the entering face is the hit; a ray starting inside +// hits at its origin with no normal. +ray_cast_hull(h: *HullData, origin: Vec3, translation: Vec3, max_fraction: float) -> CastOutput { + output = CastOutput { normal: math.vec3_zero(), point: math.vec3_zero(), fraction: 0.0, iterations: 0, triangle_index: NULL_INDEX, hit: false } + lower = 0.0 + upper = max_fraction + best_face = NULL_INDEX + planes = hull_planes(h) + i = 0 + while i < h.face_count { + plane = planes[i] + distance = plane.offset - math.dot(plane.normal, origin) + denominator = math.dot(plane.normal, translation) + if denominator == 0.0 { + if distance < 0.0 { return output } + } else { + fraction = distance / denominator + if denominator < 0.0 { + if fraction > lower { + best_face = i + lower = fraction + } + } else if fraction < upper { + upper = fraction + } + if upper < lower { return output } + } + i = i + 1 + } + if best_face >= 0 { + output.point = math.add(origin, math.mul_sv(lower, translation)) + output.normal = planes[best_face].normal + output.fraction = lower + } else { + output.point = origin + } + output.hit = true + return output +} + +compute_hull_extent(h: *HullData, origin: Vec3) -> ShapeExtent { + points = hull_points(h) + extent = ShapeExtent { min_extent: h.inner_radius, max_extent: math.vec3_zero() } + i = 0 + while i < h.vertex_count { + extent.max_extent = math.max_vec3(extent.max_extent, math.abs_vec3(math.sub(points[i], origin))) + i = i + 1 + } + return extent +} + +// The area of the hull's shadow along the direction: the faces that +// face it, fanned into triangles. +compute_hull_projected_area(h: *HullData, direction: Vec3) -> float { + area = 0.0 + faces = hull_faces(h) + edges = hull_edges(h) + points = hull_points(h) + i = 0 + while i < h.face_count { + base = faces[i] + p1 = points[edges[base].origin] + edge_index = edges[base].next + p2 = points[edges[edge_index].origin] + edge_index = edges[edge_index].next + while true { + p3 = points[edges[edge_index].origin] + n = math.cross(math.sub(p2, p1), math.sub(p3, p1)) + area = area + math.max_float(math.dot(n, direction), 0.0) + p2 = p3 + edge_index = edges[edge_index].next + if edge_index == base { break } + } + i = i + 1 + } + return 0.5 * area +} + +// --- 2D hull (Andrew's monotone chain) -------------------------------------------- + +// cross(b - a, c - a) +cross_2d(a: Vec2, b: Vec2, c: Vec2) -> float { + return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x) +} + +// Drop the vertices that add the least area until the count is at the +// target and nothing left is within tolerance. Returns the new count. +simplify_hull_2d(hull: Point2D[], count1: int, target: int) -> int { + if count1 <= 3 { return count1 } + area_tol = 0.25 * math.LINEAR_SLOP * math.LINEAR_SLOP + count2 = count1 + while true { + min_area = math.MAX_FLOAT + min_index = 0 + i = 0 + while i < count2 { + prev = (i + count2 - 1) % count2 + next = (i + 1) % count2 + area = cross_2d(hull[prev].p, hull[i].p, hull[next].p) + if area + area_tol < min_area { + min_area = area + min_index = i + } + i = i + 1 + } + if count2 <= 3 || (min_area > area_tol && count2 <= target) { break } + count2 = count2 - 1 + if min_index < count2 { + j = min_index + while j < count2 { + hull[j] = hull[j + 1] + j = j + 1 + } + } + } + return count2 +} + +// By x then y; insertion, since n is usually twenty or fewer. +sort_2d(pts: Point2D[], count: int) { + i = 1 + while i < count { + base = pts[i] + j = i - 1 + while j >= 0 && (pts[j].p.x > base.p.x || (pts[j].p.x == base.p.x && pts[j].p.y > base.p.y)) { + pts[j + 1] = pts[j] + j = j - 1 + } + pts[j + 1] = base + i = i + 1 + } +} + +// Weld sorted points within a quarter slop; the lower original index wins. +weld_2d(pts: Point2D[], count1: int) -> int { + if count1 <= 1 { return count1 } + slop = 0.25 * math.LINEAR_SLOP + tol_sqr = slop * slop + count2 = 1 + base = 0 + i = 1 + while i < count1 { + if math.distance_squared_2(pts[i].p, pts[base].p) < tol_sqr { + if pts[i].original_index < pts[base].original_index { pts[base] = pts[i] } + } else { + pts[count2] = pts[i] + base = count2 + count2 = count2 + 1 + } + i = i + 1 + } + return count2 +} + +// The convex hull of the points, counter-clockwise, into hull (room for +// 2 * count). Sorts and welds pts in place. Returns the hull's count. +hull_2d(pts: Point2D[], count: int, hull: Point2D[]) -> int { + count1 = count + if count1 <= 0 { return 0 } + if count1 == 1 { + hull[0] = pts[0] + return 1 + } + sort_2d(pts, count1) + count1 = weld_2d(pts, count1) + if count1 == 1 { + hull[0] = pts[0] + return 1 + } + if count1 == 2 { + hull[0] = pts[0] + hull[1] = pts[1] + return 2 + } + count2 = 0 + i = 0 + while i < count1 { + while count2 >= 2 { + if cross_2d(hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p) > 0.0 { break } + count2 = count2 - 1 + } + hull[count2] = pts[i] + count2 = count2 + 1 + i = i + 1 + } + lower_count = count2 + 1 + i = count1 - 2 + while i >= 0 { + while count2 >= lower_count { + if cross_2d(hull[count2 - 2].p, hull[count2 - 1].p, pts[i].p) > 0.0 { break } + count2 = count2 - 1 + } + hull[count2] = pts[i] + count2 = count2 + 1 + i = i - 1 + } + return count2 - 1 +} diff --git a/aephysics/math/module.ae b/aephysics/math/module.ae index c4f8f21..bc33a2f 100644 --- a/aephysics/math/module.ae +++ b/aephysics/math/module.ae @@ -46,7 +46,10 @@ exports ( solve3, invert_t, abs_matrix3, make_matrix_from_quat, make_diagonal_matrix, steiner, sphere_inertia, cylinder_inertia, box_inertia, make_aabb, aabb_contains, aabb_area, aabb_center, aabb_extents, aabb_union, aabb_inflate, - aabb_overlaps, aabb_transform, closest_point_to_aabb, ray_cast_aabb, + aabb_overlaps, aabb_transform, closest_point_to_aabb, ray_cast_aabb, aabb_add_point, + LINEAR_SLOP, max_element, max_element_index, make_plane_from_normal_and_point, + make_plane_from_points, make_normal_from_points, transform_plane, plane_separation, + signed_volume, rotate_inertia, transform_inertia, distance_squared_2, point_to_segment_distance, line_distance, segment_distance, is_valid_float, is_valid_vec3, is_valid_quat, is_valid_transform, is_valid_matrix3, is_valid_aabb, is_bounded_aabb, is_sane_aabb, is_valid_plane, is_valid_position, @@ -75,6 +78,8 @@ const MAX_FLOAT = 340282346600000000000000000000000000000.0 // Reasonable bound on a world coordinate (B3_HUGE with a metre as the // length unit; Box3D scales it by the length units per metre). const HUGE = 100000.0 +// The linear slop: the collision margin, 5 mm, with the length unit a metre. +const LINEAR_SLOP = 0.005 struct Vec2 { x: float @@ -711,6 +716,63 @@ aabb_transform(t: Transform, a: AABB) -> AABB { } closest_point_to_aabb(point: Vec3, a: AABB) -> Vec3 { return clamp_vec3(point, a.lower, a.upper) } +aabb_add_point(a: AABB, point: Vec3) -> AABB { return AABB { lower: min_vec3(a.lower, point), upper: max_vec3(a.upper, point) } } + +// --- planes and inertia (math_internal.h) ------------------------------------ + +max_element(v: Vec3) -> float { return max_float(v.x, max_float(v.y, v.z)) } + +max_element_index(v: Vec3) -> int { + if v.x < v.y { + if v.y < v.z { return 2 } + return 1 + } + if v.x < v.z { return 2 } + return 0 +} + +make_plane_from_normal_and_point(normal: Vec3, point: Vec3) -> Plane { + return Plane { normal: normal, offset: dot(normal, point) } +} + +make_plane_from_points(point1: Vec3, point2: Vec3, point3: Vec3) -> Plane { + normal = normalize(cross(sub(point2, point1), sub(point3, point1))) + return Plane { normal: normal, offset: dot(normal, point1) } +} + +make_normal_from_points(point1: Vec3, point2: Vec3, point3: Vec3) -> Vec3 { + return normalize(cross(sub(point2, point1), sub(point3, point1))) +} + +// normal2 = q * normal1; offset2 = dot(normal2, p) + offset1 +transform_plane(t: Transform, p: Plane) -> Plane { + normal = rotate_vector(t.q, p.normal) + return Plane { normal: normal, offset: p.offset + dot(normal, t.p) } +} + +// The signed distance of a point from a plane. +plane_separation(p: Plane, point: Vec3) -> float { return dot(p.normal, point) - p.offset } + +// Negative if p is below the triangle v1-v2-v3. +signed_volume(v1: Vec3, v2: Vec3, v3: Vec3, p: Vec3) -> float { + n = cross(sub(v2, v1), sub(v3, v1)) + return dot(n, sub(p, v1)) +} + +rotate_inertia(q: Quat, central_inertia: Matrix3) -> Matrix3 { + r = make_matrix_from_quat(q) + return mul_mm(r, mul_mm(central_inertia, transpose(r))) +} + +transform_inertia(t: Transform, central_inertia: Matrix3, mass: float) -> Matrix3 { + return add_mm(rotate_inertia(t.q, central_inertia), steiner(mass, t.p)) +} + +distance_squared_2(a: Vec2, b: Vec2) -> float { + dx = b.x - a.x + dy = b.y - a.y + return dx * dx + dy * dy +} // One axis of the slab test: narrows [t_min, t_max] (as fractions[0..1]) // to the slab, false when the interval empties. diff --git a/aephysics/test_hull.ae b/aephysics/test_hull.ae new file mode 100644 index 0000000..3e81449 --- /dev/null +++ b/aephysics/test_hull.ae @@ -0,0 +1,731 @@ +// aephysics.hull against the reference's (Box3D's) test_hull.c: the cube +// and the tetrahedron against analytic values, determinism, the vertex +// limit, redundant input, cloning, the cylinder, the sphere reduction +// and stress builds, the merge-churn stress, degenerate input, the +// transformed box hull's geometry, and the 2D hull with its weld, +// order-independence and simplification. Every allocation is freed by +// the end. + +import std.string +import aephysics.math +import aephysics.core +import aephysics.hull + +extern calloc(count: int, size: int) -> ptr +extern exit(code: int) +extern free(p: ptr) +extern memcmp(a: ptr, b: ptr, size: int) -> int +extern sin(x: float) -> float +extern cos(x: float) -> float +extern sqrt(x: float) -> float + +var failures = 0 +var checks = 0 + +ensure(name: string, ok: bool) { + checks = checks + 1 + if !ok { + println("hull: FAIL ${name}") + failures = failures + 1 + } +} + +small(name: string, value: float, tolerance: float) { + ensure("${name} (${value})", math.abs_float(value) < tolerance) +} + +const FLT_EPSILON = 0.00000011920929 + +cube_corners() -> ptr { + block = calloc(8, sizeof(Vec3)) + p = block as Vec3[] + p[0] = math.vec3(1.0, 1.0, 1.0) + p[1] = math.vec3(0.0 - 1.0, 1.0, 1.0) + p[2] = math.vec3(0.0 - 1.0, 0.0 - 1.0, 1.0) + p[3] = math.vec3(1.0, 0.0 - 1.0, 1.0) + p[4] = math.vec3(1.0, 1.0, 0.0 - 1.0) + p[5] = math.vec3(0.0 - 1.0, 1.0, 0.0 - 1.0) + p[6] = math.vec3(0.0 - 1.0, 0.0 - 1.0, 0.0 - 1.0) + p[7] = math.vec3(1.0, 0.0 - 1.0, 0.0 - 1.0) + return block +} + +euler(name: string, h: *HullData) { + ensure("${name}: Euler's identity", h.vertex_count - h.edge_count / 2 + h.face_count == 2) +} + +// The cube against the analytic box hull. +against_box(name: string, h: *HullData) { + ref = hull.make_box_hull(1.0, 1.0, 1.0) + small("${name}: volume", h.volume - ref.volume, 0.0001) + small("${name}: surface area", h.surface_area - ref.surface_area, 0.0001) + small("${name}: inner radius", h.inner_radius - ref.inner_radius, FLT_EPSILON) + small("${name}: center x", h.center.x - ref.center.x, 0.00001) + small("${name}: center y", h.center.y - ref.center.y, 0.00001) + small("${name}: center z", h.center.z - ref.center.z, 0.00001) + small("${name}: lower x", h.aabb.lower.x + 1.0, FLT_EPSILON) + small("${name}: lower y", h.aabb.lower.y + 1.0, FLT_EPSILON) + small("${name}: lower z", h.aabb.lower.z + 1.0, FLT_EPSILON) + small("${name}: upper x", h.aabb.upper.x - 1.0, FLT_EPSILON) + small("${name}: upper y", h.aabb.upper.y - 1.0, FLT_EPSILON) + small("${name}: upper z", h.aabb.upper.z - 1.0, FLT_EPSILON) + d = math.sub_mm(h.central_inertia, ref.central_inertia) + small("${name}: inertia xx", d.cx.x, 0.0001) + small("${name}: inertia yy", d.cy.y, 0.0001) + small("${name}: inertia zz", d.cz.z, 0.0001) + small("${name}: inertia xy", d.cx.y, 0.0001) + small("${name}: inertia xz", d.cx.z, 0.0001) + small("${name}: inertia yz", d.cy.z, 0.0001) + small("${name}: inertia yx", d.cy.x, 0.0001) + small("${name}: inertia zx", d.cz.x, 0.0001) + small("${name}: inertia zy", d.cz.y, 0.0001) + hull.destroy_hull(ref) +} + +test_cube() { + block = cube_corners() + h = hull.create_hull(block as Vec3[], 8, 8) + ensure("cube: built", h != null) + if h == null { + free(block) + return + } + ensure("cube: 8 vertices", h.vertex_count == 8) + ensure("cube: 24 half-edges", h.edge_count == 24) + ensure("cube: 6 faces", h.face_count == 6) + euler("cube", h) + against_box("cube", h) + ensure("cube: valid", hull.is_valid_hull(h)) + hull.destroy_hull(h) + free(block) +} + +test_tetrahedron() { + block = calloc(4, sizeof(Vec3)) + p = block as Vec3[] + p[0] = math.vec3(0.0, 0.0, 0.0) + p[1] = math.vec3(1.0, 0.0, 0.0) + p[2] = math.vec3(0.0, 1.0, 0.0) + p[3] = math.vec3(0.0, 0.0, 1.0) + h = hull.create_hull(p, 4, 4) + ensure("tetrahedron: built", h != null) + if h == null { + free(block) + return + } + ensure("tetrahedron: 4 vertices", h.vertex_count == 4) + ensure("tetrahedron: 12 half-edges", h.edge_count == 12) + ensure("tetrahedron: 4 faces", h.face_count == 4) + euler("tetrahedron", h) + expected_volume = 1.0 / 6.0 + expected_area = 1.5 + 0.5 * sqrt(3.0) + expected_inner = 0.25 / sqrt(3.0) + small("tetrahedron: volume", h.volume - expected_volume, 0.00001) + small("tetrahedron: surface area", h.surface_area - expected_area, 0.00001) + small("tetrahedron: inner radius", h.inner_radius - expected_inner, 0.00001) + small("tetrahedron: center x", h.center.x - 0.25, 0.00001) + small("tetrahedron: center y", h.center.y - 0.25, 0.00001) + small("tetrahedron: center z", h.center.z - 0.25, 0.00001) + small("tetrahedron: lower", h.aabb.lower.x + h.aabb.lower.y + h.aabb.lower.z, FLT_EPSILON) + small("tetrahedron: upper x", h.aabb.upper.x - 1.0, FLT_EPSILON) + small("tetrahedron: upper y", h.aabb.upper.y - 1.0, FLT_EPSILON) + small("tetrahedron: upper z", h.aabb.upper.z - 1.0, FLT_EPSILON) + hull.destroy_hull(h) + free(block) +} + +test_determinism() { + block = cube_corners() + h1 = hull.create_hull(block as Vec3[], 8, 8) + h2 = hull.create_hull(block as Vec3[], 8, 8) + ensure("determinism: both built", h1 != null && h2 != null) + if h1 != null && h2 != null { + ensure("determinism: same size", h1.byte_count == h2.byte_count) + ensure("determinism: hash is not zero", h1.hash != (0 as long)) + ensure("determinism: same hash", h1.hash == h2.hash) + ensure("determinism: same bytes", memcmp(h1 as ptr, h2 as ptr, h1.byte_count) == 0) + ensure("determinism: compare agrees", hull.compare_hull_data(h1, h2)) + hull.destroy_hull(h1) + hull.destroy_hull(h2) + } + free(block) +} + +const SPHERE_N = 6 + +test_max_vertex() { + count = SPHERE_N * SPHERE_N + block = calloc(count, sizeof(Vec3)) + p = block as Vec3[] + index = 0 + i = 0 + while i < SPHERE_N { + theta = math.PI * (i as float) / ((SPHERE_N - 1) as float) + j = 0 + while j < SPHERE_N { + phi = 2.0 * math.PI * (j as float) / (SPHERE_N as float) + p[index] = math.vec3(sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta)) + index = index + 1 + j = j + 1 + } + i = i + 1 + } + h1 = hull.create_hull(p, count, 8) + ensure("max vertex: built with a cap of 8", h1 != null) + if h1 != null { + ensure("max vertex: the cap held", h1.vertex_count <= 8) + euler("max vertex 8", h1) + hull.destroy_hull(h1) + } + h2 = hull.create_hull(p, count, 1) + ensure("max vertex: built with a cap below the floor", h2 != null) + if h2 != null { + ensure("max vertex: clamped up to 4", h2.vertex_count >= 4 && h2.vertex_count <= hull.MAX_HULL_VERTICES) + hull.destroy_hull(h2) + } + h3 = hull.create_hull(p, count, 1000) + ensure("max vertex: built with a cap above the ceiling", h3 != null) + if h3 != null { + ensure("max vertex: clamped down", h3.vertex_count >= 4 && h3.vertex_count <= hull.MAX_HULL_VERTICES) + euler("max vertex 1000", h3) + hull.destroy_hull(h3) + } + free(block) +} + +test_redundant_input() { + block = calloc(20, sizeof(Vec3)) + p = block as Vec3[] + corners = cube_corners() + c = corners as Vec3[] + i = 0 + while i < 8 { + p[i] = c[i] + i = i + 1 + } + free(corners) + p[8] = math.vec3(1.0, 1.0, 1.0) + p[9] = math.vec3(1.0, 1.0, 1.0) + p[10] = math.vec3(0.0, 0.0, 0.0) + p[11] = math.vec3(0.5, 0.0, 0.0) + p[12] = math.vec3(0.0, 0.5, 0.0) + p[13] = math.vec3(0.0, 0.0, 0.5) + p[14] = math.vec3(0.0 - 0.5, 0.0, 0.0) + p[15] = math.vec3(0.0, 0.0 - 0.5, 0.0) + p[16] = math.vec3(0.0, 0.0, 0.0 - 0.5) + p[17] = math.vec3(0.25, 0.25, 0.25) + p[18] = math.vec3(0.0 - 0.25, 0.0 - 0.25, 0.0 - 0.25) + p[19] = math.vec3(0.5, 0.5, 0.5) + h = hull.create_hull(p, 20, 8) + ensure("redundant: built", h != null) + if h != null { + ensure("redundant: 8 vertices", h.vertex_count == 8) + ensure("redundant: 24 half-edges", h.edge_count == 24) + ensure("redundant: 6 faces", h.face_count == 6) + against_box("redundant", h) + hull.destroy_hull(h) + } + free(block) +} + +test_clone() { + block = cube_corners() + original = hull.create_hull(block as Vec3[], 8, 8) + ensure("clone: original built", original != null) + if original != null { + clone = hull.clone_hull(original) + ensure("clone: cloned", clone != null) + if clone != null { + ensure("clone: same size", clone.byte_count == original.byte_count) + ensure("clone: same bytes", memcmp(clone as ptr, original as ptr, original.byte_count) == 0) + hull.destroy_hull(clone) + } + // A mirrored, scaled and moved copy is still a valid cube of the scaled volume. + moved = hull.clone_and_transform_hull(original, Transform { p: math.vec3(1.0, 2.0, 3.0), q: math.quat_identity() }, math.vec3(0.0 - 2.0, 1.0, 3.0)) + ensure("clone: transformed", moved != null) + if moved != null { + ensure("clone: transformed is valid", hull.is_valid_hull(moved)) + small("clone: transformed volume", moved.volume - 8.0 * 6.0, 0.001) + small("clone: transformed center x", moved.center.x - 1.0, 0.00001) + small("clone: transformed center z", moved.center.z - 3.0, 0.00001) + hull.destroy_hull(moved) + } + hull.destroy_hull(original) + } + free(block) +} + +test_cylinder() { + height = 2.0 + radius = 1.0 + sides = 8 + y_offset = 0.0 + h = hull.create_cylinder(height, radius, y_offset, sides) + ensure("cylinder: built", h != null) + if h == null { return } + ensure("cylinder: vertices", h.vertex_count == 2 * sides) + ensure("cylinder: half-edges", h.edge_count == 6 * sides) + ensure("cylinder: faces", h.face_count == sides + 2) + // Analytic n-gon prism values. + half_angle = math.PI / (sides as float) + cap_area = (sides as float) * 0.5 * radius * radius * sin(2.0 * half_angle) + chord = 2.0 * radius * sin(half_angle) + lateral = (sides as float) * chord * height + expected_volume = cap_area * height + expected_area = 2.0 * cap_area + lateral + expected_inner = radius * cos(half_angle) + small("cylinder: volume", (h.volume - expected_volume) / expected_volume, 0.0001) + small("cylinder: surface area", (h.surface_area - expected_area) / expected_area, 0.0001) + small("cylinder: inner radius", h.inner_radius - expected_inner, 0.00001) + small("cylinder: center x", h.center.x, 0.00001) + small("cylinder: center y", h.center.y - (y_offset + 0.5 * height), 0.00001) + small("cylinder: center z", h.center.z, 0.00001) + small("cylinder: lower y", h.aabb.lower.y - y_offset, FLT_EPSILON) + small("cylinder: upper y", h.aabb.upper.y - (y_offset + height), FLT_EPSILON) + hull.destroy_hull(h) + + cone = hull.create_cone(1.0, 1.0, 0.5, 8) + ensure("cone: built", cone != null) + if cone != null { + ensure("cone: faces", cone.face_count == 10) + hull.destroy_hull(cone) + } + rock = hull.create_rock(1.0) + ensure("rock: built", rock != null) + if rock != null { + ensure("rock: valid", hull.is_valid_hull(rock)) + ensure("rock: 10 vertices", rock.vertex_count == 10) + hull.destroy_hull(rock) + } +} + +// XorShift32 + Shoemake's unit-vector recipe, the reference's. +var xseed = 0 + +xorshift() -> int { + xseed = xseed ^ (xseed << 13) + xseed = xseed ^ core.lsr32(xseed, 17) + xseed = xseed ^ (xseed << 5) + return xseed +} + +unit_random() -> float { + return ((xorshift() & 32767) as float) / 32767.0 +} + +fill_sphere_sample(points: Vec3[], count: int, seed: int) { + xseed = seed + i = 0 + while i < count { + u1 = unit_random() + u2 = 2.0 * math.PI * unit_random() + u3 = 2.0 * math.PI * unit_random() + s1 = sqrt(1.0 - u1) + s2 = sqrt(u1) + points[i] = math.vec3(s1 * sin(u2), s1 * cos(u2), s2 * sin(u3)) + i = i + 1 + } +} + +fill_cube_sample(points: Vec3[], count: int, seed: int) { + xseed = seed + i = 0 + while i < count { + x = 2.0 * unit_random() - 1.0 + y = 2.0 * unit_random() - 1.0 + z = 2.0 * unit_random() - 1.0 + points[i] = math.vec3(x, y, z) + i = i + 1 + } +} + +test_sphere_reduction() { + block = calloc(64, sizeof(Vec3)) + p = block as Vec3[] + fill_sphere_sample(p, 64, 12345) + h = hull.create_hull(p, 64, 20) + ensure("sphere reduction: built", h != null) + if h != null { + ensure("sphere reduction: within the cap", h.vertex_count >= 4 && h.vertex_count <= 20) + euler("sphere reduction", h) + ensure("sphere reduction: valid", hull.is_valid_hull(h)) + hull.destroy_hull(h) + } + free(block) +} + +// Dense random spheres at several caps push the builder's pools close +// to their peaks; the free lists have to reclaim in time. +test_sphere_stress() { + n = 512 + block = calloc(n, sizeof(Vec3)) + p = block as Vec3[] + s = 0 + while s < 4 { + seed = 12345 + if s == 1 { seed = 1 } + if s == 2 { seed = 0 - 559038737 } // 0xdeadbeef + if s == 3 { seed = 0 - 889262067 } // 0xcafef00d + fill_sphere_sample(p, n, seed) + m = 0 + while m < 3 { + cap = 16 + 8 * m + h = hull.create_hull(p, n, cap) + ensure("sphere stress: built (seed ${s}, cap ${cap})", h != null) + if h != null { + ensure("sphere stress: within the cap", h.vertex_count >= 4 && h.vertex_count <= cap) + euler("sphere stress", h) + ensure("sphere stress: at least 4 faces", h.face_count >= 4) + ensure("sphere stress: valid", hull.is_valid_hull(h)) + hull.destroy_hull(h) + } + m = m + 1 + } + s = s + 1 + } + free(block) +} + +// Random points inside a cube with the corners stamped last: a small hull +// out of heavy conflict-list churn and coplanar merges. +test_merge_churn_stress() { + n = 4096 + block = calloc(n, sizeof(Vec3)) + p = block as Vec3[] + s = 0 + while s < 2 { + seed = 12345 + if s == 1 { seed = 0 - 559038737 } // 0xdeadbeef + fill_cube_sample(p, n, seed) + c = 0 + while c < 8 { + x = 0.0 - 1.0 + y = 0.0 - 1.0 + z = 0.0 - 1.0 + if (c & 1) != 0 { x = 1.0 } + if (c & 2) != 0 { y = 1.0 } + if (c & 4) != 0 { z = 1.0 } + p[n - 8 + c] = math.vec3(x, y, z) + c = c + 1 + } + h = hull.create_hull(p, n, 64) + ensure("merge churn: built (seed ${s})", h != null) + if h != null { + ensure("merge churn: 8 vertices", h.vertex_count == 8) + ensure("merge churn: 24 half-edges", h.edge_count == 24) + ensure("merge churn: 6 faces", h.face_count == 6) + hull.destroy_hull(h) + } + s = s + 1 + } + free(block) +} + +test_degenerate() { + block = calloc(8, sizeof(Vec3)) + p = block as Vec3[] + i = 0 + while i < 8 { + p[i] = math.vec3(i as float, 0.0, 0.0) + i = i + 1 + } + ensure("degenerate: empty input", hull.create_hull(p, 0, 8) == null) + ensure("degenerate: three points", hull.create_hull(p, 3, 8) == null) + ensure("degenerate: collinear", hull.create_hull(p, 8, 8) == null) + i = 0 + while i < 8 { + p[i] = math.vec3(1.0, 2.0, 3.0) + i = i + 1 + } + ensure("degenerate: coincident", hull.create_hull(p, 8, 8) == null) + p[0] = math.vec3(0.0, 0.0, 0.0) + p[1] = math.vec3(1.0, 0.0, 0.0) + p[2] = math.vec3(0.0, 1.0, 0.0) + p[3] = math.vec3(1.0, 1.0, 0.0) + p[4] = math.vec3(2.0, 0.5, 0.0) + p[5] = math.vec3(0.5, 2.0, 0.0) + ensure("degenerate: coplanar", hull.create_hull(p, 6, 8) == null) + free(block) +} + +// Rotations from libm, so the analytic corners and planes are exact. +exact_quat(axis: Vec3, radians: float) -> Quat { + half = 0.5 * radians + s = sin(half) + return Quat { v: math.mul_sv(s, axis), s: cos(half) } +} + +corner_sign(i: int) -> Vec3 { + if i == 0 { return math.vec3(1.0, 1.0, 1.0) } + if i == 1 { return math.vec3(0.0 - 1.0, 1.0, 1.0) } + if i == 2 { return math.vec3(0.0 - 1.0, 0.0 - 1.0, 1.0) } + if i == 3 { return math.vec3(1.0, 0.0 - 1.0, 1.0) } + if i == 4 { return math.vec3(1.0, 1.0, 0.0 - 1.0) } + if i == 5 { return math.vec3(0.0 - 1.0, 1.0, 0.0 - 1.0) } + if i == 6 { return math.vec3(0.0 - 1.0, 0.0 - 1.0, 0.0 - 1.0) } + return math.vec3(1.0, 0.0 - 1.0, 0.0 - 1.0) +} + +// The box hull's geometry against the transform, so an axis swap in the +// point or plane bake cannot pass silently. +check_transformed_box(name: string, h: Vec3, xf: Transform) { + box = hull.make_transformed_box_hull(h.x, h.y, h.z, xf) + tol = 0.00001 + points = hull.hull_points(box) + i = 0 + while i < 8 { + expected = math.transform_point(xf, math.mul(corner_sign(i), h)) + small("${name}: corner ${i} x", points[i].x - expected.x, tol) + small("${name}: corner ${i} y", points[i].y - expected.y, tol) + small("${name}: corner ${i} z", points[i].z - expected.z, tol) + i = i + 1 + } + planes = hull.hull_planes(box) + i = 0 + while i < 6 { + local_normal = math.vec3_axis_z() + local_offset = h.z + if i < 2 { + local_normal = math.vec3_axis_x() + local_offset = h.x + } else if i < 4 { + local_normal = math.vec3_axis_y() + local_offset = h.y + } + if (i & 1) == 0 { local_normal = math.neg(local_normal) } + n = math.rotate_vector(xf.q, local_normal) + offset = local_offset + math.dot(n, xf.p) + small("${name}: plane ${i} normal x", planes[i].normal.x - n.x, tol) + small("${name}: plane ${i} normal y", planes[i].normal.y - n.y, tol) + small("${name}: plane ${i} normal z", planes[i].normal.z - n.z, tol) + small("${name}: plane ${i} offset", planes[i].offset - offset, tol) + i = i + 1 + } + // The stored box bounds every corner; the hull is a valid hull. + point_box = math.make_aabb(((box as ptr) + box.point_offset) as float[], 8, 0.0) + ensure("${name}: the aabb holds the corners", math.aabb_contains(box.aabb, point_box)) + ensure("${name}: valid", hull.is_valid_hull(box)) + ensure("${name}: hash is not zero", box.hash != (0 as long)) + hull.destroy_hull(box) +} + +test_transformed_box() { + h = math.vec3(0.25, 0.5, 0.3) + check_transformed_box("box identity", h, math.transform_identity()) + check_transformed_box("box translated", h, Transform { p: math.vec3(0.4, 0.0 - 0.7, 0.1), q: math.quat_identity() }) + check_transformed_box("box rotated", h, Transform { p: math.vec3_zero(), q: exact_quat(math.vec3_axis_y(), 0.25 * math.PI) }) + check_transformed_box("box transformed", h, Transform { p: math.vec3(3.0, 0.0 - 2.0, 1.5), q: exact_quat(math.vec3_axis_z(), 0.25 * math.PI) }) + + // A scaled box resolves the post scale into half widths. + scaled = hull.make_scaled_box_hull(math.vec3(1.0, 1.0, 1.0), math.transform_identity(), math.vec3(2.0, 0.0 - 3.0, 0.5)) + small("scaled box: volume", scaled.volume - 8.0 * 2.0 * 3.0 * 0.5, 0.0001) + hull.destroy_hull(scaled) + + // A ray into a box from the -x side hits the -x face at a third. + box = hull.make_box_hull(1.0, 1.0, 1.0) + output = hull.ray_cast_hull(box, math.vec3(0.0 - 3.0, 0.0, 0.0), math.vec3(6.0, 0.0, 0.0), 1.0) + ensure("ray: hit", output.hit) + small("ray: fraction", output.fraction - 1.0 / 3.0, 0.00001) + small("ray: normal x", output.normal.x + 1.0, 0.00001) + small("ray: point x", output.point.x + 1.0, 0.00001) + miss = hull.ray_cast_hull(box, math.vec3(0.0 - 3.0, 2.0, 0.0), math.vec3(6.0, 0.0, 0.0), 1.0) + ensure("ray: miss", miss.hit == false) + inside = hull.ray_cast_hull(box, math.vec3(0.0, 0.0, 0.0), math.vec3(6.0, 0.0, 0.0), 1.0) + ensure("ray: from inside hits at the origin", inside.hit && inside.fraction == 0.0) + // The support vertex along +x+y+z is the (1,1,1) corner; the shadow along x is 4. + sv = hull.find_hull_support_vertex(box, math.vec3(1.0, 1.0, 1.0)) + points = hull.hull_points(box) + small("support vertex", points[sv].x + points[sv].y + points[sv].z - 3.0, 0.00001) + sf = hull.find_hull_support_face(box, math.vec3(0.0, 1.0, 0.0)) + planes = hull.hull_planes(box) + small("support face", planes[sf].normal.y - 1.0, 0.00001) + small("projected area", hull.compute_hull_projected_area(box, math.vec3(1.0, 0.0, 0.0)) - 4.0, 0.00001) + extent = hull.compute_hull_extent(box, math.vec3_zero()) + small("extent min", extent.min_extent - 1.0, 0.00001) + small("extent max", extent.max_extent.x - 1.0, 0.00001) + mass = hull.compute_hull_mass(box, 2.0) + small("mass", mass.mass - 16.0, 0.00001) + hull.destroy_hull(box) +} + +// --- 2D --------------------------------------------------------------------- + +fill_points_2d(pts: Point2D[], xs: float[], count: int) { + i = 0 + while i < count { + pts[i] = Point2D { p: math.vec2(xs[2 * i], xs[2 * i + 1]), separation: 0.0, original_index: i } + i = i + 1 + } +} + +hull_2d_area(h: Point2D[], count: int) -> float { + sum = 0.0 + i = 0 + while i < count { + next = (i + 1) % count + sum = sum + h[i].p.x * h[next].p.y - h[i].p.y * h[next].p.x + i = i + 1 + } + return 0.5 * sum +} + +test_hull_2d() { + xs_block = calloc(16, 8) + xs = xs_block as float[] + pts_block = calloc(8, sizeof(Point2D)) + pts = pts_block as Point2D[] + h_block = calloc(16, sizeof(Point2D)) + h = h_block as Point2D[] + + // A square with interior points: only the corners survive. + values = [ 0.0 - 1.0, 0.0 - 1.0, 1.0, 0.0 - 1.0, 1.0, 1.0, 0.0 - 1.0, 1.0, 0.0, 0.0, 0.5, 0.25, 0.0 - 0.3, 0.6, 0.1, 0.0 - 0.4 ] + i = 0 + while i < 16 { + xs[i] = values[i] + i = i + 1 + } + fill_points_2d(pts, xs, 8) + count = hull.hull_2d(pts, 8, h) + ensure("2d square: 4 points", count == 4) + ensure("2d square: counter-clockwise", hull_2d_area(h, count) > 0.0) + on_hull = calloc(8, 4) + marks = on_hull as int[] + i = 0 + while i < count { + ensure("2d square: original index in range", h[i].original_index >= 0 && h[i].original_index < 8) + marks[h[i].original_index] = 1 + i = i + 1 + } + i = 0 + while i < 8 { + ensure("2d square: corners on, interior off (${i})", (marks[i] == 1) == (i < 4)) + i = i + 1 + } + free(on_hull) + + // Collinear points: two survive. + i = 0 + while i < 5 { + xs[2 * i] = i as float + xs[2 * i + 1] = 0.0 + i = i + 1 + } + fill_points_2d(pts, xs, 5) + ensure("2d collinear: 2 points", hull.hull_2d(pts, 5, h) == 2) + + // A point within a tenth of a slop of a corner welds away; the lower index wins. + offset = 0.1 * math.LINEAR_SLOP + values2 = [ 0.0 - 1.0, 0.0 - 1.0, 1.0, 0.0 - 1.0, 1.0, 1.0, 0.0 - 1.0, 1.0, 0.0 - 1.0 + offset, 0.0 - 1.0 + offset ] + i = 0 + while i < 10 { + xs[i] = values2[i] + i = i + 1 + } + fill_points_2d(pts, xs, 5) + count = hull.hull_2d(pts, 5, h) + ensure("2d weld: 4 points", count == 4) + i = 0 + while i < count { + ensure("2d weld: the welded point is gone", h[i].original_index != 4) + i = i + 1 + } + + // The surviving set does not depend on the input order. + values3 = [ 0.0 - 2.0, 0.0 - 1.0, 0.0, 0.0 - 1.5, 2.0, 0.0 - 1.0, 2.5, 0.5, 1.0, 2.0, 0.0 - 1.0, 1.8, 0.0 - 2.5, 0.4, 0.2, 0.1 ] + permutation = [ 5, 2, 7, 0, 4, 6, 1, 3 ] + i = 0 + while i < 16 { + xs[i] = values3[i] + i = i + 1 + } + fill_points_2d(pts, xs, 8) + count1 = hull.hull_2d(pts, 8, h) + ensure("2d order: 7 points", count1 == 7) + on1 = calloc(8, 4) + marks1 = on1 as int[] + i = 0 + while i < count1 { + marks1[h[i].original_index] = 1 + i = i + 1 + } + i = 0 + while i < 8 { + k = permutation[i] + pts[i] = Point2D { p: math.vec2(xs[2 * k], xs[2 * k + 1]), separation: 0.0, original_index: i } + i = i + 1 + } + count2 = hull.hull_2d(pts, 8, h) + ensure("2d order: the same count", count2 == count1) + on2 = calloc(8, 4) + marks2 = on2 as int[] + i = 0 + while i < count2 { + marks2[permutation[h[i].original_index]] = 1 + i = i + 1 + } + i = 0 + while i < 8 { + ensure("2d order: the same set (${i})", marks1[i] == marks2[i]) + i = i + 1 + } + free(on1) + free(on2) + free(xs_block) + free(pts_block) + free(h_block) + + // Simplify a 20-gon down to 6; a square stays a square. + pts20 = calloc(20, sizeof(Point2D)) + p20 = pts20 as Point2D[] + h40 = calloc(40, sizeof(Point2D)) + h20 = h40 as Point2D[] + i = 0 + while i < 20 { + angle = 2.0 * math.PI * (i as float) / 20.0 + p20[i] = Point2D { p: math.vec2(cos(angle), sin(angle)), separation: 0.0, original_index: i } + i = i + 1 + } + hull_count = hull.hull_2d(p20, 20, h20) + ensure("2d simplify: 20 on the hull", hull_count == 20) + count = hull.simplify_hull_2d(h20, hull_count, 6) + ensure("2d simplify: down to 6", count == 6) + ensure("2d simplify: still counter-clockwise", hull_2d_area(h20, count) > 0.0) + sq = [ 0.0 - 1.0, 0.0 - 1.0, 1.0, 0.0 - 1.0, 1.0, 1.0, 0.0 - 1.0, 1.0 ] + i = 0 + while i < 4 { + p20[i] = Point2D { p: math.vec2(sq[2 * i], sq[2 * i + 1]), separation: 0.0, original_index: i } + i = i + 1 + } + hull_count = hull.hull_2d(p20, 4, h20) + ensure("2d simplify noop: 4", hull_count == 4) + ensure("2d simplify noop: target 8 keeps 4", hull.simplify_hull_2d(h20, hull_count, 8) == 4) + ensure("2d simplify noop: target 4 keeps 4", hull.simplify_hull_2d(h20, hull_count, 4) == 4) + free(pts20) + free(h40) +} + +main() { + before = core.alloc_count() + test_cube() + test_tetrahedron() + test_determinism() + test_max_vertex() + test_redundant_input() + test_clone() + test_cylinder() + test_sphere_reduction() + test_sphere_stress() + test_merge_churn_stress() + test_degenerate() + test_transformed_box() + test_hull_2d() + ensure("every counted allocation was freed", core.alloc_count() == before && core.bytes_in_use() == 0) + + println("hull: ${checks} checks") + if failures == 0 { + println("hull: all checks passed") + } else { + println("hull: ${failures} failure(s)") + exit(1) + } +} diff --git a/bench/RESULTS.md b/bench/RESULTS.md index 6cc443f..7db172d 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -103,3 +103,24 @@ SSE2 box tests and 24-byte float boxes show against the port's scalar tests and 48-byte double boxes; that is the number for the native wide path to beat, if the whole-step benchmark says the tree's ray cast matters. + +## hull + +`bench/hull.ae` and `bench/hull_box3d.c`: 200 hulls of 64 points on a +sphere capped at 32 vertices, 20 hulls of 4,096 points inside a cube with +the corners stamped last (every interior point through the conflict lists, +most cone faces merged out), and 2,000 box hulls. The same random sequence +on both. + +| phase | aephysics | Box3D | +|---|---|---| +| 200 sphere hulls, 64 points to 32 | 4.7 ms | **2.9** | +| 20 cube hulls, 4,096 points to 8 | 5.2 | **2.5** | +| 2,000 box hulls | 2.0 | **0.17** | + +Both produce 6,554 vertices and 12,106 faces in total: the same hulls. +The builder runs at 1.6-2x the reference's time (doubles, indices in +place of pointers, structs passed by value); the box hull is a heap block +here where the reference's is a stack value, so its cost is the +allocation and the hash. Hull construction is a load-time cost, not a +per-step one, so this is recorded rather than chased. diff --git a/bench/hull.ae b/bench/hull.ae new file mode 100644 index 0000000..cfc5231 --- /dev/null +++ b/bench/hull.ae @@ -0,0 +1,98 @@ +// The hull builder on the same scenes as bench/hull_box3d.c: 200 hulls +// of 64 sphere points capped at 32 vertices, 20 hulls of 4,096 points +// inside a cube (the merge churn), and 2,000 boxes. Single thread, wall +// time per phase. Same random sequence as the reference's. +import std.string +import std.os +import aephysics.math +import aephysics.hull + +extern calloc(count: int, size: int) -> ptr +extern free(p: ptr) +extern sin(x: float) -> float +extern cos(x: float) -> float +extern sqrt(x: float) -> float + +clock() -> long { return os.now_monotonic_ns() } +ms(ns: long) -> float { return (ns as float) / 1000000.0 } + +var xseed = 12345 + +unit_random() -> float { + xseed = xseed ^ (xseed << 13) + xseed = xseed ^ (((xseed >> 17) & 32767)) + xseed = xseed ^ (xseed << 5) + return ((xseed & 32767) as float) / 32767.0 +} + +fill_sphere(points: Vec3[], count: int) { + i = 0 + while i < count { + u1 = unit_random() + u2 = 2.0 * math.PI * unit_random() + u3 = 2.0 * math.PI * unit_random() + s1 = sqrt(1.0 - u1) + s2 = sqrt(u1) + points[i] = math.vec3(s1 * sin(u2), s1 * cos(u2), s2 * sin(u3)) + i = i + 1 + } +} + +fill_cube(points: Vec3[], count: int) { + i = 0 + while i < count { + points[i] = math.vec3(2.0 * unit_random() - 1.0, 2.0 * unit_random() - 1.0, 2.0 * unit_random() - 1.0) + i = i + 1 + } + c = 0 + while c < 8 { + x = 0.0 - 1.0 + y = 0.0 - 1.0 + z = 0.0 - 1.0 + if (c & 1) != 0 { x = 1.0 } + if (c & 2) != 0 { y = 1.0 } + if (c & 4) != 0 { z = 1.0 } + points[count - 8 + c] = math.vec3(x, y, z) + c = c + 1 + } +} + +main() { + block = calloc(4096, sizeof(Vec3)) + points = block as Vec3[] + vertices = 0 + faces = 0 + xseed = 12345 + t0 = clock() + i = 0 + while i < 200 { + fill_sphere(points, 64) + h = hull.create_hull(points, 64, 32) + vertices = vertices + h.vertex_count + faces = faces + h.face_count + hull.destroy_hull(h) + i = i + 1 + } + t1 = clock() + i = 0 + while i < 20 { + fill_cube(points, 4096) + h = hull.create_hull(points, 4096, 64) + vertices = vertices + h.vertex_count + faces = faces + h.face_count + hull.destroy_hull(h) + i = i + 1 + } + t2 = clock() + volume = 0.0 + i = 0 + while i < 2000 { + box = hull.make_box_hull(0.5 + 0.001 * (i as float), 0.5, 0.5) + volume = volume + box.volume + hull.destroy_hull(box) + i = i + 1 + } + t3 = clock() + println("aephysics hull: 200 spheres (64 -> 32) ${ms(t1 - t0)} ms, 20 cubes (4096 -> 8) ${ms(t2 - t1)} ms, 2000 boxes ${ms(t3 - t2)} ms, ${vertices} vertices, ${faces} faces, box volume ${volume}") + free(block) +} diff --git a/bench/hull_box3d.c b/bench/hull_box3d.c new file mode 100644 index 0000000..8be56a8 --- /dev/null +++ b/bench/hull_box3d.c @@ -0,0 +1,88 @@ +// The hull builder of the reference on the same scenes as bench/hull.ae: +// 200 hulls of 64 sphere points capped at 32 vertices, 20 hulls of 4,096 +// points inside a cube (the merge churn), and 2,000 boxes. Single +// thread, wall time per phase. +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#include +#include +#include + +static double now_ms( void ) +{ + struct timespec ts; + timespec_get( &ts, TIME_UTC ); + return ts.tv_sec * 1000.0 + ts.tv_nsec / 1.0e6; +} + +static uint32_t s_seed = 12345; +static float unit_random( void ) +{ + s_seed ^= s_seed << 13; + s_seed ^= s_seed >> 17; + s_seed ^= s_seed << 5; + return (float)( s_seed & 32767u ) / 32767.0f; +} + +static void fill_sphere( b3Vec3* points, int count ) +{ + for ( int i = 0; i < count; ++i ) + { + float u1 = unit_random(); + float u2 = 2.0f * B3_PI * unit_random(); + float u3 = 2.0f * B3_PI * unit_random(); + float s1 = sqrtf( 1.0f - u1 ), s2 = sqrtf( u1 ); + points[i] = (b3Vec3){ s1 * sinf( u2 ), s1 * cosf( u2 ), s2 * sinf( u3 ) }; + } +} + +static void fill_cube( b3Vec3* points, int count ) +{ + for ( int i = 0; i < count; ++i ) + { + points[i] = (b3Vec3){ 2.0f * unit_random() - 1.0f, 2.0f * unit_random() - 1.0f, 2.0f * unit_random() - 1.0f }; + } + for ( int c = 0; c < 8; ++c ) + { + points[count - 8 + c] = (b3Vec3){ ( c & 1 ) ? 1.0f : -1.0f, ( c & 2 ) ? 1.0f : -1.0f, ( c & 4 ) ? 1.0f : -1.0f }; + } +} + +int main( void ) +{ + b3Vec3* points = malloc( 4096 * sizeof( b3Vec3 ) ); + int vertices = 0, faces = 0; + + s_seed = 12345; + double t0 = now_ms(); + for ( int i = 0; i < 200; ++i ) + { + fill_sphere( points, 64 ); + b3HullData* hull = b3CreateHull( points, 64, 32 ); + vertices += hull->vertexCount; + faces += hull->faceCount; + b3DestroyHull( hull ); + } + double t1 = now_ms(); + for ( int i = 0; i < 20; ++i ) + { + fill_cube( points, 4096 ); + b3HullData* hull = b3CreateHull( points, 4096, 64 ); + vertices += hull->vertexCount; + faces += hull->faceCount; + b3DestroyHull( hull ); + } + double t2 = now_ms(); + float volume = 0.0f; + for ( int i = 0; i < 2000; ++i ) + { + b3BoxHull box = b3MakeBoxHull( 0.5f + 0.001f * i, 0.5f, 0.5f ); + volume += box.base.volume; + } + double t3 = now_ms(); + printf( "box3d hull: 200 spheres (64 -> 32) %.2f ms, 20 cubes (4096 -> 8) %.2f ms, 2000 boxes %.2f ms, %d vertices, %d faces, box volume %.1f\n", + t1 - t0, t2 - t1, t3 - t2, vertices, faces, volume ); + free( points ); + return 0; +} diff --git a/design.md b/design.md index 805fba2..139329a 100644 --- a/design.md +++ b/design.md @@ -34,25 +34,31 @@ started until its tests pass. scene (same hits, height, area ratio), insert faster, ray cast 1.9x. Save/load not ported; the atomic moved-marking waits for the parallel layer. -4. **collision, static**: `aabb`, `hull` (quickhull, 3,100 lines), +4. **hull** (done): quickhull as `aephysics.hull`, the builder's + pointers as indices with the intrusive lists chained through the + pools and their sentinels in extra slots, int half-edge indices, no + SOA mirrors, the box hull a heap block. 438 checks from test_hull.c + at the reference's tolerances; the same hulls on the benchmark + scenes at 1.6-2x its time. +5. **collision, static**: `aabb`, `distance` (GJK, shape cast, segment distance), `manifold` and `convex_manifold` (sphere/capsule/hull contact manifolds), `triangle_manifold`, `mesh`, `height_field`, `shape` (mass properties, ray and shape casts per shape). Tests: `test_collision`, `test_distance`, `test_hull`, `test_manifold`, `test_sat`, `test_shape`, `test_mesh`, `test_height_field`. -5. **dynamics**: `body`, `contact`, `constraint_graph` (graph colouring), +6. **dynamics**: `body`, `contact`, `constraint_graph` (graph colouring), `solver_set`, `island`, `solver` (the Soft Step: sub-stepping, relax iterations, restitution), `contact_solver` (scalar first; the wide SIMD path second, measured), the joints (revolute, prismatic, distance, motor, weld, wheel, spherical), `sensor`, `mover` (the character mover), `physics_world`. Tests: `test_body`, `test_joint`, `test_world`, `test_mover`, `test_determinism`, `test_large_world`. -6. **parallel**: `parallel_for` and the scheduler over Aether's actors; +7. **parallel**: `parallel_for` and the scheduler over Aether's actors; the benchmarks by thread count as the original records them. -7. **recording and replay**, `world_snapshot`: last, since they are the +8. **recording and replay**, `world_snapshot`: last, since they are the tooling and not the engine. -8. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run +9. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run against the C build on the same machine, recorded under `benchmark/`. ## Measures