From ffeb14309e9a6833bb3e7161c98116c36d4a7995 Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Sun, 20 Sep 2026 00:34:19 -0300 Subject: [PATCH] mesh_contact: a convex shape against a mesh or height field aephysics.mesh_contact, the world-free part of Box3D's mesh_contact.c in Aether: the triangles under the convex shape's bounds cached and refreshed only when the shape leaves the bounds they were queried with, each triangle keeping its simplex and separating-axis warm starts across a refresh; a manifold per triangle in the convex shape's frame; the acceptance rules that stop a shape crossing an interior edge or vertex from colliding with the seam (a triangle face always, a hull face when it lies with the triangle or is deep, the rest tentative -- spheres by nearest-first feature ownership, hulls and capsules skipping only the flat edges an accepted triangle owns); clusters by manifold and triangle normal within cos 5 degrees; and the four-point cull (the farthest pair, the largest triangle, the most area outside). The results live in the caller's arena. What the reference does next (matching the old manifolds for warm starting, the materials, rolling resistance and tangent velocity) is the contact's and comes with the dynamics. test_mesh_contact.ae (ours; the reference tests this through its world): the cull on placed points, a box on a grid mesh with the cache kept and refreshed, a sphere on an interior vertex and edge (one point), a capsule across triangles, a box on a height field and on a ridge, and the mesh under a transform. 64 checks. Two reference quirks kept and noted: the cull's tie rule and a hull face's far clip points. bench/mesh_contact.ae runs alone (the reference's needs its world): a box dragged across a wave at 3.2 us a step, a sphere at 0.7 us. --- README.md | 1 + aephysics/mesh_contact/module.ae | 710 +++++++++++++++++++++++++++++++ aephysics/test_mesh_contact.ae | 406 ++++++++++++++++++ bench/RESULTS.md | 23 + bench/mesh_contact.ae | 98 +++++ design.md | 23 +- 6 files changed, 1256 insertions(+), 5 deletions(-) create mode 100644 aephysics/mesh_contact/module.ae create mode 100644 aephysics/test_mesh_contact.ae create mode 100644 bench/mesh_contact.ae diff --git a/README.md b/README.md index c20d4fd..9fb9781 100644 --- a/README.md +++ b/README.md @@ -41,6 +41,7 @@ so a test written against the reference reads the same here. | `aephysics.shape` | the shape of any kind (sphere, capsule, hull, mesh, height field, compound) with the dispatch over every kind under a transform: bounds, swept and fat bounds, centroid, areas, mass, extent, ray and shape casts, overlap, the mover's planes, the proxy; the collision filters | done, `test_shape.ae` (440 checks); [same results as the reference, rays and masses at parity, radius casts 2.5x](bench/RESULTS.md#shape) | | `aephysics.mover` | the character mover's plane solver: pushes accumulated and clamped over twenty sweeps, the velocity clip | done, `test_mover.ae` (56 checks); [same results as the reference, 0.9x its time](bench/RESULTS.md#mover) | | `aephysics.broad_phase` | the broad phase: a tree per body type, proxies keyed by type, the pair update through the moved siblings and cross-tree seeds with the filter and compound lookups as visitors, the pair set, the keys sorted | done, `test_broad_phase.ae` (36 checks against a brute force); [10,000 moving boxes at 4.6 ms a step](bench/RESULTS.md#broad_phase) | +| `aephysics.mesh_contact` | a convex shape against a mesh or height field: the triangle cache with per-triangle warm starts, the manifolds per triangle, the seam rules against ghost collisions, clusters by normal, the four-point cull | done, `test_mesh_contact.ae` (64 checks); [a box on a wave at 3.2 us a step](bench/RESULTS.md#mesh_contact) | | `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/mesh_contact/module.ae b/aephysics/mesh_contact/module.ae new file mode 100644 index 0000000..293d6f2 --- /dev/null +++ b/aephysics/mesh_contact/module.ae @@ -0,0 +1,710 @@ +// aephysics.mesh_contact -- a convex shape against a mesh or a height +// field: the triangles under the shape's bounds are cached (refreshed +// only when the shape leaves the bounds it was queried with, each +// triangle keeping its own simplex or separating-axis cache), every +// triangle gives a manifold in the shape's frame, the manifolds are +// accepted or held back so a shape crossing an interior edge or vertex +// never collides with the seam (the ghost collision), the accepted +// manifolds are clustered by their normals, and each cluster is reduced +// to four points spanning the largest area. +// +// This is the world-free part of Box3D's mesh_contact.c (Erin Catto, +// MIT), the reference this engine is measured against: the cache +// matching by sorted triangle index, the acceptance rules (a triangle +// face always; a hull face when it lies with the triangle or is deep; +// the rest tentative, spheres taking the nearest tentative triangles +// whose feature no accepted triangle already owns, hulls and capsules +// skipping only the flat edges an accepted triangle owns), the cluster +// threshold of cos 5 degrees, and the four-point cull by farthest pair, +// largest triangle, most area outside. Names are the reference's +// without its prefix, in snake case: b3ComputeMeshManifolds is +// compute_mesh_manifolds. What the reference does after the clusters +// (matching the old manifolds for warm starting, the materials' +// friction and restitution, the rolling resistance and tangent +// velocity) belongs to the contact and comes with the dynamics. +// +// Differences: the per-triangle cache holds both a simplex cache and a +// separating-axis cache where the reference has a union; the results +// live in the caller's arena from where this function started bumping. +import std.string +import aephysics.math +import aephysics.core +import aephysics.hull +import aephysics.distance +import aephysics.manifold +import aephysics.triangle_manifold +import aephysics.mesh +import aephysics.height_field +import aephysics.material +import aephysics.sphere +import aephysics.capsule +import aephysics.compound +import aephysics.shape + +exports ( + TriangleCache, MeshContactCache, Cluster, MeshManifolds, + MAX_MESH_CONTACT_TRIANGLES, MAX_POINTS_PER_TRIANGLE, MAX_MANIFOLD_POINTS, MESH_REST_OFFSET, NULL_INDEX, + mesh_contact_cache_create, mesh_contact_cache_destroy, refresh_cache, compute_mesh_manifolds, + cull_points, reduce_cluster +) + +extern memcpy(dst: ptr, src: ptr, size: int) -> ptr + +const NULL_INDEX = 0 - 1 +const MAX_MESH_CONTACT_TRIANGLES = 256 +const MAX_POINTS_PER_TRIANGLE = 32 +const MAX_MANIFOLD_POINTS = 4 +const MAX_EDGE_COUNT = 64 +const MAX_VERTEX_COUNT = 64 +// The gap left after a time of impact (the "rest offset"). +const MESH_REST_OFFSET = 0.005 +const MAX_AABB_MARGIN = 0.05 +const SPECULATIVE_DISTANCE = 0.02 +// A triangle's manifold and one accepted for the reduction: the +// triangle_manifold module's features and the manifold module's axes, +// restated (a module constant does not reach every expression). +const FEATURE_NONE = 0 +const FEATURE_TRIANGLE_FACE = 1 +const FEATURE_HULL_FACE = 2 +const FEATURE_EDGE1 = 3 +const FEATURE_EDGE2 = 4 +const FEATURE_EDGE3 = 5 +const FEATURE_VERTEX1 = 6 +const FEATURE_VERTEX2 = 7 +const FEATURE_VERTEX3 = 8 +const AXIS_EDGE_PAIR = 4 +const FLAT_EDGE1 = 0x11 +const FLAT_EDGE2 = 0x22 +const FLAT_EDGE3 = 0x44 +const ALL_FLAT_EDGES = 0x77 +const KIND_SPHERE = 0 +const KIND_CAPSULE = 1 +const KIND_HULL = 2 +const KIND_MESH = 3 +const KIND_HEIGHT_FIELD = 4 + +// A triangle under the shape and its warm start. +struct TriangleCache { + triangle_index: int + simplex_cache: SimplexCache // a capsule's + sat_cache: SATCache // a hull's +} + +// The contact's memory of the mesh: the bounds the triangles were +// queried with and the triangles, sorted by index. +struct MeshContactCache { + query_bounds: AABB + triangles: ptr // TriangleCache[] + count: int + capacity: int +} + +// A cluster of accepted manifolds with alike normals, reduced. +struct Cluster { + manifold_normal: Vec3 // in the convex shape's frame + triangle_normal: Vec3 + points: ptr // LocalManifoldPoint[], in the convex shape's frame + point_capacity: int + point_count: int +} + +// What compute_mesh_manifolds leaves in the arena. +struct MeshManifolds { + clusters: ptr // Cluster[] + cluster_count: int + sat_calls: int // diagnostics: hull collisions, and how many the cache answered + sat_cache_hits: int +} + +mesh_contact_cache_create() -> MeshContactCache { + return MeshContactCache { query_bounds: math.aabb_empty(), triangles: null, count: 0, capacity: 0 } +} + +mesh_contact_cache_destroy(cache: *MeshContactCache) { + core.free_bytes(cache.triangles, cache.capacity * sizeof(TriangleCache)) + cache.triangles = null + cache.count = 0 + cache.capacity = 0 +} + +empty_triangle_cache(triangle_index: int) -> TriangleCache { + return TriangleCache { triangle_index: triangle_index, simplex_cache: distance.empty_cache(), sat_cache: manifold.empty_sat_cache() } +} + +// --- the triangle cache ------------------------------------------------------------------------ + +struct TriangleQuery { + indices: ptr // int[] + capacity: int + count: int +} + +collect_triangle(a: Vec3, b: Vec3, c: Vec3, triangle_index: int, context: ptr) -> bool { + q = context as *TriangleQuery + if q.count == q.capacity { return false } + indices = q.indices as int[] + indices[q.count] = triangle_index + q.count = q.count + 1 + return q.count < q.capacity +} + +var g_indices: ptr = null // int[MAX_MESH_CONTACT_TRIANGLES] +var g_matched: ptr = null // TriangleCache[MAX_MESH_CONTACT_TRIANGLES] +var g_edges: ptr = null // long[MAX_EDGE_COUNT] +var g_vertices: ptr = null // int[MAX_VERTEX_COUNT] +var g_triangle: ptr = null // Vec3[3] + +scratch_ready() { + if g_indices == null { + g_indices = core.alloc(MAX_MESH_CONTACT_TRIANGLES * 4) + g_matched = core.alloc(MAX_MESH_CONTACT_TRIANGLES * sizeof(TriangleCache)) + g_edges = core.alloc(MAX_EDGE_COUNT * 8) + g_vertices = core.alloc(MAX_VERTEX_COUNT * 4) + g_triangle = core.alloc(3 * sizeof(Vec3)) + } +} + +// The triangles under the convex shape's world bounds, kept while the +// shape stays inside the bounds they were last queried with (enlarged +// by the margin and the speculative distance); a refresh keeps each +// surviving triangle's warm start. +refresh_cache(cache: *MeshContactCache, mesh_shape: *Shape, xf_a: Transform, bounds: AABB) { + if math.aabb_contains(cache.query_bounds, bounds) { return } + scratch_ready() + radius = MAX_AABB_MARGIN + SPECULATIVE_DISTANCE + extension = math.vec3(radius, radius, radius) + cache.query_bounds = AABB { lower: math.sub(bounds.lower, extension), upper: math.add(bounds.upper, extension) } + local_bounds = math.aabb_transform(math.invert_transform(xf_a), cache.query_bounds) + q = TriangleQuery { indices: g_indices, capacity: MAX_MESH_CONTACT_TRIANGLES, count: 0 } + if mesh_shape.kind == KIND_MESH { + mesh.query_mesh(mesh_shape.mesh, local_bounds, collect_triangle, (&q) as ptr) + } else { + height_field.query_height_field(mesh_shape.height_field, local_bounds, collect_triangle, (&q) as ptr) + } + indices = g_indices as int[] + triangle_count = q.count + // The indices arrive sorted (both queries walk in index order), so + // the old cache is matched in one pass. + old = cache.triangles as TriangleCache[] + matched = g_matched as TriangleCache[] + index2 = 0 + index1 = 0 + while index1 < triangle_count { + matched[index1] = empty_triangle_cache(indices[index1]) + while index2 < cache.count && old[index2].triangle_index < indices[index1] { index2 = index2 + 1 } + if index2 < cache.count && old[index2].triangle_index == indices[index1] { + matched[index1].simplex_cache = old[index2].simplex_cache + matched[index1].sat_cache = old[index2].sat_cache + } + index1 = index1 + 1 + } + if triangle_count > cache.capacity { + core.free_bytes(cache.triangles, cache.capacity * sizeof(TriangleCache)) + cache.capacity = math.max_int(triangle_count, 8) + cache.triangles = core.alloc(cache.capacity * sizeof(TriangleCache)) + } + if triangle_count > 0 { memcpy(cache.triangles, g_matched, triangle_count * sizeof(TriangleCache)) } + cache.count = triangle_count +} + +// --- the seams ------------------------------------------------------------------------------------- + +// The edges and vertices the accepted triangles own, so a tentative +// triangle cannot collide with a seam twice. Full lists let a ghost +// through rather than dropping a contact. +struct Found { + edge_count: int + vertex_count: int +} + +edge_key(vertex1: int, vertex2: int) -> long { + return ((math.min_int(vertex1, vertex2) as long) << 32) | (math.max_int(vertex1, vertex2) as long) +} + +// True when the edge was not there (and is now, if there was room). +add_edge(found: *Found, vertex1: int, vertex2: int) -> bool { + key = edge_key(vertex1, vertex2) + keys = g_edges as long[] + i = 0 + while i < found.edge_count { + if keys[i] == key { return false } + i = i + 1 + } + if found.edge_count == MAX_EDGE_COUNT { return true } + keys[found.edge_count] = key + found.edge_count = found.edge_count + 1 + return true +} + +find_edge(found: *Found, vertex1: int, vertex2: int) -> bool { + key = edge_key(vertex1, vertex2) + keys = g_edges as long[] + i = 0 + while i < found.edge_count { + if keys[i] == key { return true } + i = i + 1 + } + return false +} + +add_vertex(found: *Found, vertex: int) -> bool { + keys = g_vertices as int[] + i = 0 + while i < found.vertex_count { + if keys[i] == vertex { return false } + i = i + 1 + } + if found.vertex_count == MAX_VERTEX_COUNT { return true } + keys[found.vertex_count] = vertex + found.vertex_count = found.vertex_count + 1 + return true +} + +add_triangle(found: *Found, i1: int, i2: int, i3: int) { + add_edge(found, i1, i2) + add_edge(found, i2, i3) + add_edge(found, i3, i1) + add_vertex(found, i1) + add_vertex(found, i2) + add_vertex(found, i3) +} + +// --- the reduction --------------------------------------------------------------------------------- + +// Whether (score, separation) beats (best score, best separation): a +// clearly larger score, or a tie broken by the deeper separation. +is_better_cull_candidate(score: float, separation: float, best_score: float, best_separation: float, score_tol: float, separation_tol: float) -> bool { + if score > best_score + score_tol { return true } + if score < best_score - score_tol { return false } + return separation < best_separation - separation_tol +} + +var g_final: ptr = null // Point2D[4] + +// Up to four of the points, in place: the farthest pair, then the point +// making the largest triangle with them, then the point adding the most +// area outside that triangle; ties to the deeper point. Returns how many. +cull_points(points: Point2D[], count: int) -> int { + if count <= 1 { return count } + if g_final == null { g_final = core.alloc(4 * sizeof(Point2D)) } + final = g_final as Point2D[] + tol = 0.25 * math.LINEAR_SLOP + tol_sqr = tol * tol + separation_tol = math.LINEAR_SLOP + count1 = count + + // The two farthest apart. + best_score = 0.0 + best_separation = math.MAX_FLOAT + best_index1 = NULL_INDEX + best_index2 = NULL_INDEX + i = 0 + while i < count1 { + p1 = points[i].p + j = i + 1 + while j < count1 { + score = math.distance_squared_2(p1, points[j].p) + separation = points[i].separation + points[j].separation + if is_better_cull_candidate(score, separation, best_score, best_separation, tol_sqr, separation_tol) { + best_index1 = i + best_index2 = j + best_score = score + best_separation = separation + } + j = j + 1 + } + i = i + 1 + } + if best_score < tol_sqr { + // All in one place: the deepest. + deepest = 0 + i = 1 + while i < count1 { + if points[i].separation < points[deepest].separation { deepest = i } + i = i + 1 + } + if deepest != 0 { points[0] = points[deepest] } + return 1 + } + final[0] = points[best_index1] + final[1] = points[best_index2] + points[best_index2] = points[count1 - 1] + points[best_index1] = points[count1 - 2] + count1 = count1 - 2 + if count1 == 0 { + points[0] = final[0] + points[1] = final[1] + return 2 + } + + // The largest triangle on the pair. + a = final[0].p + b = final[1].p + ba = math.vec2(b.x - a.x, b.y - a.y) + best_score = 0.0 + best_separation = math.MAX_FLOAT + best_index = NULL_INDEX + best_signed_area = 0.0 + i = 0 + while i < count1 { + p = points[i].p + signed_area = ba.x * (p.y - a.y) - ba.y * (p.x - a.x) + score = math.abs_float(signed_area) + if is_better_cull_candidate(score, points[i].separation, best_score, best_separation, tol_sqr, separation_tol) { + best_signed_area = signed_area + best_score = score + best_separation = points[i].separation + best_index = i + } + i = i + 1 + } + if best_index == NULL_INDEX { + // All collinear. + points[0] = final[0] + points[1] = final[1] + return 2 + } + final[2] = points[best_index] + if count1 == 1 { + points[0] = final[0] + points[1] = final[1] + points[2] = final[2] + return 3 + } + points[best_index] = points[count1 - 1] + count1 = count1 - 1 + + // The point adding the most area outside the triangle, wound counter-clockwise. + c = final[2].p + if best_signed_area < 0.0 { + swap = b + b = c + c = swap + ba = math.vec2(b.x - a.x, b.y - a.y) + } + cb = math.vec2(c.x - b.x, c.y - b.y) + ac = math.vec2(a.x - c.x, a.y - c.y) + best_score = 0.0 + best_separation = math.MAX_FLOAT + best_index = NULL_INDEX + i = 0 + while i < count1 { + p = points[i].p + u1 = (p.x - a.x) * ba.y - (p.y - a.y) * ba.x + u2 = (p.x - b.x) * cb.y - (p.y - b.y) * cb.x + u3 = (p.x - c.x) * ac.y - (p.y - c.y) * ac.x + score = math.max_float(u1, math.max_float(u2, u3)) + if is_better_cull_candidate(score, points[i].separation, best_score, best_separation, tol_sqr, separation_tol) { + best_score = score + best_separation = points[i].separation + best_index = i + } + i = i + 1 + } + if best_index == NULL_INDEX { + points[0] = final[0] + points[1] = final[1] + points[2] = final[2] + return 3 + } + final[3] = points[best_index] + points[0] = final[0] + points[1] = final[1] + points[2] = final[2] + points[3] = final[3] + return 4 +} + +// A cluster's points projected onto the plane of the triangle normal, +// culled to four, and the survivors moved to the front. Returns how many. +reduce_cluster(points: LocalManifoldPoint[], count: int, normal: Vec3, arena: *Arena) -> int { + if count <= 1 { return count } + pts_block = core.arena_bump(arena, count * sizeof(Point2D)) + pts = pts_block as Point2D[] + u = math.perp(normal) + v = math.cross(normal, u) + origin = points[0].point + i = 0 + while i < count { + d = math.sub(points[i].point, origin) + pts[i] = Point2D { p: math.vec2(math.dot(d, u), math.dot(d, v)), separation: points[i].separation, original_index: i } + i = i + 1 + } + count2 = cull_points(pts, count) + final_block = core.arena_bump(arena, MAX_MANIFOLD_POINTS * sizeof(LocalManifoldPoint)) + final = final_block as LocalManifoldPoint[] + i = 0 + while i < count2 { + final[i] = points[pts[i].original_index] + i = i + 1 + } + i = 0 + while i < count2 { + points[i] = final[i] + i = i + 1 + } + return count2 +} + +// --- the manifolds --------------------------------------------------------------------------------- + +struct Tentative { + squared_distance: float + index: int +} + +// The clusters of a convex shape (a sphere, capsule or hull, with its +// transform) against a mesh or height field shape (with its transform), +// through the contact's triangle cache; bounds_b is the convex shape's +// world bounds. Everything is computed in the convex shape's frame. The +// clusters and their points live in the arena from where it stood at +// the call; the caller reads them and winds the arena back. is_fast +// drops a cached edge axis (a fast hull can turn around an edge and +// tunnel); enable_speculative keeps points ahead of contact. +compute_mesh_manifolds(cache: *MeshContactCache, mesh_shape: *Shape, xf_a: Transform, convex: *Shape, xf_b: Transform, + bounds_b: AABB, is_fast: bool, enable_speculative: bool, arena: *Arena) -> MeshManifolds { + scratch_ready() + result = MeshManifolds { clusters: null, cluster_count: 0, sat_calls: 0, sat_cache_hits: 0 } + refresh_cache(cache, mesh_shape, xf_a, bounds_b) + triangle_count = cache.count + if triangle_count == 0 { return result } + + accepted_block = core.arena_bump(arena, triangle_count * 4) + accepted = accepted_block as int[] // manifold indices + accepted_count = 0 + tentative_block = core.arena_bump(arena, triangle_count * 4) + tentative = tentative_block as int[] + tentative_count = 0 + tentative_triangles_block = core.arena_bump(arena, triangle_count * sizeof(Tentative)) + tentative_triangles = tentative_triangles_block as Tentative[] + found = Found { edge_count: 0, vertex_count: 0 } + + // From the mesh's frame into the convex shape's. + transform_a_to_b = math.inv_mul_transforms(xf_b, xf_a) + relative = math.make_matrix_from_quat(transform_a_to_b.q) + linear_slop = math.LINEAR_SLOP + + point_capacity_total = MAX_POINTS_PER_TRIANGLE * triangle_count + point_buffer = core.arena_bump(arena, point_capacity_total * sizeof(LocalManifoldPoint)) + total_point_count = 0 + manifold_buffer = core.arena_bump(arena, triangle_count * sizeof(LocalManifold)) + manifolds = manifold_buffer as LocalManifold[] + manifold_count = 0 + kind_b = convex.kind + tri = g_triangle as Vec3[] + + index = 0 + while index < triangle_count && total_point_count + 3 < point_capacity_total { + tc = (cache.triangles + index * sizeof(TriangleCache)) as *TriangleCache + triangle_index = tc.triangle_index + triangle = Triangle { v1: math.vec3_zero(), v2: math.vec3_zero(), v3: math.vec3_zero(), i1: 0, i2: 0, i3: 0, flags: 0 } + if mesh_shape.kind == KIND_MESH { + triangle = mesh.get_mesh_triangle(mesh_shape.mesh, triangle_index) + } else { + triangle = height_field.get_height_field_triangle(mesh_shape.height_field, triangle_index) + } + tri[0] = math.add(math.mul_mv(relative, triangle.v1), transform_a_to_b.p) + tri[1] = math.add(math.mul_mv(relative, triangle.v2), transform_a_to_b.p) + tri[2] = math.add(math.mul_mv(relative, triangle.v3), transform_a_to_b.p) + + point_capacity = point_capacity_total - total_point_count + m = (manifold_buffer + manifold_count * sizeof(LocalManifold)) as *LocalManifold + mm = manifold.local_manifold(point_buffer + total_point_count * sizeof(LocalManifoldPoint)) + mm.triangle_flags = triangle.flags + manifolds[manifold_count] = mm + if kind_b == KIND_CAPSULE { + triangle_manifold.collide_triangle_and_capsule(m, point_capacity, tri[0], tri[1], tri[2], convex.capsule, &tc.simplex_cache) + } else if kind_b == KIND_HULL { + // A cached edge pair is dangerous at speed: the hull can turn around the edge and tunnel. + if is_fast && tc.sat_cache.kind == AXIS_EDGE_PAIR { manifold.clear_sat_cache(&tc.sat_cache) } + triangle_manifold.collide_triangle_and_hull(m, point_capacity, tri[0], tri[1], tri[2], triangle.flags, convex.hull, + &tc.sat_cache, enable_speculative) + result.sat_calls = result.sat_calls + 1 + result.sat_cache_hits = result.sat_cache_hits + tc.sat_cache.hit + } else { + triangle_manifold.collide_triangle_and_sphere(m, point_capacity, tri[0], tri[1], tri[2], convex.sphere) + } + + point_count = m.point_count + if point_count > 0 { + manifold_count = manifold_count + 1 + total_point_count = total_point_count + point_count + m.triangle_index = triangle_index + m.triangle_normal = math.make_normal_from_points(tri[0], tri[1], tri[2]) + m.i1 = triangle.i1 + m.i2 = triangle.i2 + m.i3 = triangle.i3 + this_manifold = manifold_count - 1 + if m.feature == FEATURE_TRIANGLE_FACE { + add_triangle(&found, triangle.i1, triangle.i2, triangle.i3) + accepted[accepted_count] = this_manifold + accepted_count = accepted_count + 1 + } else if m.feature == FEATURE_HULL_FACE { + cos_normal_angle = math.dot(m.triangle_normal, m.normal) + if cos_normal_angle > 0.5 { + add_triangle(&found, triangle.i1, triangle.i2, triangle.i3) + accepted[accepted_count] = this_manifold + accepted_count = accepted_count + 1 + } else { + min_separation = manifold.manifold_point(m, 0).separation + i = 1 + while i < point_count { + min_separation = math.min_float(min_separation, manifold.manifold_point(m, i).separation) + i = i + 1 + } + if min_separation < 0.0 - 2.0 * linear_slop { + // Deep: accepted whatever the angle. + add_triangle(&found, triangle.i1, triangle.i2, triangle.i3) + accepted[accepted_count] = this_manifold + accepted_count = accepted_count + 1 + } else { + tentative_triangles[tentative_count] = Tentative { squared_distance: m.squared_distance, index: tentative_count } + tentative[tentative_count] = this_manifold + tentative_count = tentative_count + 1 + } + } + } else { + tentative_triangles[tentative_count] = Tentative { squared_distance: m.squared_distance, index: tentative_count } + tentative[tentative_count] = this_manifold + tentative_count = tentative_count + 1 + } + } + index = index + 1 + } + + if kind_b == KIND_SPHERE { + // The nearest tentative triangles first; each collides only when + // no accepted triangle owns the feature it touched. + sort_tentative(tentative_triangles, tentative_count) + i = 0 + while i < tentative_count { + m = (manifold_buffer + tentative[tentative_triangles[i].index] * sizeof(LocalManifold)) as *LocalManifold + added_edge1 = add_edge(&found, m.i1, m.i2) + added_edge2 = add_edge(&found, m.i2, m.i3) + added_edge3 = add_edge(&found, m.i3, m.i1) + added_vertex1 = add_vertex(&found, m.i1) + added_vertex2 = add_vertex(&found, m.i2) + added_vertex3 = add_vertex(&found, m.i3) + should_collide = false + feature = m.feature + if feature == FEATURE_EDGE1 { should_collide = added_edge1 } + else if feature == FEATURE_EDGE2 { should_collide = added_edge2 } + else if feature == FEATURE_EDGE3 { should_collide = added_edge3 } + else if feature == FEATURE_VERTEX1 { should_collide = added_vertex1 } + else if feature == FEATURE_VERTEX2 { should_collide = added_vertex2 } + else if feature == FEATURE_VERTEX3 { should_collide = added_vertex3 } + if should_collide { + accepted[accepted_count] = tentative[tentative_triangles[i].index] + accepted_count = accepted_count + 1 + } + i = i + 1 + } + } else { + // A hull can tunnel at a concave edge (a flat box sliding down a + // ramp onto the floor), so only the flat edges an accepted + // triangle owns are skipped. + i = 0 + while i < tentative_count { + m = (manifold_buffer + tentative[i] * sizeof(LocalManifold)) as *LocalManifold + flags = m.triangle_flags + skip = false + if (flags & ALL_FLAT_EDGES) == ALL_FLAT_EDGES { skip = true } + if skip == false && (flags & FLAT_EDGE1) == FLAT_EDGE1 && find_edge(&found, m.i1, m.i2) { skip = true } + if skip == false && (flags & FLAT_EDGE2) == FLAT_EDGE2 && find_edge(&found, m.i2, m.i3) { skip = true } + if skip == false && (flags & FLAT_EDGE3) == FLAT_EDGE3 && find_edge(&found, m.i3, m.i1) { skip = true } + if skip == false { + accepted[accepted_count] = tentative[i] + accepted_count = accepted_count + 1 + } + i = i + 1 + } + } + if accepted_count == 0 { return result } + + // Clusters by the manifold and triangle normals, within cos 5 degrees + // (tighter than the warm start's matching); the first fit is taken. + clusters_block = core.arena_bump(arena, accepted_count * sizeof(Cluster)) + clusters = clusters_block as Cluster[] + memberships_block = core.arena_bump(arena, accepted_count * 4) + memberships = memberships_block as int[] + cluster_threshold = 0.996 + cluster_count = 0 + cluster_point_count = 0 + i = 0 + while i < accepted_count { + memberships[i] = NULL_INDEX + m = (manifold_buffer + accepted[i] * sizeof(LocalManifold)) as *LocalManifold + cluster_point_count = cluster_point_count + m.point_count + manifold_normal = m.normal + triangle_normal = m.triangle_normal + cluster_index = NULL_INDEX + j = 0 + while j < cluster_count && cluster_index == NULL_INDEX { + if math.dot(clusters[j].manifold_normal, manifold_normal) > cluster_threshold && + math.dot(clusters[j].triangle_normal, triangle_normal) > cluster_threshold { + cluster_index = j + } + j = j + 1 + } + if cluster_index != NULL_INDEX { + memberships[i] = cluster_index + clusters[cluster_index].point_capacity = clusters[cluster_index].point_capacity + m.point_count + } else { + clusters[cluster_count] = Cluster { manifold_normal: manifold_normal, triangle_normal: triangle_normal, points: null, + point_capacity: m.point_count, point_count: 0 } + memberships[i] = cluster_count + cluster_count = cluster_count + 1 + } + i = i + 1 + } + if cluster_point_count == 0 { return result } + + cluster_points = core.arena_bump(arena, cluster_point_count * sizeof(LocalManifoldPoint)) + point_offset = 0 + i = 0 + while i < cluster_count { + clusters[i].points = cluster_points + point_offset * sizeof(LocalManifoldPoint) + clusters[i].point_count = 0 + point_offset = point_offset + clusters[i].point_capacity + i = i + 1 + } + i = 0 + while i < accepted_count { + cluster_index = memberships[i] + m = (manifold_buffer + accepted[i] * sizeof(LocalManifold)) as *LocalManifold + target = clusters[cluster_index].points as LocalManifoldPoint[] + j = 0 + while j < m.point_count { + p = manifold.manifold_point(m, j) + p.triangle_index = m.triangle_index + target[clusters[cluster_index].point_count] = p + clusters[cluster_index].point_count = clusters[cluster_index].point_count + 1 + j = j + 1 + } + i = i + 1 + } + i = 0 + while i < cluster_count { + clusters[i].point_count = reduce_cluster(clusters[i].points as LocalManifoldPoint[], clusters[i].point_count, clusters[i].triangle_normal, arena) + i = i + 1 + } + result.clusters = clusters_block + result.cluster_count = cluster_count + return result +} + +// The tentative triangles by squared distance, an insertion sort (the +// reference's qsort; the counts are small). +sort_tentative(items: Tentative[], count: int) { + i = 1 + while i < count { + v = items[i] + j = i - 1 + while j >= 0 && items[j].squared_distance > v.squared_distance { + items[j + 1] = items[j] + j = j - 1 + } + items[j + 1] = v + i = i + 1 + } +} diff --git a/aephysics/test_mesh_contact.ae b/aephysics/test_mesh_contact.ae new file mode 100644 index 0000000..3d66e2d --- /dev/null +++ b/aephysics/test_mesh_contact.ae @@ -0,0 +1,406 @@ +// aephysics.mesh_contact: the reference tests its mesh contact through +// its world, so this one is ours: the four-point cull on hand-placed +// points, a box resting on a grid mesh (one cluster of four points, the +// cache kept while the box stays in its query bounds and its warm start +// surviving a refresh), a sphere on an interior edge and on an interior +// vertex (one point, no ghost), a sphere touching a seam from one +// triangle only, a capsule lying across triangles, a box on a height +// field, a box straddling a ridge (two clusters), and the mesh under a +// transform. + +import std.string +import aephysics.math +import aephysics.core +import aephysics.hull +import aephysics.distance +import aephysics.manifold +import aephysics.triangle_manifold +import aephysics.mesh +import aephysics.height_field +import aephysics.material +import aephysics.sphere +import aephysics.capsule +import aephysics.compound +import aephysics.shape +import aephysics.mesh_contact + +extern calloc(count: int, size: int) -> ptr +extern exit(code: int) +extern free(p: ptr) + +var failures = 0 +var checks = 0 + +ensure(name: string, ok: bool) { + checks = checks + 1 + if !ok { + println("mesh_contact: FAIL ${name}") + failures = failures + 1 + } +} + +small(name: string, value: float, tolerance: float) { + ensure("${name} (${value})", math.abs_float(value) < tolerance) +} + +var g_arena_block: ptr = null + +arena() -> *Arena { + if g_arena_block == null { + g_arena_block = calloc(1, sizeof(Arena)) + a = g_arena_block as *Arena + made = core.arena_create(1 << 20) + a.memory = made.memory + a.capacity = made.capacity + a.index = 0 + a.max_index = 0 + a.overflows = made.overflows + a.overflow_bytes = 0 + a.peak_demand = 0 + } + a = g_arena_block as *Arena + a.index = 0 + return a +} + +// One update of a convex shape against a mesh shape. +manifolds(cache: *MeshContactCache, mesh_shape: *Shape, xf_a: Transform, convex: *Shape, xf_b: Transform, is_fast: bool) -> MeshManifolds { + bounds = shape.compute_shape_aabb(convex, xf_b) + return mesh_contact.compute_mesh_manifolds(cache, mesh_shape, xf_a, convex, xf_b, bounds, is_fast, true, arena()) +} + +cluster(r: MeshManifolds, i: int) -> Cluster { + clusters = r.clusters as Cluster[] + return clusters[i] +} + +cluster_point(c: Cluster, i: int) -> LocalManifoldPoint { + points = c.points as LocalManifoldPoint[] + return points[i] +} + +// Every point of every cluster lies in the convex shape's frame within +// the tolerance of the plane through the cluster's normal. +min_separation(r: MeshManifolds) -> float { + lowest = math.MAX_FLOAT + i = 0 + while i < r.cluster_count { + c = cluster(r, i) + j = 0 + while j < c.point_count { + lowest = math.min_float(lowest, cluster_point(c, j).separation) + j = j + 1 + } + i = i + 1 + } + return lowest +} + +test_cull() { + block = calloc(8, sizeof(Point2D)) + points = block as Point2D[] + // A square's corners with two interior points: the four corners survive. + points[0] = Point2D { p: math.vec2(0.0, 0.0), separation: 0.0, original_index: 0 } + points[1] = Point2D { p: math.vec2(0.4, 0.5), separation: 0.0, original_index: 1 } + points[2] = Point2D { p: math.vec2(1.0, 0.0), separation: 0.0, original_index: 2 } + points[3] = Point2D { p: math.vec2(1.0, 1.0), separation: 0.0, original_index: 3 } + points[4] = Point2D { p: math.vec2(0.6, 0.4), separation: 0.0, original_index: 4 } + points[5] = Point2D { p: math.vec2(0.0, 1.0), separation: 0.0, original_index: 5 } + n = mesh_contact.cull_points(points, 6) + ensure("cull keeps four", n == 4) + corners = 0 + i = 0 + while i < n { + o = points[i].original_index + if o == 0 || o == 2 || o == 3 || o == 5 { corners = corners + 1 } + i = i + 1 + } + ensure("cull keeps the corners", corners == 4) + // Collinear points: the two ends lead. (The reference's tie rule lets + // a zero-area point through on its separation, so more may follow.) + i = 0 + while i < 5 { + points[i] = Point2D { p: math.vec2(0.2 * (i as float), 0.0), separation: 0.0, original_index: i } + i = i + 1 + } + n = mesh_contact.cull_points(points, 5) + ensure("collinear keeps the ends", n >= 2 && ((points[0].original_index == 0 && points[1].original_index == 4) || (points[0].original_index == 4 && points[1].original_index == 0))) + // Coincident points: the deepest. + i = 0 + while i < 4 { + points[i] = Point2D { p: math.vec2(0.0, 0.0), separation: 0.0 - 0.1 * (i as float), original_index: i } + i = i + 1 + } + n = mesh_contact.cull_points(points, 4) + ensure("coincident keeps the deepest", n == 1 && points[0].original_index == 3) + // A triangle: three. + points[0] = Point2D { p: math.vec2(0.0, 0.0), separation: 0.0, original_index: 0 } + points[1] = Point2D { p: math.vec2(1.0, 0.0), separation: 0.0, original_index: 1 } + points[2] = Point2D { p: math.vec2(0.0, 1.0), separation: 0.0, original_index: 2 } + ensure("triangle keeps three", mesh_contact.cull_points(points, 3) == 3) + ensure("one stays one", mesh_contact.cull_points(points, 1) == 1) + ensure("none stays none", mesh_contact.cull_points(points, 0) == 0) + // A tie on distance broken by the deeper pair. + points[0] = Point2D { p: math.vec2(0.0, 0.0), separation: 0.0, original_index: 0 } + points[1] = Point2D { p: math.vec2(1.0, 0.0), separation: 0.0, original_index: 1 } + points[2] = Point2D { p: math.vec2(0.0, 0.0), separation: 0.0 - 0.1, original_index: 2 } + points[3] = Point2D { p: math.vec2(1.0, 0.0), separation: 0.0 - 0.1, original_index: 3 } + n = mesh_contact.cull_points(points, 4) + ensure("tie goes to the deeper", n >= 2 && ((points[0].original_index == 2 && points[1].original_index == 3) || (points[0].original_index == 3 && points[1].original_index == 2))) + free(block) +} + +test_box_on_grid() { + grid = mesh.create_grid_mesh(8, 8, 1.0, 1, true) + ground = shape.mesh_shape(mesh.mesh(grid, math.vec3_one()), 1) + box = hull.make_box_hull(0.5, 0.5, 0.5) + b = shape.hull_shape(box, 1.0) + xf_a = math.transform_identity() + xf_b = Transform { p: math.vec3(0.3, 0.49, 0.2), q: math.quat_identity() } + cache = mesh_contact.mesh_contact_cache_create() + r = manifolds(&cache, &ground, xf_a, &b, xf_b, false) + ensure("box: triangles cached (${cache.count})", cache.count == 8) + ensure("box: one cluster (${r.cluster_count})", r.cluster_count == 1) + if r.cluster_count == 1 { + c = cluster(r, 0) + ensure("box: four points (${c.point_count})", c.point_count == 4) + small("box: normal up", c.manifold_normal.y - 1.0, 0.000001) + small("box: triangle normal up", c.triangle_normal.y - 1.0, 0.000001) + i = 0 + while i < c.point_count { + p = cluster_point(c, i) + small("box: point ${i} separation", p.separation + 0.01, 0.0001) + small("box: point ${i} on the bottom face", p.point.y + 0.5, 0.011) + ensure("box: point ${i} triangle", p.triangle_index >= 0 && p.triangle_index < grid.triangle_count) + i = i + 1 + } + // The four span the box's bottom: two at x = -0.5 and two at 0.5 in the box's frame. + left = 0 + i = 0 + while i < c.point_count { + if cluster_point(c, i).point.x < 0.0 { left = left + 1 } + i = i + 1 + } + ensure("box: points span the face", left == 2) + } + ensure("box: eight hull collisions", r.sat_calls == 8) + // The same again: the caches answer. + r = manifolds(&cache, &ground, xf_a, &b, xf_b, false) + ensure("box: cache hits on repeat (${r.sat_cache_hits})", r.sat_cache_hits > 0) + // A small move stays in the query bounds: the same triangles, the caches kept. + query_before = cache.query_bounds + xf_b.p = math.vec3(0.32, 0.49, 0.21) + r = manifolds(&cache, &ground, xf_a, &b, xf_b, false) + ensure("box: query bounds kept", cache.query_bounds.lower.x == query_before.lower.x && cache.count == 8) + ensure("box: still one cluster of four", r.cluster_count == 1 && cluster(r, 0).point_count == 4) + // A move out of the bounds refreshes; the surviving triangles keep their warm start. + xf_b.p = math.vec3(1.3, 0.49, 0.2) + r = manifolds(&cache, &ground, xf_a, &b, xf_b, false) + ensure("box: query bounds refreshed", cache.query_bounds.lower.x != query_before.lower.x) + ensure("box: refreshed to one cluster of four", r.cluster_count == 1 && cluster(r, 0).point_count == 4 && cache.count == 8) + // Lifted clear of the speculative band: nothing. + xf_b.p = math.vec3(1.3, 0.6, 0.2) + r = manifolds(&cache, &ground, xf_a, &b, xf_b, false) + ensure("box: lifted, no clusters", r.cluster_count == 0) + // Just above, within the speculative distance: points with positive separation. + xf_b.p = math.vec3(1.3, 0.51, 0.2) + r = manifolds(&cache, &ground, xf_a, &b, xf_b, false) + ensure("box: speculative points", r.cluster_count == 1) + if r.cluster_count == 1 { small("box: speculative separation", min_separation(r) - 0.01, 0.0001) } + // A tilted box resting on one edge: the two faces at the edge each + // give a hull-face manifold (the reference keeps the far clip points + // as speculative ones), so two clusters whose normals are the box's + // own x and y, each touching at the edge. + xf_b = Transform { p: math.vec3(2.0, 0.7071 - 0.01, 2.0), q: math.make_quat_from_axis_angle(math.vec3_axis_z(), 0.25 * math.PI) } + r = manifolds(&cache, &ground, xf_a, &b, xf_b, false) + ensure("tilted box: two clusters (${r.cluster_count})", r.cluster_count == 2) + if r.cluster_count == 2 { + n0 = cluster(r, 0).manifold_normal + n1 = cluster(r, 1).manifold_normal + ensure("tilted box: the faces at the edge", math.abs_float(n0.x + n1.x - 1.0) < 0.0001 && math.abs_float(n0.y + n1.y - 1.0) < 0.0001) + ensure("tilted box: touching (${min_separation(r)})", min_separation(r) < 0.0 && min_separation(r) > 0.0 - 0.02) + } + mesh_contact.mesh_contact_cache_destroy(&cache) + hull.destroy_hull(box) + mesh.destroy_mesh(grid) +} + +test_sphere_seams() { + grid = mesh.create_grid_mesh(8, 8, 1.0, 1, true) + ground = shape.mesh_shape(mesh.mesh(grid, math.vec3_one()), 1) + s = shape.sphere_shape(manifold.sphere(math.vec3_zero(), 0.5), 1.0) + xf_a = math.transform_identity() + cache = mesh_contact.mesh_contact_cache_create() + // On an interior vertex of the grid (a whole number of cells from the corner): one point. + xf_b = Transform { p: math.vec3(1.0, 0.49, 1.0), q: math.quat_identity() } + r = manifolds(&cache, &ground, xf_a, &s, xf_b, false) + ensure("sphere on a vertex: one cluster (${r.cluster_count})", r.cluster_count == 1) + if r.cluster_count == 1 { + c = cluster(r, 0) + ensure("sphere on a vertex: one point (${c.point_count})", c.point_count == 1) + small("sphere on a vertex: normal up", c.manifold_normal.y - 1.0, 0.0001) + small("sphere on a vertex: separation", cluster_point(c, 0).separation + 0.01, 0.0001) + } + // On an interior edge: one point, no ghost from the neighbour. + xf_b.p = math.vec3(1.5, 0.49, 1.0) + r = manifolds(&cache, &ground, xf_a, &s, xf_b, false) + ensure("sphere on an edge: one cluster", r.cluster_count == 1) + if r.cluster_count == 1 { ensure("sphere on an edge: one point", cluster(r, 0).point_count == 1) } + // Inside a triangle: one point on its face. + xf_b.p = math.vec3(1.2, 0.49, 1.7) + r = manifolds(&cache, &ground, xf_a, &s, xf_b, false) + ensure("sphere on a face: one cluster of one", r.cluster_count == 1 && cluster(r, 0).point_count == 1) + // A wave: the sphere in a valley touches two sloped faces meeting at a concave edge. + mesh.destroy_mesh(grid) + wave = mesh.create_wave_mesh(20, 20, 1.0, 0.5, 0.25, 0.0) + ground = shape.mesh_shape(mesh.mesh(wave, math.vec3_one()), 1) + mesh_contact.mesh_contact_cache_destroy(&cache) + cache = mesh_contact.mesh_contact_cache_create() + // Sampling the surface for the lowest point along x at the mesh's middle row. + lowest = math.MAX_FLOAT + lowest_x = 0.0 + x = 0.0 - 9.0 + while x < 9.0 { + out = mesh.ray_cast_mesh(mesh.mesh(wave, math.vec3_one()), math.vec3(x, 5.0, 0.25), math.vec3(0.0, 0.0 - 10.0, 0.0), 1.0) + if out.hit && out.point.y < lowest { + lowest = out.point.y + lowest_x = x + } + x = x + 0.05 + } + xf_b.p = math.vec3(lowest_x, lowest + 0.49, 0.25) + r = manifolds(&cache, &ground, xf_a, &s, xf_b, false) + ensure("sphere in a valley: clusters (${r.cluster_count})", r.cluster_count >= 1 && r.cluster_count <= 2) + ensure("sphere in a valley: touching", min_separation(r) < 0.02) + mesh_contact.mesh_contact_cache_destroy(&cache) + mesh.destroy_mesh(wave) +} + +test_capsule_and_field() { + grid = mesh.create_grid_mesh(8, 8, 1.0, 1, true) + ground = shape.mesh_shape(mesh.mesh(grid, math.vec3_one()), 1) + xf_a = math.transform_identity() + cache = mesh_contact.mesh_contact_cache_create() + // A capsule lying along x across three triangles: one cluster, two points at its ends. + c_shape = shape.capsule_shape(manifold.capsule(math.vec3(0.0 - 1.0, 0.0, 0.0), math.vec3(1.0, 0.0, 0.0), 0.3), 1.0) + xf_b = Transform { p: math.vec3(0.4, 0.29, 0.3), q: math.quat_identity() } + r = manifolds(&cache, &ground, xf_a, &c_shape, xf_b, false) + ensure("capsule: one cluster (${r.cluster_count})", r.cluster_count == 1) + if r.cluster_count == 1 { + c = cluster(r, 0) + ensure("capsule: two to four points (${c.point_count})", c.point_count >= 2 && c.point_count <= 4) + small("capsule: normal up", c.manifold_normal.y - 1.0, 0.0001) + small("capsule: separation", min_separation(r) + 0.01, 0.001) + on_axis = true + ends = 0 + i = 0 + while i < c.point_count { + p = cluster_point(c, i) + if math.abs_float(p.point.y + 0.295) > 0.001 || math.abs_float(p.point.z) > 0.001 { on_axis = false } + if math.abs_float(math.abs_float(p.point.x) - 1.0) < 0.001 { ends = ends + 1 } + i = i + 1 + } + ensure("capsule: points under the axis", on_axis) + ensure("capsule: both ends kept", ends == 2) + } + mesh_contact.mesh_contact_cache_destroy(&cache) + mesh.destroy_mesh(grid) + + // The box on a flat height field: the same one cluster of four. + field = height_field.create_grid(6, 6, math.vec3(1.0, 1.0, 1.0), false) + f_shape = shape.height_field_shape(field, 1) + box = hull.make_box_hull(0.5, 0.5, 0.5) + b = shape.hull_shape(box, 1.0) + cache = mesh_contact.mesh_contact_cache_create() + xf_b = Transform { p: math.vec3(2.3, 0.49, 2.2), q: math.quat_identity() } + r = manifolds(&cache, &f_shape, xf_a, &b, xf_b, false) + ensure("field: triangles cached (${cache.count})", cache.count >= 2 && cache.count <= 8) + ensure("field: one cluster of four", r.cluster_count == 1 && cluster(r, 0).point_count == 4) + if r.cluster_count == 1 { + small("field: normal up", cluster(r, 0).manifold_normal.y - 1.0, 0.0001) + // The grid's quantum puts its surface 0.0039 under y = 0. + small("field: separation", min_separation(r) + 0.01 - 0.0039, 0.001) + } + mesh_contact.mesh_contact_cache_destroy(&cache) + height_field.destroy_height_field(field) + + // A ridge: a field rising to a crest along z at x = 3; a box straddling the crest + // touches two faces with different normals: two clusters. + heights_block = calloc(36, 8) + heights = heights_block as float[] + i = 0 + while i < 36 { + col = i % 6 + h = 0.0 + if col == 3 { h = 0.5 } + if col == 2 || col == 4 { h = 0.25 } + heights[i] = h + i = i + 1 + } + def = height_field.height_field_def() + def.heights = heights_block + def.count_x = 6 + def.count_z = 6 + def.global_minimum_height = 0.0 - 1.0 + def.global_maximum_height = 1.0 + ridge = height_field.create_height_field(&def) + r_shape = shape.height_field_shape(ridge, 1) + cache = mesh_contact.mesh_contact_cache_create() + xf_b = Transform { p: math.vec3(3.0, 0.5 + 0.5 - 0.01, 2.5), q: math.quat_identity() } + r = manifolds(&cache, &r_shape, xf_a, &b, xf_b, false) + ensure("ridge: clusters (${r.cluster_count})", r.cluster_count >= 2) + leaning_left = 0 + leaning_right = 0 + i = 0 + while i < r.cluster_count { + // The box's bottom face is the manifold normal on both sides; the triangle normals lean. + n = cluster(r, i).triangle_normal + if n.x < 0.0 - 0.1 { leaning_left = leaning_left + 1 } + if n.x > 0.1 { leaning_right = leaning_right + 1 } + i = i + 1 + } + ensure("ridge: triangle normals lean both ways", leaning_left >= 1 && leaning_right >= 1) + ensure("ridge: touching", min_separation(r) < 0.0 && min_separation(r) > 0.0 - 0.05) + mesh_contact.mesh_contact_cache_destroy(&cache) + height_field.destroy_height_field(ridge) + free(heights_block) + + // The mesh under a transform: the grid turned a quarter about z at + // (10, 20, 30); the box against its local +y face from world -x. + grid = mesh.create_grid_mesh(8, 8, 1.0, 1, true) + ground = shape.mesh_shape(mesh.mesh(grid, math.vec3_one()), 1) + q = math.make_quat_from_axis_angle(math.vec3_axis_z(), 0.5 * math.PI) + xf_a = Transform { p: math.vec3(10.0, 20.0, 30.0), q: q } + // Local (0.3, 0.49, 0.2) in world: p + q * local = (10 - 0.49, 20 + 0.3, 30.2). + xf_b = Transform { p: math.transform_point(xf_a, math.vec3(0.3, 0.49, 0.2)), q: q } + cache = mesh_contact.mesh_contact_cache_create() + r = manifolds(&cache, &ground, xf_a, &b, xf_b, false) + ensure("moved mesh: one cluster of four", r.cluster_count == 1 && cluster(r, 0).point_count == 4) + if r.cluster_count == 1 { + // In the box's frame the normal is still its local +y. + small("moved mesh: normal in the box's frame", cluster(r, 0).manifold_normal.y - 1.0, 0.0001) + small("moved mesh: separation", min_separation(r) + 0.01, 0.0001) + } + mesh_contact.mesh_contact_cache_destroy(&cache) + mesh.destroy_mesh(grid) + hull.destroy_hull(box) +} + +main() { + before = core.alloc_count() + test_cull() + test_box_on_grid() + test_sphere_seams() + test_capsule_and_field() + // The scratch stays allocated: this module's six blocks, mesh's three, the height field's + // three, triangle_manifold's three; the test's arena is not counted. + ensure("every other counted allocation was freed (${core.alloc_count() - before})", core.alloc_count() == before + 15) + + println("mesh_contact: ${checks} checks") + if failures == 0 { + println("mesh_contact: all checks passed") + } else { + println("mesh_contact: ${failures} failure(s)") + exit(1) + } +} diff --git a/bench/RESULTS.md b/bench/RESULTS.md index 235d9c7..4dfbd59 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -351,3 +351,26 @@ signed longs, which is undefined in the C underneath, and gcc at -O2 made two inlined copies of it disagree, so a key stored by one copy was not found by the other. The new mixer stays in 32-bit products; the mesh builds above moved from 199 to 221 ms and 316 to 335 ms with it. + +## mesh_contact + +`bench/mesh_contact.ae`: a box, a sphere and a capsule each dragged +100,000 steps across a 100 x 100 wave mesh, riding a hundredth inside +the surface with a small move each step, the manifolds computed every +step. The reference's mesh contact lives inside its world (it needs a +contact, a worker context and the world's material callbacks), so it +has no free-standing counterpart; it is measured against ours through +the world benchmarks once the world steps. + +| shape, 100,000 steps | aephysics | per step | +|---|---|---| +| box (0.8 wide): 876,526 clusters, 3.17 M points, 910,432 cache hits | 322 ms | 3.2 us | +| sphere: 127,842 clusters, 129,088 points | 69 ms | 0.7 us | +| capsule: 797,124 clusters, 1.35 M points | 371 ms | 3.7 us | + +The wave's cells are half the box's width, so a box straddles several +triangles of different normals and the clusters stay many (the cluster +threshold is cos 5 degrees); the far clip points of a hull face are +kept as speculative ones, as the reference keeps them. The test checks +the seams (a sphere on an interior edge or vertex gives one point), the +cache's persistence and the cull. diff --git a/bench/mesh_contact.ae b/bench/mesh_contact.ae new file mode 100644 index 0000000..8974386 --- /dev/null +++ b/bench/mesh_contact.ae @@ -0,0 +1,98 @@ +// The mesh contact on its own: a box, a sphere and a capsule each +// dragged 100,000 steps across a 100 x 100 wave mesh (a small move each +// step, so the triangle cache mostly holds and the warm starts answer), +// the manifolds computed at every step. The reference's +// b3ComputeMeshManifolds lives inside its world (it needs a contact, a +// worker context and the world's callbacks), so it has no free-standing +// counterpart; it is measured against ours through the world +// benchmarks. Single thread, wall time per shape, with the cluster and +// point counts and the separation sum as the checksum. +import std.string +import std.os +import aephysics.math +import aephysics.core +import aephysics.hull +import aephysics.distance +import aephysics.manifold +import aephysics.triangle_manifold +import aephysics.mesh +import aephysics.height_field +import aephysics.material +import aephysics.sphere +import aephysics.capsule +import aephysics.compound +import aephysics.shape +import aephysics.mesh_contact + +extern calloc(count: int, size: int) -> ptr +extern free(p: ptr) +extern sin(x: float) -> float +extern cos(x: float) -> float + +clock() -> long { return os.now_monotonic_ns() } +ms(ns: long) -> float { return (ns as float) / 1000000.0 } + +const STEPS = 100000 + +var g_arena: ptr = null + +run(name: string, ground: *Shape, convex: *Shape, radius: float) { + arena = g_arena as *Arena + cache = mesh_contact.mesh_contact_cache_create() + xf_a = math.transform_identity() + clusters = 0 + points = 0 + separation_sum = 0.0 + hits = 0 + t0 = clock() + i = 0 + while i < STEPS { + t = (i as float) / (STEPS as float) + x = 0.0 - 20.0 + 40.0 * t + z = 10.0 * sin(6.2831853 * 3.0 * t) + // The surface height under the shape, so it rides just inside it. + out = mesh.ray_cast_mesh(ground.mesh, math.vec3(x, 5.0, z), math.vec3(0.0, 0.0 - 10.0, 0.0), 1.0) + y = out.point.y + radius - 0.01 + xf_b = Transform { p: math.vec3(x, y, z), q: math.make_quat_from_axis_angle(math.vec3_axis_y(), 0.3 * t) } + arena.index = 0 + r = mesh_contact.compute_mesh_manifolds(&cache, ground, xf_a, convex, xf_b, shape.compute_shape_aabb(convex, xf_b), false, true, arena) + clusters = clusters + r.cluster_count + hits = hits + r.sat_cache_hits + cs = r.clusters as Cluster[] + k = 0 + while k < r.cluster_count { + points = points + cs[k].point_count + pts = cs[k].points as LocalManifoldPoint[] + j = 0 + while j < cs[k].point_count { + separation_sum = separation_sum + pts[j].separation + j = j + 1 + } + k = k + 1 + } + i = i + 1 + } + t1 = clock() + println("aephysics mesh_contact: ${name} ${STEPS} steps ${ms(t1 - t0)} ms (${clusters} clusters, ${points} points, separation sum ${separation_sum}, ${hits} cache hits)") + mesh_contact.mesh_contact_cache_destroy(&cache) +} + +main() { + g_arena = calloc(1, sizeof(Arena)) + arena = g_arena as *Arena + made = core.arena_create(1 << 20) + arena.memory = made.memory + arena.capacity = made.capacity + arena.overflows = made.overflows + wave = mesh.create_wave_mesh(100, 100, 0.5, 0.3, 0.2, 0.3) + ground = shape.mesh_shape(mesh.mesh(wave, math.vec3_one()), 1) + box = hull.make_box_hull(0.4, 0.4, 0.4) + b = shape.hull_shape(box, 1.0) + run("box", &ground, &b, 0.4) + s = shape.sphere_shape(manifold.sphere(math.vec3_zero(), 0.4), 1.0) + run("sphere", &ground, &s, 0.4) + c = shape.capsule_shape(manifold.capsule(math.vec3(0.0 - 0.5, 0.0, 0.0), math.vec3(0.5, 0.0, 0.0), 0.3), 1.0) + run("capsule", &ground, &c, 0.3) + hull.destroy_hull(box) + mesh.destroy_mesh(wave) +} diff --git a/design.md b/design.md index 6f4141f..b12f6d4 100644 --- a/design.md +++ b/design.md @@ -135,11 +135,24 @@ started until its tests pass. constraint_graph.c, sensor.c, broad_phase.c and half of physics_world.c call into each other), so the cut into Aether's acyclic modules is: - - `aephysics.mesh_contact`: mesh_contact.c's cluster reduction of a - mesh's or height field's triangle manifolds against a convex shape - (the point culling and the per-cluster reduction are pure; the - triangle cache it refreshes is the contact's, so the entry point - takes the cache as a struct). + - `aephysics.mesh_contact` (done): mesh_contact.c's world-free + part: the triangle cache refreshed when the shape leaves its query + bounds (each triangle keeping a simplex and a separating-axis + cache; the reference's union is two fields), the manifold per + triangle in the convex shape's frame, the acceptance rules against + ghost collisions (a triangle face always; a hull face when aligned + or deep; the rest tentative, spheres by nearest-first feature + ownership, hulls and capsules skipping only owned flat edges), + clusters within cos 5 degrees, the four-point cull. The results + live in the caller's arena. What follows in the reference (the + warm-start matching, materials, rolling resistance, tangent + velocity) is the contact's and comes with the dynamics. 64 checks + of our own (the reference tests this through its world): the cull, + a box on a grid, the cache kept and refreshed, a sphere on seams, a + capsule, a height field, a ridge, a moved mesh. Two reference + quirks kept: the cull's tie rule lets zero-area points through on + their separation (collinear points can keep four), and a hull + face's far clip points stay as speculative ones. - `aephysics.broad_phase` (done): broad_phase.c's trees per body type, proxies keyed by type in the low bits, the moved-sibling gathering, the self and cross pair walks and the pair set; the pair