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