From def762d413b78fb09b3b71730ca1113bfac15875 Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Sat, 19 Sep 2026 18:40:07 -0300 Subject: [PATCH] aephysics.triangle_manifold: one mesh triangle against sphere, capsule, hull Box3D's triangle_manifold.c in Aether: the back side culled with hysteresis, the closest point on a triangle by region with its feature, GJK for the shallow sphere and capsule cases, the separating axis test for the deep ones -- the triangle's face, the hull's faces, the edge pairs whose arcs cross, the triangle's edges as zero-area faces with a side normal so the axis with a capsule points out of the triangle -- the SAT cache tried first (a face reused only when not deep, an edge only when its separation held), the hull face refused as reference when it opposes the triangle, the edge contact when the face has nothing or one point and the edge is clearly better, and GJK last so nothing tunnels. The feature the contact came from is recorded for the mesh contact's ghost-collision reduction. test_triangle_manifold.ae: 1,540 checks from the triangle parts of test_manifold.c (the tipped cube's edge on a tilted triangle edge from speculative gap into deep overlap, the edge axis carrying the cull, the parallel pair through the rejection threshold, the edge sweep against the cross product, the capsule pushed across an edge and straddling the face) plus the sphere on every feature, the resting cube, the back-side hysteresis and the cache. bench/triangle.ae against bench/triangle_box3d.c: the same manifolds (point counts, cache hits and separation sums equal); triangle-hull within 10% cold and warm, the capsule 1.35x, the sphere twice as fast. --- README.md | 3 +- aephysics/test_triangle_manifold.ae | 449 ++++++++++++++ aephysics/triangle_manifold/module.ae | 862 ++++++++++++++++++++++++++ bench/RESULTS.md | 22 + bench/triangle.ae | 137 ++++ bench/triangle_box3d.c | 114 ++++ design.md | 26 +- 7 files changed, 1604 insertions(+), 9 deletions(-) create mode 100644 aephysics/test_triangle_manifold.ae create mode 100644 aephysics/triangle_manifold/module.ae create mode 100644 bench/triangle.ae create mode 100644 bench/triangle_box3d.c diff --git a/README.md b/README.md index 176bd04..202a4d4 100644 --- a/README.md +++ b/README.md @@ -31,7 +31,8 @@ so a test written against the reference reads the same here. | `aephysics.hull` | quickhull with face merging, the half-edge hull with its mass properties, box / cylinder / cone / rock hulls, clone-and-transform with mirroring, support functions, ray cast, the 2D hull | done, `test_hull.ae` (438 checks); [same hulls as the reference, 1.6-2x its time](bench/RESULTS.md#hull) | | `aephysics.distance` | GJK with the warm-started simplex cache, the shape cast by conservative advancement, the time of impact by separating-axis root finding | done, `test_distance.ae` (1.1k checks); [same results as the reference, 1.3-1.5x its time](bench/RESULTS.md#distance) | | `aephysics.manifold` | contact manifolds for sphere, capsule and hull in every pairing: the separating axis test with its cache, reference-face clipping, the feature pairs, reduction to four points | done, `test_manifold.ae` (43k checks, 7,000 pairs against a brute-force oracle); [same manifolds as the reference, warm cache at parity](bench/RESULTS.md#manifold) | -| `aephysics.collision` | triangle manifolds, triangle mesh, height field, shapes with mass properties, ray and shape casts | next | +| `aephysics.triangle_manifold` | one mesh triangle against a sphere, capsule or hull: back-side cull with hysteresis, GJK shallow, the separating axis test deep with the triangle's edges as zero-area faces, the feature recorded for the mesh contact's ghost-collision reduction | done, `test_triangle_manifold.ae` (1.5k checks); [same manifolds as the reference, within 10% on hulls](bench/RESULTS.md#triangle_manifold) | +| `aephysics.collision` | triangle mesh (BVH), height field, shapes with mass properties, ray and shape casts | next | | `aephysics.dynamics` | bodies, contacts, the constraint graph, islands, the Soft Step solver, joints (spherical, revolute, prismatic, distance, motor, weld, wheel), sensors, the character mover, the world | | | `aephysics` | the public API | | diff --git a/aephysics/test_triangle_manifold.ae b/aephysics/test_triangle_manifold.ae new file mode 100644 index 0000000..864ae69 --- /dev/null +++ b/aephysics/test_triangle_manifold.ae @@ -0,0 +1,449 @@ +// aephysics.triangle_manifold against the triangle parts of the +// reference's (Box3D's) test_manifold.c: the tipped cube's edge on a +// tilted triangle edge from speculative gap into deep overlap and the +// edge axis carrying the cull, the parallel edge pair through the +// rejection threshold, the triangle-hull edge sweep against the cross +// product, the capsule pushed across a triangle edge, the capsule core +// straddling the face; and beyond it the closest point on a triangle by +// region, the sphere on every feature, the face contact of a resting +// cube, the back-side cull with hysteresis and the cache. + +import std.string +import aephysics.math +import aephysics.core +import aephysics.distance +import aephysics.hull +import aephysics.manifold +import aephysics.triangle_manifold + +extern calloc(count: int, size: int) -> ptr +extern exit(code: int) +extern free(p: ptr) +extern sin(x: float) -> float +extern cos(x: float) -> float +extern sqrt(x: float) -> float + +var failures = 0 +var checks = 0 + +ensure(name: string, ok: bool) { + checks = checks + 1 + if !ok { + println("triangle_manifold: FAIL ${name}") + failures = failures + 1 + } +} + +small(name: string, value: float, tolerance: float) { + ensure("${name} (${value})", math.abs_float(value) < tolerance) +} + +const ROOT2 = 1.41421356 +const HALF_ROOT2 = 0.70710678 +const HALF_DIAGONAL = 0.87 + +exact_quat(axis: Vec3, radians: float) -> Quat { + half = 0.5 * radians + return Quat { v: math.mul_sv(sin(half), axis), s: cos(half) } +} + +exact_rotation(axis: Vec3, radians: float) -> Transform { + return Transform { p: math.vec3_zero(), q: exact_quat(axis, radians) } +} + +new_points() -> ptr { return calloc(8, sizeof(LocalManifoldPoint)) } + +min_separation(m: *LocalManifold) -> float { + s = math.MAX_FLOAT + i = 0 + while i < m.point_count { + s = math.min_float(s, manifold.manifold_point(m, i).separation) + i = i + 1 + } + return s +} + +// The edge pair axis must match the cross product: the axis, separation +// and point rebuilt from the two edges and the radius. +check_edge_contact(name: string, m: *LocalManifold, p1: Vec3, e1: Vec3, p2: Vec3, e2: Vec3, orient: Vec3, radius: float, + normal_tol: float, sep_tol: float, point_tol: float) { + axis = math.normalize(math.cross(e1, e2)) + if math.dot(axis, orient) < 0.0 { axis = math.neg(axis) } + small("${name}: normal x", m.normal.x - axis.x, normal_tol) + small("${name}: normal y", m.normal.y - axis.y, normal_tol) + small("${name}: normal z", m.normal.z - axis.z, normal_tol) + small("${name}: perpendicular to e1", math.dot(m.normal, math.normalize(e1)), normal_tol) + small("${name}: perpendicular to e2", math.dot(m.normal, math.normalize(e2)), normal_tol) + closest = math.line_distance(p1, e1, p2, e2) + expected_separation = math.dot(axis, math.sub(closest.point2, closest.point1)) - radius + p = manifold.manifold_point(m, 0) + small("${name}: separation", p.separation - expected_separation, sep_tol) + expected_point = math.mul_sv(0.5, math.add(math.mul_sub(closest.point1, radius, axis), closest.point2)) + small("${name}: point x", p.point.x - expected_point.x, point_tol) + small("${name}: point y", p.point.y - expected_point.y, point_tol) + small("${name}: point z", p.point.z - expected_point.z, point_tol) +} + +hull_edge_tail(h: *HullData, edge_index: int) -> Vec3 { + edges = hull.hull_edges(h) + points = hull.hull_points(h) + return points[edges[edge_index].origin] +} + +hull_edge_vector(h: *HullData, edge_index: int) -> Vec3 { + edges = hull.hull_edges(h) + points = hull.hull_points(h) + return math.sub(points[edges[edges[edge_index].twin].origin], points[edges[edge_index].origin]) +} + +// A cube pitched 45 degrees rests an edge along x at y = -h root2; a +// tilted triangle edge crosses under it so the arc intersection lands +// inside the arc. +test_triangle_edge() { + beta = 20.0 * math.PI / 180.0 + gamma = 30.0 * math.PI / 180.0 + h = hull.make_transformed_box_hull(0.5, 0.5, 0.5, exact_rotation(math.vec3_axis_x(), 0.25 * math.PI)) + tri_normal = math.vec3(sin(beta), cos(beta), 0.0) + tri_edge = math.vec3(sin(gamma) * cos(beta), 0.0 - sin(gamma) * sin(beta), cos(gamma)) + axis = math.normalize(math.cross(math.vec3_axis_x(), tri_edge)) + hull_point = math.vec3(0.0, 0.0 - HALF_ROOT2, 0.0) + points = new_points() + gaps = [ 0.03, 0.01, 0.0, 0.0 - 0.01, 0.0 - 0.1 ] + i = 0 + while i < 5 { + gap = gaps[i] + triangle_point = math.mul_add(hull_point, gap, axis) + v1 = math.mul_add(triangle_point, 0.0 - 1.0, tri_edge) + v2 = math.mul_add(triangle_point, 1.0, tri_edge) + v3 = math.mul_add(v1, 1.5, math.cross(tri_normal, tri_edge)) + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + cache.kind = manifold.AXIS_MANUAL_EDGE_PAIR + triangle_manifold.collide_triangle_and_hull(&m, 8, v1, v2, v3, 0, h, &cache, true) + expected_normal = math.neg(axis) + expected_point = math.mul_add(hull_point, 0.5 * gap, axis) + ensure("triangle edge ${i}: one point", m.point_count == 1) + ensure("triangle edge ${i}: edge pair", cache.kind == manifold.AXIS_EDGE_PAIR) + small("triangle edge ${i}: normal x", m.normal.x - expected_normal.x, 0.00001) + small("triangle edge ${i}: normal y", m.normal.y - expected_normal.y, 0.00001) + small("triangle edge ${i}: normal z", m.normal.z - expected_normal.z, 0.00001) + if m.point_count == 1 { + p = manifold.manifold_point(&m, 0) + small("triangle edge ${i}: separation", p.separation - gap, 0.00001) + small("triangle edge ${i}: point x", p.point.x - expected_point.x, 0.00001) + small("triangle edge ${i}: point y", p.point.y - expected_point.y, 0.00001) + small("triangle edge ${i}: point z", p.point.z - expected_point.z, 0.00001) + } + i = i + 1 + } + // The tipped plane buries a hull corner: only the edge axis can cull. + culled = [ 0.03, 0.05 ] + i = 0 + while i < 2 { + gap = culled[i] + triangle_point = math.mul_add(hull_point, gap, axis) + v1 = math.mul_add(triangle_point, 0.0 - 1.0, tri_edge) + v2 = math.mul_add(triangle_point, 1.0, tri_edge) + v3 = math.mul_add(v1, 1.5, math.cross(tri_normal, tri_edge)) + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + triangle_manifold.collide_triangle_and_hull(&m, 8, v1, v2, v3, 0, h, &cache, true) + ensure("triangle edge cull ${i}: no points", m.point_count == 0) + ensure("triangle edge cull ${i}: edge pair cached", cache.kind == manifold.AXIS_EDGE_PAIR) + small("triangle edge cull ${i}: cached separation", cache.separation - gap, 0.00001) + i = i + 1 + } + + // The cube's bottom edge on a triangle whose first edge runs along x, + // tipped from exactly parallel through the rejection threshold. + overlap = 0.01 + y = 0.0 - HALF_ROOT2 + overlap + axes = [ 0.57735027, 0.57735027, 0.57735027, 0.70710678, 0.0, 0.70710678, 0.26726124, 0.53452248, 0.80178373, 0.0 - 0.48507125, 0.72760688, 0.0 - 0.48507125 ] + angles = [ 0.0, 0.0000001, 0.000001, 0.00001, 0.0001, 0.001, 0.004, 0.005, 0.006, 0.01, 0.05 ] + i = 0 + while i < 4 { + tilt_axis = math.vec3(axes[3 * i], axes[3 * i + 1], axes[3 * i + 2]) + j = 0 + while j < 11 { + q = exact_quat(tilt_axis, angles[j]) + v1 = math.rotate_vector(q, math.vec3(0.0 - 2.0, y, 0.0 - 1.0)) + v2 = math.rotate_vector(q, math.vec3(0.0, y, 2.0)) + v3 = math.rotate_vector(q, math.vec3(2.0, y, 0.0 - 1.0)) + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + triangle_manifold.collide_triangle_and_hull(&m, 8, v1, v2, v3, 0, h, &cache, true) + ensure("parallel ${i} ${j}: face points", m.point_count >= 4) + ensure("parallel ${i} ${j}: the triangle face", cache.kind == manifold.AXIS_FACE_A) + ensure("parallel ${i} ${j}: normal up", math.dot(m.normal, math.vec3_axis_y()) > 0.99) + small("parallel ${i} ${j}: separation", min_separation(&m) + overlap, HALF_DIAGONAL * angles[j] + 0.00001) + j = j + 1 + } + i = i + 1 + } + + // The edge sweep: plane tip, edge yaw, gap; every edge contact against the cross product. + betas = [ 8.0, 20.0, 32.0 ] + gammas = [ 20.0, 35.0, 50.0, 70.0 ] + gaps2 = [ 0.02, 0.0, 0.0 - 0.03, 0.0 - 0.08 ] + edge_contacts = 0 + a = 0 + while a < 3 { + b = 0 + while b < 4 { + c = 0 + while c < 4 { + beta2 = betas[a] * math.PI / 180.0 + gamma2 = gammas[b] * math.PI / 180.0 + tri_normal2 = math.vec3(sin(beta2), cos(beta2), 0.0) + tri_edge2 = math.vec3(sin(gamma2) * cos(beta2), 0.0 - sin(gamma2) * sin(beta2), cos(gamma2)) + axis2 = math.normalize(math.cross(math.vec3_axis_x(), tri_edge2)) + triangle_point = math.mul_add(hull_point, gaps2[c], axis2) + v1 = math.mul_add(triangle_point, 0.0 - 1.0, tri_edge2) + v2 = math.mul_add(triangle_point, 1.0, tri_edge2) + v3 = math.mul_add(v1, 1.5, math.cross(tri_normal2, tri_edge2)) + triangle_center = math.mul_sv(1.0 / 3.0, math.add(v1, math.add(v2, v3))) + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + cache.kind = manifold.AXIS_MANUAL_EDGE_PAIR + triangle_manifold.collide_triangle_and_hull(&m, 8, v1, v2, v3, 0, h, &cache, true) + if cache.kind == manifold.AXIS_EDGE_PAIR && m.point_count == 1 { + p1 = v1 + e1 = math.sub(v2, v1) + if cache.index_a == 1 { + p1 = v2 + e1 = math.sub(v3, v2) + } else if cache.index_a == 2 { + p1 = v3 + e1 = math.sub(v1, v3) + } + p2 = hull_edge_tail(h, cache.index_b) + e2 = hull_edge_vector(h, cache.index_b) + orient = math.sub(h.center, triangle_center) + check_edge_contact("edge sweep", &m, p1, e1, p2, e2, orient, 0.0, 0.0001, 0.0001, 0.001) + edge_contacts = edge_contacts + 1 + } + c = c + 1 + } + b = b + 1 + } + a = a + 1 + } + ensure("edge sweep: drove the edge path (${edge_contacts})", edge_contacts >= 30) + free(points) + hull.destroy_hull(h) +} + +// A capsule nearly in the triangle plane pushed across an edge, and a +// capsule core straddling the face. +test_triangle_capsule() { + v1 = math.vec3(0.0 - 2.0, 0.0, 0.0) + v2 = math.vec3(2.0, 0.0, 0.0) + v3 = math.vec3(0.0, 0.0, 0.0 - 2.0) + triangle_center = math.mul_sv(1.0 / 3.0, math.add(v1, math.add(v2, v3))) + points = new_points() + z0s = [ 0.0 - 0.05, 0.0 - 0.03, 0.0 - 0.01 ] + tilts = [ 0.2, 0.3, 0.4 ] + yaws = [ 0.4, 0.6, 0.8 ] + radii = [ 0.05, 0.1 ] + edge_contacts = 0 + i = 0 + while i < 3 { + j = 0 + while j < 3 { + y = 0 + while y < 3 { + r = 0 + while r < 2 { + dir = math.normalize(math.vec3(sin(yaws[y]), tilts[j], cos(yaws[y]))) + mid = math.vec3(0.0, 0.0, z0s[i]) + c1 = math.mul_add(mid, 0.0 - 0.6, dir) + c2 = math.mul_add(mid, 0.6, dir) + c = manifold.capsule(c1, c2, radii[r]) + m = manifold.local_manifold(points) + cache = distance.empty_cache() + triangle_manifold.collide_triangle_and_capsule(&m, 8, v1, v2, v3, c, &cache) + if m.point_count == 1 && m.feature >= triangle_manifold.FEATURE_EDGE1 && m.feature <= triangle_manifold.FEATURE_EDGE3 { + edge_index = m.feature - triangle_manifold.FEATURE_EDGE1 + p1 = v1 + e1 = math.sub(v2, v1) + if edge_index == 1 { + p1 = v2 + e1 = math.sub(v3, v2) + } else if edge_index == 2 { + p1 = v3 + e1 = math.sub(v1, v3) + } + orient = math.sub(math.lerp(c1, c2, 0.5), triangle_center) + check_edge_contact("capsule edge deep", &m, p1, e1, c1, math.sub(c2, c1), orient, radii[r], 0.0001, 0.0001, 0.0002) + edge_contacts = edge_contacts + 1 + } + r = r + 1 + } + y = y + 1 + } + j = j + 1 + } + i = i + 1 + } + ensure("capsule edge deep: reached the edge path (${edge_contacts})", edge_contacts >= 15) + + // The core straddling the face: two points at the endpoint heights. + w1 = math.vec3(0.0 - 3.0, 0.0, 0.0 - 2.0) + w2 = math.vec3(0.0, 0.0, 4.0) + w3 = math.vec3(3.0, 0.0, 0.0 - 2.0) + yaws2 = [ 0.0, 0.6, 1.2, 1.8, 2.4 ] + tilts2 = [ 0.06, 0.1, 0.15 ] + radii2 = [ 0.05, 0.1, 0.2 ] + bias = 0.01 + half_length = 1.0 + face_contacts = 0 + i = 0 + while i < 5 { + j = 0 + while j < 3 { + r = 0 + while r < 3 { + axis = math.vec3(cos(tilts2[j]) * cos(yaws2[i]), sin(tilts2[j]), cos(tilts2[j]) * sin(yaws2[i])) + center = math.vec3(0.0, bias, 0.0) + c1 = math.mul_add(center, 0.0 - half_length, axis) + c2 = math.mul_add(center, half_length, axis) + c = manifold.capsule(c1, c2, radii2[r]) + m = manifold.local_manifold(points) + cache = distance.empty_cache() + triangle_manifold.collide_triangle_and_capsule(&m, 8, w1, w2, w3, c, &cache) + ensure("capsule face deep ${i} ${j} ${r}: two points", m.point_count == 2) + ensure("capsule face deep: the triangle face", m.feature == triangle_manifold.FEATURE_TRIANGLE_FACE) + small("capsule face deep: normal x", m.normal.x, 0.00001) + small("capsule face deep: normal y", m.normal.y - 1.0, 0.00001) + small("capsule face deep: normal z", m.normal.z, 0.00001) + if m.point_count == 2 { + lower = math.min_float(c1.y, c2.y) - radii2[r] + upper = math.max_float(c1.y, c2.y) - radii2[r] + s0 = manifold.manifold_point(&m, 0).separation + s1 = manifold.manifold_point(&m, 1).separation + small("capsule face deep: min separation", math.min_float(s0, s1) - lower, 0.00001) + small("capsule face deep: max separation", math.max_float(s0, s1) - upper, 0.00001) + ensure("capsule face deep: penetrating", math.min_float(s0, s1) < 0.0) + } + face_contacts = face_contacts + 1 + r = r + 1 + } + j = j + 1 + } + i = i + 1 + } + ensure("capsule face deep: all cases", face_contacts == 45) + free(points) +} + +// The closest point by region and the sphere against every feature. +test_sphere_and_closest() { + // Wound so the normal is +y. + v1 = math.vec3(0.0 - 1.0, 0.0, 0.0 - 1.0) + v2 = math.vec3(0.0, 0.0, 1.0) + v3 = math.vec3(1.0, 0.0, 0.0 - 1.0) + inside = triangle_manifold.closest_point_on_triangle(v1, v2, v3, math.vec3(0.0, 2.0, 0.0)) + ensure("closest: face", inside.feature == triangle_manifold.FEATURE_TRIANGLE_FACE) + small("closest: face point", math.length(inside.point), 0.00001) + corner = triangle_manifold.closest_point_on_triangle(v1, v2, v3, math.vec3(0.0 - 3.0, 1.0, 0.0 - 3.0)) + ensure("closest: vertex 1", corner.feature == triangle_manifold.FEATURE_VERTEX1) + small("closest: vertex point", math.distance(corner.point, v1), 0.00001) + edge = triangle_manifold.closest_point_on_triangle(v1, v2, v3, math.vec3(0.0, 1.0, 0.0 - 3.0)) + ensure("closest: edge 3 (v3-v1)", edge.feature == triangle_manifold.FEATURE_EDGE3) + small("closest: edge point", math.distance(edge.point, math.vec3(0.0, 0.0, 0.0 - 1.0)), 0.00001) + corner2 = triangle_manifold.closest_point_on_triangle(v1, v2, v3, math.vec3(0.0, 0.0 - 1.0, 4.0)) + ensure("closest: vertex 2", corner2.feature == triangle_manifold.FEATURE_VERTEX2) + + points = new_points() + // Above the face: the gap is the height minus the radius, the normal up. + s = manifold.sphere(math.vec3(0.0, 0.26, 0.0), 0.25) + m = manifold.local_manifold(points) + triangle_manifold.collide_triangle_and_sphere(&m, 8, v1, v2, v3, s) + ensure("sphere face: one point", m.point_count == 1) + ensure("sphere face: feature", m.feature == triangle_manifold.FEATURE_TRIANGLE_FACE) + small("sphere face: normal y", m.normal.y - 1.0, 0.00001) + if m.point_count == 1 { small("sphere face: separation", manifold.manifold_point(&m, 0).separation - 0.01, 0.00001) } + // Off an edge: the normal from the edge to the centre. + s = manifold.sphere(math.vec3(0.0, 0.15, 0.0 - 1.15), 0.25) + m = manifold.local_manifold(points) + triangle_manifold.collide_triangle_and_sphere(&m, 8, v1, v2, v3, s) + ensure("sphere edge: one point", m.point_count == 1) + ensure("sphere edge: feature", m.feature == triangle_manifold.FEATURE_EDGE3) + if m.point_count == 1 { small("sphere edge: separation", manifold.manifold_point(&m, 0).separation - (sqrt(0.045) - 0.25), 0.00001) } + // Below the plane: culled. Far above: nothing. + s = manifold.sphere(math.vec3(0.0, 0.0 - 0.1, 0.0), 0.25) + m = manifold.local_manifold(points) + triangle_manifold.collide_triangle_and_sphere(&m, 8, v1, v2, v3, s) + ensure("sphere back side: culled", m.point_count == 0) + s = manifold.sphere(math.vec3(0.0, 1.0, 0.0), 0.25) + m = manifold.local_manifold(points) + triangle_manifold.collide_triangle_and_sphere(&m, 8, v1, v2, v3, s) + ensure("sphere far: nothing", m.point_count == 0) + free(points) +} + +// A cube resting on a big triangle: the face contact, the cache reused, +// the back side culled with hysteresis, the separation when lifted. +test_resting_cube() { + h = hull.make_box_hull(0.5, 0.5, 0.5) + points = new_points() + y = 0.0 - 0.5 - 0.005 + v1 = math.vec3(0.0 - 3.0, y, 0.0 - 2.0) + v2 = math.vec3(0.0, y, 4.0) + v3 = math.vec3(3.0, y, 0.0 - 2.0) + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + triangle_manifold.collide_triangle_and_hull(&m, 8, v1, v2, v3, 0, h, &cache, true) + ensure("resting: four points", m.point_count == 4) + // Both face axes tie at the gap; the reference takes the hull's face on a tie. + ensure("resting: the hull face", cache.kind == manifold.AXIS_FACE_B && m.feature == triangle_manifold.FEATURE_HULL_FACE) + small("resting: normal up", m.normal.y - 1.0, 0.00001) + i = 0 + while i < m.point_count { + small("resting: separation", manifold.manifold_point(&m, i).separation - 0.005, 0.00001) + i = i + 1 + } + ensure("resting: cache miss the first time", cache.hit == 0) + triangle_manifold.collide_triangle_and_hull(&m, 8, v1, v2, v3, 0, h, &cache, true) + ensure("resting: the cache answered", cache.hit == 1 && m.point_count == 4) + + // Lifted past the speculative distance: the axis alone, no points. + y2 = 0.0 - 0.5 - 0.05 + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + triangle_manifold.collide_triangle_and_hull(&m, 8, math.vec3(0.0 - 3.0, y2, 0.0 - 2.0), math.vec3(0.0, y2, 4.0), math.vec3(3.0, y2, 0.0 - 2.0), 0, h, &cache, true) + ensure("lifted: no points", m.point_count == 0) + ensure("lifted: the triangle face cached", cache.kind == manifold.AXIS_FACE_A) + small("lifted: cached separation", cache.separation - 0.05, 0.00001) + + // A triangle above the cube, facing up: the hull's centre is behind it, culled with hysteresis. + y3 = 1.0 + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + triangle_manifold.collide_triangle_and_hull(&m, 8, math.vec3(0.0 - 3.0, y3, 0.0 - 2.0), math.vec3(0.0, y3, 4.0), math.vec3(3.0, y3, 0.0 - 2.0), 0, h, &cache, true) + ensure("back side: culled", m.point_count == 0 && cache.kind == manifold.AXIS_BACKSIDE) + small("back side: the offset cached", cache.separation + 1.0, 0.00001) + y4 = 1.002 + triangle_manifold.collide_triangle_and_hull(&m, 8, math.vec3(0.0 - 3.0, y4, 0.0 - 2.0), math.vec3(0.0, y4, 4.0), math.vec3(3.0, y4, 0.0 - 2.0), 0, h, &cache, true) + ensure("back side: hysteresis holds", m.point_count == 0 && cache.kind == manifold.AXIS_BACKSIDE) + free(points) + hull.destroy_hull(h) +} + +main() { + before = core.alloc_count() + test_sphere_and_closest() + test_triangle_edge() + test_triangle_capsule() + test_resting_cube() + // This module's scratch stays allocated: three blocks. + ensure("every other counted allocation was freed", core.alloc_count() == before + 3) + + println("triangle_manifold: ${checks} checks") + if failures == 0 { + println("triangle_manifold: all checks passed") + } else { + println("triangle_manifold: ${failures} failure(s)") + exit(1) + } +} diff --git a/aephysics/triangle_manifold/module.ae b/aephysics/triangle_manifold/module.ae new file mode 100644 index 0000000..e61af86 --- /dev/null +++ b/aephysics/triangle_manifold/module.ae @@ -0,0 +1,862 @@ +// aephysics.triangle_manifold -- contact manifolds between one triangle +// of a mesh or height field and a sphere, a capsule or a hull, in the +// convex shape's frame, with the triangle feature the contact came from +// recorded for the mesh contact's ghost-collision reduction. +// +// The shape is Box3D's triangle_manifold.c (Erin Catto, MIT), the +// reference this engine is measured against: the back side culled with +// hysteresis, GJK for the shallow sphere and capsule cases, the +// separating axis test for the deep ones (the triangle's face, the hull's +// faces, the edge pairs whose Gauss-map arcs cross, the triangle's edges +// treated as zero-area faces with a side normal so an edge-edge axis +// points out of the triangle), the SAT cache tried first, the hull face +// rejected as reference when it opposes the triangle (a ghost collision), +// and GJK as the last resort when speculation leaves the SAT with no +// points. Names are the reference's without its prefix, in snake case: +// b3CollideTriangleAndHull is collide_triangle_and_hull. +// +// Differences: the clip buffers are module scratch rather than the C +// stack, as in aephysics.manifold. +import std.string +import aephysics.math +import aephysics.core +import aephysics.distance +import aephysics.hull +import aephysics.manifold + +exports ( + TrianglePoint, + FEATURE_NONE, FEATURE_TRIANGLE_FACE, FEATURE_HULL_FACE, FEATURE_EDGE1, FEATURE_EDGE2, FEATURE_EDGE3, + FEATURE_VERTEX1, FEATURE_VERTEX2, FEATURE_VERTEX3, + CONCAVE_EDGE1, CONCAVE_EDGE2, CONCAVE_EDGE3, INVERSE_CONCAVE_EDGE1, INVERSE_CONCAVE_EDGE2, INVERSE_CONCAVE_EDGE3, + ALL_CONCAVE_EDGES, FLAT_EDGE1, FLAT_EDGE2, FLAT_EDGE3, ALL_FLAT_EDGES, + closest_point_on_triangle, get_triangle_feature, + collide_triangle_and_sphere, collide_triangle_and_capsule, collide_triangle_and_hull +) + +const NULL_INDEX = 0 - 1 +// aephysics.manifold's constants, restated: a module constant inside a +// struct literal in a return is not resolved by aetherc. +const AXIS_INVALID = 0 +const AXIS_BACKSIDE = 1 +const AXIS_FACE_A = 2 +const AXIS_FACE_B = 3 +const AXIS_EDGE_PAIR = 4 +const AXIS_MANUAL_FACE_A = 6 +const AXIS_MANUAL_FACE_B = 7 +const AXIS_MANUAL_EDGE_PAIR = 8 +const FEATURE_SHAPE_A = 0 +const FEATURE_SHAPE_B = 1 +const SPECULATIVE_DISTANCE = 0.02 +const MAX_CLIP_POINTS = 64 +const FLOAT_EPSILON = 0.00000011920929 +const FLOAT_MIN = 0.000000000000000000000000000000000000011754944 + +const FEATURE_NONE = 0 +const FEATURE_TRIANGLE_FACE = 1 +const FEATURE_HULL_FACE = 2 +const FEATURE_EDGE1 = 3 // v1-v2 +const FEATURE_EDGE2 = 4 // v2-v3 +const FEATURE_EDGE3 = 5 // v3-v1 +const FEATURE_VERTEX1 = 6 +const FEATURE_VERTEX2 = 7 +const FEATURE_VERTEX3 = 8 + +// Mesh edge flags: the edges of a triangle that are concave or flat +// against the neighbouring triangle. +const CONCAVE_EDGE1 = 0x01 +const CONCAVE_EDGE2 = 0x02 +const CONCAVE_EDGE3 = 0x04 +const INVERSE_CONCAVE_EDGE1 = 0x10 +const INVERSE_CONCAVE_EDGE2 = 0x20 +const INVERSE_CONCAVE_EDGE3 = 0x40 +const ALL_CONCAVE_EDGES = 0x07 +const FLAT_EDGE1 = 0x11 +const FLAT_EDGE2 = 0x22 +const FLAT_EDGE3 = 0x44 +const ALL_FLAT_EDGES = 0x77 + +// The closest point of a triangle to a point, and which feature it is on. +struct TrianglePoint { + point: Vec3 + feature: int +} + +// A triangle with its edges and plane, made once per collision. +struct TriangleData { + v1: Vec3 + v2: Vec3 + v3: Vec3 + e1: Vec3 + e2: Vec3 + e3: Vec3 + plane: Plane + flags: int +} + +// --- scratch -------------------------------------------------------------------- + +var g_clip1: ptr = null +var g_clip2: ptr = null +var g_triangle: ptr = null // Vec3[3], the GJK proxy of the triangle + +scratch_ready() { + if g_clip1 == null { + g_clip1 = core.alloc(2 * MAX_CLIP_POINTS * sizeof(ClipVertex)) + g_clip2 = core.alloc(2 * MAX_CLIP_POINTS * sizeof(ClipVertex)) + g_triangle = core.alloc(3 * sizeof(Vec3)) + } +} + +triangle_proxy(v1: Vec3, v2: Vec3, v3: Vec3) -> ShapeProxy { + scratch_ready() + points = g_triangle as Vec3[] + points[0] = v1 + points[1] = v2 + points[2] = v3 + return distance.shape_proxy(g_triangle, 3, 0.0) +} + +triangle_vertex(t: *TriangleData, i: int) -> Vec3 { + if i == 0 { return t.v1 } + if i == 1 { return t.v2 } + return t.v3 +} + +triangle_edge(t: *TriangleData, i: int) -> Vec3 { + if i == 0 { return t.e1 } + if i == 1 { return t.e2 } + return t.e3 +} + +edge_feature(i: int) -> int { + if i == 0 { return FEATURE_EDGE1 } + if i == 1 { return FEATURE_EDGE2 } + return FEATURE_EDGE3 +} + +// --- geometry ------------------------------------------------------------------------- + +// The closest point on the triangle a-b-c to q, by Voronoi regions +// (Real-Time Collision Detection), with the feature it lies on. +closest_point_on_triangle(a: Vec3, b: Vec3, c: Vec3, q: Vec3) -> TrianglePoint { + ab = math.sub(b, a) + ac = math.sub(c, a) + aq = math.sub(q, a) + d1 = math.dot(ab, aq) + d2 = math.dot(ac, aq) + if d1 <= 0.0 && d2 <= 0.0 { return TrianglePoint { point: a, feature: FEATURE_VERTEX1 } } + + bq = math.sub(q, b) + d3 = math.dot(ab, bq) + d4 = math.dot(ac, bq) + if d3 > 0.0 && d4 <= d3 { return TrianglePoint { point: b, feature: FEATURE_VERTEX2 } } + + vc = d1 * d4 - d3 * d2 + if vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0 { + t = d1 / (d1 - d3) + return TrianglePoint { point: math.mul_add(a, t, ab), feature: FEATURE_EDGE1 } + } + + cq = math.sub(q, c) + d5 = math.dot(ab, cq) + d6 = math.dot(ac, cq) + if d6 >= 0.0 && d5 <= d6 { return TrianglePoint { point: c, feature: FEATURE_VERTEX3 } } + + vb = d5 * d2 - d1 * d6 + if vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0 { + t = d2 / (d2 - d6) + return TrianglePoint { point: math.mul_add(a, t, ac), feature: FEATURE_EDGE3 } + } + + va = d3 * d6 - d5 * d4 + if va <= 0.0 && d4 >= d3 && d5 >= d6 { + t = (d4 - d3) / ((d4 - d3) + (d5 - d6)) + return TrianglePoint { point: math.mul_add(b, t, math.sub(c, b)), feature: FEATURE_EDGE2 } + } + + t1 = vb / (va + vb + vc) + t2 = vc / (va + vb + vc) + p = math.mul_add(math.mul_add(a, t1, ab), t2, ac) + return TrianglePoint { point: p, feature: FEATURE_TRIANGLE_FACE } +} + +// The triangle feature the GJK simplex touched, from the vertices of A +// (the triangle) in the cache. +get_triangle_feature(cache: *SimplexCache) -> int { + mask = 0 + i = 0 + while i < cache.count { + mask = mask | (1 << distance.cache_index_a(cache, i)) + i = i + 1 + } + if mask == 1 { return FEATURE_VERTEX1 } + if mask == 2 { return FEATURE_VERTEX2 } + if mask == 3 { return FEATURE_EDGE1 } + if mask == 4 { return FEATURE_VERTEX3 } + if mask == 5 { return FEATURE_EDGE3 } + if mask == 6 { return FEATURE_EDGE2 } + if mask == 7 { return FEATURE_TRIANGLE_FACE } + return FEATURE_NONE +} + +copy_of(c: *SATCache) -> SATCache { + return SATCache { separation: c.separation, kind: c.kind, index_a: c.index_a, index_b: c.index_b, hit: c.hit } +} + +set_point(m: *LocalManifold, i: int, point: Vec3, separation: float, pair: FeaturePair) { + manifold.set_manifold_point(m, i, LocalManifoldPoint { point: point, separation: separation, pair: pair, triangle_index: NULL_INDEX }) +} + +// --- sphere ------------------------------------------------------------------------------- + +// The sphere against the triangle's closest point; the back side culled. +collide_triangle_and_sphere(m: *LocalManifold, capacity: int, v1: Vec3, v2: Vec3, v3: Vec3, sphere_b: Sphere) { + m.point_count = 0 + if capacity == 0 { return } + center = sphere_b.center + plane = math.make_plane_from_points(v1, v2, v3) + if math.plane_separation(plane, center) < 0.0 { return } + + radius = sphere_b.radius + closest = closest_point_on_triangle(v1, v2, v3, center) + squared_distance = math.distance_squared(closest.point, center) + max_distance = radius + SPECULATIVE_DISTANCE + if squared_distance > max_distance * max_distance { return } + + d = math.sqrt_float(squared_distance) + normal = math.normalize(math.cross(math.sub(v2, v1), math.sub(v3, v1))) + if d * d > 1000.0 * FLOAT_MIN { normal = math.mul_sv(1.0 / d, math.sub(center, closest.point)) } + contact = math.mul_sv(0.5, math.add(math.sub(center, math.mul_sv(radius, normal)), closest.point)) + m.normal = normal + m.point_count = 1 + m.feature = closest.feature + m.squared_distance = squared_distance + set_point(m, 0, contact, d - radius, manifold.feature_pair_single()) +} + +// --- capsule ------------------------------------------------------------------------------- + +// The segment against the triangle's three side planes, in place; true +// if two points survive. +clip_segment_to_triangle_face(segment: ClipVertex[], v1: Vec3, v2: Vec3, v3: Vec3, plane: Plane) -> bool { + previous = v3 + i = 0 + while i < 3 { + vertex = v1 + if i == 1 { vertex = v2 } + if i == 2 { vertex = v3 } + tangent = math.normalize(math.sub(vertex, previous)) + binormal = math.cross(tangent, plane.normal) + clip_plane = math.make_plane_from_normal_and_point(binormal, previous) + count = 0 + p1 = segment[0] + p2 = segment[1] + distance1 = math.plane_separation(clip_plane, p1.position) + distance2 = math.plane_separation(clip_plane, p2.position) + if distance1 <= 0.0 { + segment[count] = p1 + count = count + 1 + } + if distance2 <= 0.0 { + segment[count] = p2 + count = count + 1 + } + if (distance1 > 0.0) != (distance2 > 0.0) { + t = distance1 / (distance1 - distance2) + pair = p2.pair + if distance1 > 0.0 { pair = p1.pair } + segment[count] = ClipVertex { position: math.lerp(p1.position, p2.position, t), separation: 0.0, pair: pair } + count = count + 1 + } + if count != 2 { return false } + previous = vertex + i = i + 1 + } + return true +} + +query_triangle_face_and_capsule(plane: Plane, c: Capsule) -> SeparatingAxis { + separation1 = math.plane_separation(plane, c.center1) + separation2 = math.plane_separation(plane, c.center2) + if separation1 < separation2 { + return SeparatingAxis { normal: plane.normal, separation: separation1, index_a: 0, index_b: 0, kind: AXIS_FACE_A } + } + return SeparatingAxis { normal: plane.normal, separation: separation2, index_a: 0, index_b: 1, kind: AXIS_FACE_A } +} + +// The triangle's edges as zero-area faces with a side normal, so the +// edge-edge axis with the capsule points out of the triangle. +query_triangle_and_capsule_edges(v1: Vec3, v2: Vec3, v3: Vec3, plane: Plane, c: Capsule) -> SeparatingAxis { + p1 = c.center1 + capsule_edge = math.sub(c.center2, p1) + max_normal = math.vec3_zero() + max_separation = 0.0 - math.MAX_FLOAT + max_index1 = NULL_INDEX + max_index2 = NULL_INDEX + squared_tolerance = 0.005 * 0.005 + edge_index = 2 + a = math.dot(capsule_edge, plane.normal) + previous = v3 + i = 0 + while i < 3 { + vertex = v1 + if i == 1 { vertex = v2 } + if i == 2 { vertex = v3 } + triangle_edge = math.sub(vertex, previous) + side_normal = math.normalize(math.cross(triangle_edge, plane.normal)) + b = math.dot(capsule_edge, side_normal) + // Parallel to the triangle edge: the face contact handles it. + if a * a + b * b >= squared_tolerance * math.length_squared(capsule_edge) { + axis = math.vec3_zero() + if a * b <= 0.0 { + t = b / (b - a) + axis = math.lerp(side_normal, plane.normal, t) + } else { + t = b / (a + b) + axis = math.lerp(side_normal, math.neg(plane.normal), t) + } + axis = math.normalize(axis) + separation = math.dot(axis, math.sub(p1, previous)) + if separation > max_separation { + max_normal = axis + max_separation = separation + max_index1 = edge_index + max_index2 = 0 + } + } + previous = vertex + edge_index = i + i = i + 1 + } + return SeparatingAxis { normal: max_normal, separation: max_separation, index_a: max_index1, index_b: max_index2, kind: AXIS_EDGE_PAIR } +} + +capsule_segment(c: Capsule) -> ClipVertex[] { + scratch_ready() + segment = g_clip1 as ClipVertex[] + segment[0] = ClipVertex { position: c.center1, separation: 0.0, pair: manifold.make_feature_pair(FEATURE_SHAPE_A, 0, FEATURE_SHAPE_A, 0) } + segment[1] = ClipVertex { position: c.center2, separation: 0.0, pair: manifold.make_feature_pair(FEATURE_SHAPE_A, 1, FEATURE_SHAPE_A, 1) } + return segment +} + +build_triangle_and_capsule_face_contact(m: *LocalManifold, v1: Vec3, v2: Vec3, v3: Vec3, plane: Plane, c: Capsule) { + segment = capsule_segment(c) + if clip_segment_to_triangle_face(segment, v1, v2, v3, plane) == false { return } + radius = c.radius + distance1 = math.plane_separation(plane, segment[0].position) + distance2 = math.plane_separation(plane, segment[1].position) + if distance1 > SPECULATIVE_DISTANCE + radius && distance2 > SPECULATIVE_DISTANCE + radius { return } + point1 = math.mul_sub(segment[0].position, 0.5 * (distance1 + radius), plane.normal) + point2 = math.mul_sub(segment[1].position, 0.5 * (distance2 + radius), plane.normal) + m.normal = plane.normal + m.feature = FEATURE_TRIANGLE_FACE + m.point_count = 2 + set_point(m, 0, point1, distance1 - radius, segment[0].pair) + set_point(m, 1, point2, distance2 - radius, segment[1].pair) +} + +build_triangle_and_capsule_edge_contact(m: *LocalManifold, v1: Vec3, v2: Vec3, v3: Vec3, plane: Plane, c: Capsule, query: SeparatingAxis) { + p1 = c.center1 + capsule_edge = math.sub(c.center2, p1) + va = v1 + vb = v2 + if query.index_a == 1 { + va = v2 + vb = v3 + } else if query.index_a == 2 { + va = v3 + vb = v1 + } + triangle_edge = math.sub(vb, va) + side_normal = math.normalize(math.cross(triangle_edge, plane.normal)) + a = math.dot(capsule_edge, plane.normal) + b = math.dot(capsule_edge, side_normal) + if a * a + b * b < 0.005 * 0.005 * math.length_squared(capsule_edge) { return } + normal = query.normal + result = math.line_distance(va, triangle_edge, p1, capsule_edge) + if result.fraction1 < 0.0 || 1.0 < result.fraction1 || result.fraction2 < 0.0 || 1.0 < result.fraction2 { return } + point = math.lerp(result.point1, math.mul_sub(result.point2, c.radius, normal), 0.5) + separation = math.dot(normal, math.sub(p1, va)) + m.normal = normal + m.point_count = 1 + m.feature = edge_feature(query.index_a) + set_point(m, 0, point, separation - c.radius, manifold.make_feature_pair(FEATURE_SHAPE_A, query.index_a, FEATURE_SHAPE_B, query.index_b)) +} + +// GJK for the shallow case, the segment clipped to the face when the +// closest direction is near the normal; face and edge axes deep. +collide_triangle_and_capsule(m: *LocalManifold, capacity: int, v1: Vec3, v2: Vec3, v3: Vec3, capsule_b: Capsule, cache: *SimplexCache) { + m.point_count = 0 + if capacity < 2 { return } + plane = math.make_plane_from_points(v1, v2, v3) + capsule_center = math.lerp(capsule_b.center1, capsule_b.center2, 0.5) + if math.plane_separation(plane, capsule_center) < 0.0 { return } + + input = DistanceInput { proxy_a: triangle_proxy(v1, v2, v3), proxy_b: distance.shape_proxy((&capsule_b) as ptr, 2, 0.0), + transform: math.transform_identity(), use_radii: false } + output = distance.shape_distance(&input, cache, null, 0) + radius = capsule_b.radius + if output.distance > radius + SPECULATIVE_DISTANCE { return } + + if output.distance > 100.0 * FLOAT_EPSILON { + delta = math.normalize(math.sub(output.point_b, output.point_a)) + if math.abs_float(math.dot(plane.normal, delta)) > 0.2 { + segment = capsule_segment(capsule_b) + if clip_segment_to_triangle_face(segment, v1, v2, v3, plane) { + distance1 = math.plane_separation(plane, segment[0].position) + distance2 = math.plane_separation(plane, segment[1].position) + normal = plane.normal + point1 = math.mul_sub(segment[0].position, 0.5 * (radius + distance1), normal) + point2 = math.mul_sub(segment[1].position, 0.5 * (radius + distance2), normal) + m.normal = normal + m.feature = FEATURE_TRIANGLE_FACE + m.point_count = 2 + set_point(m, 0, point1, distance1 - radius, segment[0].pair) + set_point(m, 1, point2, distance2 - radius, segment[1].pair) + return + } + } + point = math.lerp(output.point_a, math.mul_sub(output.point_b, radius, delta), 0.5) + m.normal = delta + m.point_count = 1 + m.feature = get_triangle_feature(cache) + set_point(m, 0, point, output.distance - radius, manifold.feature_pair_single()) + return + } + + // Deep. + face_query = query_triangle_face_and_capsule(plane, capsule_b) + if face_query.separation > radius { return } + edge_query = query_triangle_and_capsule_edges(v1, v2, v3, plane, capsule_b) + if edge_query.separation > radius { return } + + face_separation = face_query.separation - radius + build_triangle_and_capsule_face_contact(m, v1, v2, v3, plane, capsule_b) + if m.point_count == 2 { + face_separation = math.min_float(manifold.manifold_point(m, 0).separation, manifold.manifold_point(m, 1).separation) + } + if edge_query.index_a == NULL_INDEX { return } + edge_separation = edge_query.separation - radius + if m.point_count == 0 || edge_separation > face_separation + math.LINEAR_SLOP { + build_triangle_and_capsule_edge_contact(m, v1, v2, v3, plane, capsule_b, edge_query) + } +} + +// --- hull ------------------------------------------------------------------------------------- + +triangle_support(t: *TriangleData, direction: Vec3) -> int { + index = 0 + best = math.dot(t.v1, direction) + d = math.dot(t.v2, direction) + if d > best { + best = d + index = 1 + } + if math.dot(t.v3, direction) > best { return 2 } + return index +} + +// The triangle's plane against the hull's deepest vertex. +query_triangle_face(t: *TriangleData, h: *HullData) -> SeparatingAxis { + points = hull.hull_points(h) + vertex_index = hull.find_hull_support_vertex(h, math.neg(t.plane.normal)) + return SeparatingAxis { normal: t.plane.normal, separation: math.plane_separation(t.plane, points[vertex_index]), + index_a: 0, index_b: vertex_index, kind: AXIS_FACE_A } +} + +// The hull's faces against the triangle's vertices; the normal from the +// triangle to the hull. +query_hull_face(t: *TriangleData, h: *HullData) -> SeparatingAxis { + planes = hull.hull_planes(h) + max_normal = math.vec3_zero() + max_separation = 0.0 - math.MAX_FLOAT + max_face = NULL_INDEX + max_vertex = NULL_INDEX + i = 0 + while i < h.face_count { + plane = planes[i] + vertex_index = triangle_support(t, math.neg(plane.normal)) + separation = math.plane_separation(plane, triangle_vertex(t, vertex_index)) + if separation > max_separation { + max_normal = plane.normal + max_separation = separation + max_face = i + max_vertex = vertex_index + } + i = i + 1 + } + return SeparatingAxis { normal: math.neg(max_normal), separation: max_separation, index_a: max_vertex, index_b: max_face, kind: AXIS_FACE_B } +} + +// The edge pairs of triangle (A) and hull (B) whose arcs cross. +query_triangle_and_hull_edges(t: *TriangleData, h: *HullData) -> SeparatingAxis { + result = SeparatingAxis { normal: math.vec3_zero(), separation: 0.0 - math.MAX_FLOAT, index_a: NULL_INDEX, index_b: NULL_INDEX, kind: AXIS_EDGE_PAIR } + tri_normal = t.plane.normal + edges = hull.hull_edges(h) + points = hull.hull_points(h) + planes = hull.hull_planes(h) + squared_tolerance = 0.005 * 0.005 + i = 0 + while i < h.edge_count { + twin = i + 1 + hull_point = points[edges[i].origin] + hull_edge = math.sub(points[edges[twin].origin], hull_point) + hull_normal1 = planes[edges[i].face].normal + hull_normal2 = planes[edges[twin].face].normal + j = 0 + while j < 3 { + tri_edge = triangle_edge(t, j) + cab = math.dot(hull_normal1, tri_edge) + dab = math.dot(hull_normal2, tri_edge) + bcd = math.dot(tri_normal, hull_edge) + if cab * dab < 0.0 && cab * bcd > 0.0 { + if math.max_float(cab * cab, dab * dab) >= squared_tolerance * math.length_squared(tri_edge) { + // The normal points out of the hull by construction. + tt = cab / (cab - dab) + axis = math.normalize(math.lerp(hull_normal1, hull_normal2, tt)) + separation = math.dot(axis, math.sub(triangle_vertex(t, j), hull_point)) + if separation > result.separation { + result.normal = math.neg(axis) + result.separation = separation + result.index_a = j + result.index_b = i + } + } + } + j = j + 1 + } + i = i + 2 + } + return result +} + +// The triangle clipped to a hull face (the reference): the points pushed +// onto the hull face, the pairs flipped, the normal from the triangle to +// the hull. Returns the clipped separation. +collide_hull_face(m: *LocalManifold, point_capacity: int, t: *TriangleData, h: *HullData, query: SeparatingAxis, + cache: *SATCache, enable_speculative: bool) -> float { + m.point_count = 0 + faces = hull.hull_faces(h) + edges = hull.hull_edges(h) + planes = hull.hull_planes(h) + points = hull.hull_points(h) + ref_plane = planes[query.index_b] + scratch_ready() + input = g_clip1 as ClipVertex[] + output = g_clip2 as ClipVertex[] + input[0] = ClipVertex { position: t.v1, separation: math.plane_separation(ref_plane, t.v1), pair: manifold.make_feature_pair(FEATURE_SHAPE_B, 2, FEATURE_SHAPE_B, 0) } + input[1] = ClipVertex { position: t.v2, separation: math.plane_separation(ref_plane, t.v2), pair: manifold.make_feature_pair(FEATURE_SHAPE_B, 0, FEATURE_SHAPE_B, 1) } + input[2] = ClipVertex { position: t.v3, separation: math.plane_separation(ref_plane, t.v3), pair: manifold.make_feature_pair(FEATURE_SHAPE_B, 1, FEATURE_SHAPE_B, 2) } + point_count = 3 + + start = faces[query.index_b] + edge_index = start + while true { + next_index = edges[edge_index].next + vertex1 = points[edges[edge_index].origin] + vertex2 = points[edges[next_index].origin] + tangent = math.normalize(math.sub(vertex2, vertex1)) + binormal = math.cross(tangent, ref_plane.normal) + clip_plane = math.make_plane_from_normal_and_point(binormal, vertex1) + point_count = manifold.clip_polygon(output, input, point_count, clip_plane, edge_index, ref_plane) + if point_count < 3 { + // A stale cache. + manifold.clear_sat_cache(cache) + return query.separation + } + swap = input + input = output + output = swap + edge_index = next_index + if edge_index == start { break } + } + + point_count = math.min_int(point_count, point_capacity) + min_separation = math.MAX_FLOAT + final_count = 0 + i = 0 + while i < point_count { + cp = input[i] + min_separation = math.min_float(min_separation, cp.separation) + if enable_speculative || cp.separation <= 0.0 { + // Onto the hull face, for better culling. + set_point(m, final_count, math.mul_sub(cp.position, cp.separation, ref_plane.normal), cp.separation, manifold.flip_pair(cp.pair)) + final_count = final_count + 1 + } + i = i + 1 + } + speculative = 0.0 + if enable_speculative { speculative = SPECULATIVE_DISTANCE } + if min_separation > speculative { + m.point_count = 0 + manifold.clear_sat_cache(cache) + return min_separation + } + m.point_count = final_count + m.normal = math.neg(ref_plane.normal) + m.feature = FEATURE_HULL_FACE + cache.separation = min_separation + cache.kind = AXIS_FACE_B + cache.index_a = query.index_a + cache.index_b = query.index_b + return min_separation +} + +// The hull's incident face clipped to the triangle (the reference): the +// points where they are, the normal the triangle's. Returns the clipped +// separation, or MAX_FLOAT when everything clipped away. +collide_triangle_face(m: *LocalManifold, point_capacity: int, t: *TriangleData, h: *HullData, query: SeparatingAxis, + cache: *SATCache, enable_speculative: bool) -> float { + faces = hull.hull_faces(h) + edges = hull.hull_edges(h) + points = hull.hull_points(h) + ref_plane = t.plane + inc_face = manifold.find_incident_face(h, ref_plane.normal, query.index_b) + scratch_ready() + input = g_clip1 as ClipVertex[] + output = g_clip2 as ClipVertex[] + point_count = 0 + start = faces[inc_face] + hull_edge_index = start + while true { + next_index = edges[hull_edge_index].next + hull_point = points[edges[next_index].origin] + input[point_count] = ClipVertex { position: hull_point, separation: math.plane_separation(ref_plane, hull_point), + pair: manifold.make_feature_pair(FEATURE_SHAPE_B, hull_edge_index, FEATURE_SHAPE_B, next_index) } + point_count = point_count + 1 + hull_edge_index = next_index + if hull_edge_index == start || point_count >= 2 * MAX_CLIP_POINTS { break } + } + + i = 0 + while i < 3 && point_count > 0 { + side_normal = math.normalize(math.cross(triangle_edge(t, i), ref_plane.normal)) + clip_plane = math.make_plane_from_normal_and_point(side_normal, triangle_vertex(t, i)) + point_count = manifold.clip_polygon(output, input, point_count, clip_plane, i, ref_plane) + swap = input + input = output + output = swap + i = i + 1 + } + if point_count == 0 { + manifold.clear_sat_cache(cache) + return math.MAX_FLOAT + } + + point_count = math.min_int(point_count, point_capacity) + min_separation = math.MAX_FLOAT + final_count = 0 + i = 0 + while i < point_count { + cp = input[i] + min_separation = math.min_float(min_separation, cp.separation) + if enable_speculative || cp.separation <= 0.0 { + set_point(m, final_count, cp.position, cp.separation, cp.pair) + final_count = final_count + 1 + } + i = i + 1 + } + speculative = 0.0 + if enable_speculative { speculative = SPECULATIVE_DISTANCE } + if min_separation >= speculative { + manifold.clear_sat_cache(cache) + return min_separation + } + m.point_count = final_count + m.normal = ref_plane.normal + m.feature = FEATURE_TRIANGLE_FACE + cache.separation = min_separation + cache.kind = AXIS_FACE_A + cache.index_a = query.index_a + cache.index_b = query.index_b + return min_separation +} + +// The edge contact: the closest points of the triangle edge and hull edge +// lines, if within both segments. +collide_triangle_and_hull_edges(m: *LocalManifold, capacity: int, triangle_point: Vec3, triangle_edge_vector: Vec3, h: *HullData, + query: SeparatingAxis, cache: *SATCache) { + edges = hull.hull_edges(h) + points = hull.hull_points(h) + p_b = points[edges[query.index_b].origin] + e_b = math.sub(points[edges[edges[query.index_b].twin].origin], p_b) + result = math.line_distance(triangle_point, triangle_edge_vector, p_b, e_b) + if capacity == 0 || result.fraction1 < 0.0 || 1.0 < result.fraction1 || result.fraction2 < 0.0 || 1.0 < result.fraction2 { + manifold.clear_sat_cache(cache) + return + } + separation = math.dot(query.normal, math.sub(p_b, triangle_point)) + point = math.mul_sv(0.5, math.add(result.point1, result.point2)) + set_point(m, 0, point, separation, manifold.make_feature_pair(FEATURE_SHAPE_A, query.index_a, FEATURE_SHAPE_B, query.index_b)) + cache.separation = separation + cache.kind = AXIS_EDGE_PAIR + cache.index_a = query.index_a + cache.index_b = query.index_b + m.normal = query.normal + m.point_count = 1 + m.feature = edge_feature(query.index_a) +} + +// A triangle (in the hull's frame) against the hull. The cache's last +// axis is tried first; then the axis of least penetration among the +// triangle's face, the hull's faces and the edge pairs, with the hull +// face refused as reference when it opposes the triangle; the edge +// contact when the face finds nothing or is clearly worse; GJK last. +collide_triangle_and_hull(m: *LocalManifold, capacity: int, v1: Vec3, v2: Vec3, v3: Vec3, triangle_flags: int, + hull_b: *HullData, cache: *SATCache, enable_speculative: bool) { + m.point_count = 0 + m.feature = FEATURE_NONE + if capacity < 4 { return } + plane = math.make_plane_from_points(v1, v2, v3) + linear_slop = math.LINEAR_SLOP + + offset = math.plane_separation(plane, hull_b.center) + if cache.kind == AXIS_BACKSIDE { + // Hysteresis against jitter on wavy meshes. + if math.abs_float(cache.separation - offset) < linear_slop { return } + cache.kind = AXIS_INVALID + } + if offset < 0.0 - linear_slop { + cache.kind = AXIS_BACKSIDE + cache.separation = offset + return + } + + t = TriangleData { v1: v1, v2: v2, v3: v3, e1: math.sub(v2, v1), e2: math.sub(v3, v2), e3: math.sub(v1, v3), plane: plane, flags: triangle_flags } + edges = hull.hull_edges(hull_b) + planes = hull.hull_planes(hull_b) + points = hull.hull_points(hull_b) + speculative = 0.0 + if enable_speculative { speculative = SPECULATIVE_DISTANCE } + cache.hit = 1 + + if cache.kind == AXIS_FACE_A { + vertex_index = hull.find_hull_support_vertex(hull_b, math.neg(plane.normal)) + separation = math.plane_separation(plane, points[vertex_index]) + if separation > speculative { return } + face_query = SeparatingAxis { normal: plane.normal, separation: separation, index_a: cache.index_a, index_b: vertex_index, kind: AXIS_FACE_A } + local_cache = copy_of(cache) + clipped = collide_triangle_face(m, capacity, &t, hull_b, face_query, &local_cache, enable_speculative) + if m.point_count > 0 && math.abs_float(cache.separation - clipped) < linear_slop { return } + m.point_count = 0 + manifold.clear_sat_cache(cache) + } else if cache.kind == AXIS_FACE_B { + hull_plane = planes[cache.index_b] + vertex_index = triangle_support(&t, math.neg(hull_plane.normal)) + separation = math.plane_separation(hull_plane, triangle_vertex(&t, vertex_index)) + if separation > speculative { return } + // A deep overlap may have made the cache wrong; do not persist it. + if separation >= 0.0 - 2.0 * linear_slop { + face_query = SeparatingAxis { normal: math.neg(hull_plane.normal), separation: separation, index_a: vertex_index, index_b: cache.index_b, kind: AXIS_FACE_B } + local_cache = copy_of(cache) + clipped = collide_hull_face(m, capacity, &t, hull_b, face_query, &local_cache, enable_speculative) + if m.point_count > 0 && math.abs_float(cache.separation - clipped) < linear_slop { return } + } + m.point_count = 0 + manifold.clear_sat_cache(cache) + } else if cache.kind == AXIS_EDGE_PAIR { + index_a = cache.index_a + tri_point = triangle_vertex(&t, index_a) + tri_edge = triangle_edge(&t, index_a) + index_b = cache.index_b + hull_point = points[edges[index_b].origin] + hull_edge = math.sub(points[edges[index_b + 1].origin], hull_point) + hull_normal1 = planes[edges[index_b].face].normal + hull_normal2 = planes[edges[index_b + 1].face].normal + cab = math.dot(hull_normal1, tri_edge) + dab = math.dot(hull_normal2, tri_edge) + bcd = math.dot(plane.normal, hull_edge) + if cab * dab < 0.0 && cab * bcd > 0.0 { + if math.max_float(cab * cab, dab * dab) >= 0.005 * 0.005 * math.length_squared(tri_edge) { + tt = cab / (cab - dab) + axis = math.normalize(math.lerp(hull_normal1, hull_normal2, tt)) + separation = math.dot(axis, math.sub(tri_point, hull_point)) + if separation > speculative { return } + if math.abs_float(cache.separation - separation) < linear_slop { + edge_query = SeparatingAxis { normal: math.neg(axis), separation: separation, index_a: index_a, index_b: index_b, kind: AXIS_EDGE_PAIR } + local_cache = copy_of(cache) + collide_triangle_and_hull_edges(m, capacity, tri_point, tri_edge, hull_b, edge_query, &local_cache) + if m.point_count > 0 { return } + } + } + } + manifold.clear_sat_cache(cache) + } else if cache.kind == AXIS_MANUAL_FACE_A { + query = query_triangle_face(&t, hull_b) + collide_triangle_face(m, capacity, &t, hull_b, query, cache, enable_speculative) + return + } else if cache.kind == AXIS_MANUAL_FACE_B { + query = query_hull_face(&t, hull_b) + collide_hull_face(m, capacity, &t, hull_b, query, cache, enable_speculative) + return + } else if cache.kind == AXIS_MANUAL_EDGE_PAIR { + query = query_triangle_and_hull_edges(&t, hull_b) + if query.index_a != NULL_INDEX { + collide_triangle_and_hull_edges(m, capacity, triangle_vertex(&t, query.index_a), triangle_edge(&t, query.index_a), hull_b, query, cache) + } + return + } + + cache.hit = 0 + + face_query_a = query_triangle_face(&t, hull_b) + if face_query_a.separation > speculative { + cache.separation = face_query_a.separation + cache.kind = AXIS_FACE_A + cache.index_a = face_query_a.index_a + cache.index_b = face_query_a.index_b + return + } + face_query_b = query_hull_face(&t, hull_b) + if face_query_b.separation > speculative { + cache.separation = face_query_b.separation + cache.kind = AXIS_FACE_B + cache.index_a = face_query_b.index_a + cache.index_b = face_query_b.index_b + return + } + edge_query = query_triangle_and_hull_edges(&t, hull_b) + if edge_query.separation > speculative { + cache.separation = edge_query.separation + cache.kind = AXIS_EDGE_PAIR + cache.index_a = edge_query.index_a + cache.index_b = edge_query.index_b + return + } + + // A hull face significantly opposed to the triangle is a ghost collision. + pushing_down = math.dot(face_query_b.normal, plane.normal) < 0.0 - 0.25 + clip_separation = 0.0 + if face_query_b.separation >= face_query_a.separation && pushing_down == false { + clip_separation = collide_hull_face(m, capacity, &t, hull_b, face_query_b, cache, enable_speculative) + } else { + clip_separation = collide_triangle_face(m, capacity, &t, hull_b, face_query_a, cache, enable_speculative) + } + + if edge_query.index_a != NULL_INDEX { + // With aligned axes the edge separation can be garbage; with a + // positive face separation there may be no points. + max_face_separation = math.max_float(face_query_a.separation, face_query_b.separation) + if (m.point_count == 0 && edge_query.separation > max_face_separation) || + (m.point_count == 1 && edge_query.separation > clip_separation + linear_slop) { + m.point_count = 0 + collide_triangle_and_hull_edges(m, capacity, triangle_vertex(&t, edge_query.index_a), triangle_edge(&t, edge_query.index_a), hull_b, edge_query, cache) + } + } + + // Speculation can leave the SAT with no points; GJK then, so nothing tunnels. + if m.point_count == 0 { + input = DistanceInput { proxy_a: triangle_proxy(v1, v2, v3), proxy_b: hull.hull_proxy(hull_b), + transform: math.transform_identity(), use_radii: false } + simplex_cache = distance.empty_cache() + output = distance.shape_distance(&input, &simplex_cache, null, 0) + if output.distance > 0.0 { + m.point_count = 1 + m.feature = get_triangle_feature(&simplex_cache) + m.normal = output.normal + set_point(m, 0, output.point_b, output.distance, manifold.feature_pair_single()) + } + manifold.clear_sat_cache(cache) + } +} diff --git a/bench/RESULTS.md b/bench/RESULTS.md index 7ffcdcc..2e19568 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -169,3 +169,25 @@ the cache warm -- the state a resting stack is in -- the port is at parity; the cold separating axis test is 1.6x, the reference's being SIMD four edge pairs at a time, which is the wide path to consider if a step benchmark ever shows the cold SAT. + +## triangle_manifold + +`bench/triangle.ae` and `bench/triangle_box3d.c`: 20,000 triangle-hull +collisions of a box at a hundred attitudes sinking onto a big triangle +with a cold SAT cache each time; 20,000 of a box resting on a creeping +triangle with the cache carried; 20,000 triangle-capsule collisions of a +capsule swung across a triangle and its edge; 20,000 triangle-sphere +collisions of a sphere swept over the triangle and off it. + +| phase | aephysics | Box3D | +|---|---|---| +| 20,000 triangle-hull, cold cache | 2.8 ms | **2.6** | +| 20,000 triangle-hull, warm cache | 2.6 | **2.4** | +| 20,000 triangle-capsule | 5.1 | **3.8** | +| 20,000 triangle-sphere | **0.32** | 0.61 | + +The same manifolds come out: 39,536 / 80,000 / 29,778 / 6,422 contact +points, 19,999 cache hits, equal separation sums. Triangle against hull +is within 10% of the reference both cold and warm; the capsule is 1.35x +(its GJK); the sphere, which is the closest point on a triangle and +nothing else, is faster here. diff --git a/bench/triangle.ae b/bench/triangle.ae new file mode 100644 index 0000000..be5386f --- /dev/null +++ b/bench/triangle.ae @@ -0,0 +1,137 @@ +// The triangle manifolds on the same scenes as bench/triangle_box3d.c: +// 20,000 triangle-hull collisions of a tipped box rolling over a triangle, +// cold cache, then the same with the cache carried; 20,000 +// triangle-capsule and 20,000 triangle-sphere collisions along a sweep. +// Single thread, wall time per phase, with the point counts and +// separation sums as the checksum. +import std.string +import std.os +import aephysics.math +import aephysics.distance +import aephysics.hull +import aephysics.manifold +import aephysics.triangle_manifold + +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 N = 20000 + +exact_quat(axis: Vec3, radians: float) -> Quat { + h = 0.5 * radians + return Quat { v: math.mul_sv(sin(h), axis), s: cos(h) } +} + +sum_separations(m: *LocalManifold) -> float { + s = 0.0 + k = 0 + while k < m.point_count { + s = s + manifold.manifold_point(m, k).separation + k = k + 1 + } + return s +} + +main() { + axis = math.normalize(math.vec3(1.0, 0.3, 0.5)) + points = calloc(8, sizeof(LocalManifoldPoint)) + + // A box at a hundred attitudes (built outside the timing) sinking onto a + // big triangle, the triangle in the hull's frame. + hulls_block = calloc(100, 8) + hulls = hulls_block as ptr[] + i = 0 + while i < 100 { + hulls[i] = hull.make_transformed_box_hull(0.5, 0.5, 0.5, Transform { p: math.vec3_zero(), q: exact_quat(axis, 0.015 * (i as float)) }) as ptr + i = i + 1 + } + points_cold = 0 + sep_cold = 0.0 + t0 = clock() + i = 0 + while i < N { + t = (i as float) / (N as float) + y = 0.0 - 0.8 + 0.1 * sin(20.0 * t) + v1 = math.vec3(0.0 - 3.0 + 0.5 * t, y, 0.0 - 2.0) + v2 = math.vec3(0.0, y, 4.0) + v3 = math.vec3(3.0, y + 0.02 * t, 0.0 - 2.0) + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + triangle_manifold.collide_triangle_and_hull(&m, 8, v1, v2, v3, 0, hulls[i % 100] as *HullData, &cache, true) + points_cold = points_cold + m.point_count + sep_cold = sep_cold + sum_separations(&m) + i = i + 1 + } + t1 = clock() + + points_warm = 0 + hits = 0 + sep_warm = 0.0 + warm = manifold.empty_sat_cache() + hull_w = hull.make_transformed_box_hull(0.5, 0.5, 0.5, Transform { p: math.vec3_zero(), q: exact_quat(axis, 0.3) }) + i = 0 + while i < N { + t = (i as float) / (N as float) + // Resting on the triangle, the triangle creeping: the cache's steady state. + y = 0.0 - 0.66 + 0.004 * sin(20.0 * t) + v1 = math.vec3(0.0 - 3.0 + 0.5 * t, y, 0.0 - 2.0) + v2 = math.vec3(0.0, y, 4.0) + v3 = math.vec3(3.0, y + 0.002 * t, 0.0 - 2.0) + m = manifold.local_manifold(points) + triangle_manifold.collide_triangle_and_hull(&m, 8, v1, v2, v3, 0, hull_w, &warm, true) + points_warm = points_warm + m.point_count + hits = hits + warm.hit + sep_warm = sep_warm + sum_separations(&m) + i = i + 1 + } + t2 = clock() + + points_capsule = 0 + sep_capsule = 0.0 + tv1 = math.vec3(0.0 - 2.0, 0.0, 0.0) + tv2 = math.vec3(2.0, 0.0, 0.0) + tv3 = math.vec3(0.0, 0.0, 0.0 - 2.0) + i = 0 + while i < N { + t = (i as float) / (N as float) + dir = math.normalize(math.vec3(sin(2.0 * t), 0.3 * sin(7.0 * t), cos(2.0 * t))) + mid = math.vec3(0.3 * cos(3.0 * t), 0.25 - 0.3 * t, 0.0 - 0.6 + 0.6 * t) + c = manifold.capsule(math.mul_add(mid, 0.0 - 0.6, dir), math.mul_add(mid, 0.6, dir), 0.1) + m = manifold.local_manifold(points) + simplex_cache = distance.empty_cache() + triangle_manifold.collide_triangle_and_capsule(&m, 8, tv1, tv2, tv3, c, &simplex_cache) + points_capsule = points_capsule + m.point_count + sep_capsule = sep_capsule + sum_separations(&m) + i = i + 1 + } + t3 = clock() + + points_sphere = 0 + sep_sphere = 0.0 + i = 0 + while i < N { + t = (i as float) / (N as float) + s = manifold.sphere(math.vec3(2.5 * sin(5.0 * t), 0.3 - 0.25 * t, 0.0 - 2.5 + 3.0 * t), 0.2) + m = manifold.local_manifold(points) + triangle_manifold.collide_triangle_and_sphere(&m, 8, tv1, tv2, tv3, s) + points_sphere = points_sphere + m.point_count + sep_sphere = sep_sphere + sum_separations(&m) + i = i + 1 + } + t4 = clock() + + println("aephysics triangle: ${N} triangle-hull cold ${ms(t1 - t0)} ms (${points_cold} points, sum ${sep_cold}), warm ${ms(t2 - t1)} ms (${points_warm} points, sum ${sep_warm}, ${hits} cache hits), ${N} triangle-capsule ${ms(t3 - t2)} ms (${points_capsule} points, sum ${sep_capsule}), ${N} triangle-sphere ${ms(t4 - t3)} ms (${points_sphere} points, sum ${sep_sphere})") + hull.destroy_hull(hull_w) + i = 0 + while i < 100 { + hull.destroy_hull(hulls[i] as *HullData) + i = i + 1 + } + free(hulls_block) + free(points) +} diff --git a/bench/triangle_box3d.c b/bench/triangle_box3d.c new file mode 100644 index 0000000..1646448 --- /dev/null +++ b/bench/triangle_box3d.c @@ -0,0 +1,114 @@ +// The triangle manifolds of the reference on the same scenes as +// bench/triangle.ae: 20,000 triangle-hull collisions of a tipped box +// rolling over a triangle, cold cache, then the same with the cache +// carried; 20,000 triangle-capsule and 20,000 triangle-sphere collisions +// along a sweep. Single thread, wall time per phase, with the point +// counts and separation sums as the checksum. +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#include +#include +#include + +static double now_ms( void ) +{ + struct timespec ts; + timespec_get( &ts, TIME_UTC ); + return ts.tv_sec * 1000.0 + ts.tv_nsec / 1.0e6; +} + +static b3Quat exact_quat( b3Vec3 axis, float radians ) +{ + float h = 0.5f * radians, s = sinf( h ); + return (b3Quat){ { s * axis.x, s * axis.y, s * axis.z }, cosf( h ) }; +} + +#define N 20000 + +int main( void ) +{ + b3Vec3 axis = b3Normalize( (b3Vec3){ 1.0f, 0.3f, 0.5f } ); + b3LocalManifoldPoint points[8]; + + // A box at a hundred attitudes (built outside the timing) sinking onto a + // big triangle, the triangle in the hull's frame. + static b3BoxHull hulls[100]; + for ( int i = 0; i < 100; ++i ) + { + hulls[i] = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, (b3Transform){ b3Vec3_zero, exact_quat( axis, 0.015f * i ) } ); + } + int pointsCold = 0; + double sepCold = 0.0; + double t0 = now_ms(); + for ( int i = 0; i < N; ++i ) + { + float t = (float)i / (float)N; + float y = -0.8f + 0.1f * sinf( 20.0f * t ); + b3Vec3 v1 = { -3.0f + 0.5f * t, y, -2.0f }, v2 = { 0.0f, y, 4.0f }, v3 = { 3.0f, y + 0.02f * t, -2.0f }; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hulls[i % 100].base, &cache, true ); + pointsCold += manifold.pointCount; + for ( int k = 0; k < manifold.pointCount; ++k ) sepCold += points[k].separation; + } + double t1 = now_ms(); + + int pointsWarm = 0, hits = 0; + double sepWarm = 0.0; + b3SATCache warm = { 0 }; + b3BoxHull hullW = b3MakeTransformedBoxHull( 0.5f, 0.5f, 0.5f, (b3Transform){ b3Vec3_zero, exact_quat( axis, 0.3f ) } ); + for ( int i = 0; i < N; ++i ) + { + float t = (float)i / (float)N; + // Resting on the triangle, the triangle creeping: the cache's steady state. + float y = -0.66f + 0.004f * sinf( 20.0f * t ); + b3Vec3 v1 = { -3.0f + 0.5f * t, y, -2.0f }, v2 = { 0.0f, y, 4.0f }, v3 = { 3.0f, y + 0.002f * t, -2.0f }; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3CollideTriangleAndHull( &manifold, 8, v1, v2, v3, 0, &hullW.base, &warm, true ); + pointsWarm += manifold.pointCount; + hits += warm.hit; + for ( int k = 0; k < manifold.pointCount; ++k ) sepWarm += points[k].separation; + } + double t2 = now_ms(); + + int pointsCapsule = 0; + double sepCapsule = 0.0; + b3Vec3 tv1 = { -2.0f, 0.0f, 0.0f }, tv2 = { 2.0f, 0.0f, 0.0f }, tv3 = { 0.0f, 0.0f, -2.0f }; + for ( int i = 0; i < N; ++i ) + { + float t = (float)i / (float)N; + b3Vec3 dir = b3Normalize( (b3Vec3){ sinf( 2.0f * t ), 0.3f * sinf( 7.0f * t ), cosf( 2.0f * t ) } ); + b3Vec3 mid = { 0.3f * cosf( 3.0f * t ), 0.25f - 0.3f * t, -0.6f + 0.6f * t }; + b3Capsule capsule = { b3MulAdd( mid, -0.6f, dir ), b3MulAdd( mid, 0.6f, dir ), 0.1f }; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SimplexCache cache = { 0 }; + b3CollideTriangleAndCapsule( &manifold, 8, (b3Vec3[]){ tv1, tv2, tv3 }, &capsule, &cache ); + pointsCapsule += manifold.pointCount; + for ( int k = 0; k < manifold.pointCount; ++k ) sepCapsule += points[k].separation; + } + double t3 = now_ms(); + + int pointsSphere = 0; + double sepSphere = 0.0; + for ( int i = 0; i < N; ++i ) + { + float t = (float)i / (float)N; + b3Sphere sphere = { { 2.5f * sinf( 5.0f * t ), 0.3f - 0.25f * t, -2.5f + 3.0f * t }, 0.2f }; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3CollideTriangleAndSphere( &manifold, 8, (b3Vec3[]){ tv1, tv2, tv3 }, &sphere ); + pointsSphere += manifold.pointCount; + for ( int k = 0; k < manifold.pointCount; ++k ) sepSphere += points[k].separation; + } + double t4 = now_ms(); + + printf( "box3d triangle: %d triangle-hull cold %.2f ms (%d points, sum %.3f), warm %.2f ms (%d points, sum %.3f, %d cache hits), " + "%d triangle-capsule %.2f ms (%d points, sum %.3f), %d triangle-sphere %.2f ms (%d points, sum %.3f)\n", + N, t1 - t0, pointsCold, sepCold, t2 - t1, pointsWarm, sepWarm, hits, N, t3 - t2, pointsCapsule, sepCapsule, N, t4 - t3, + pointsSphere, sepSphere ); + return 0; +} diff --git a/design.md b/design.md index a135589..63b5610 100644 --- a/design.md +++ b/design.md @@ -55,22 +55,32 @@ started until its tests pass. sphere and capsule parts. The same manifolds as the reference; warm cache at parity, cold SAT 1.6x. Found aether#2119 (array literals of float expressions typed as int) on the way. -7. **collision, static**: `triangle_manifold`, `mesh`, `height_field`, - `shape` (mass properties, ray and shape casts per shape). Tests: - `test_collision`, the triangle parts of `test_manifold`, `test_shape`, - `test_mesh`, `test_height_field`. -8. **dynamics**: `body`, `contact`, `constraint_graph` (graph colouring), +7. **triangle_manifold** (done): triangle_manifold.c as + `aephysics.triangle_manifold`, with closest_point_on_triangle and the + triangle features. 1,540 checks: the triangle parts of test_manifold.c + (the tipped cube's edge on a tilted triangle edge, the parallel pair + through the threshold, the edge sweep against the cross product, the + capsule across an edge and straddling the face) plus the sphere on + every feature, the resting cube, the back-side hysteresis and the + cache. The same manifolds as the reference within 10% on hulls. + aetherc does not resolve a module constant inside a struct literal in + a return statement; the constants are restated locally. +8. **collision, static**: `mesh` (the BVH, the edge flags, the mesh + contact with its ghost-collision reduction), `height_field`, `shape` + (mass properties, ray and shape casts per shape). Tests: + `test_collision`, `test_shape`, `test_mesh`, `test_height_field`. +9. **dynamics**: `body`, `contact`, `constraint_graph` (graph colouring), `solver_set`, `island`, `solver` (the Soft Step: sub-stepping, relax iterations, restitution), `contact_solver` (scalar first; the wide SIMD path second, measured), the joints (revolute, prismatic, distance, motor, weld, wheel, spherical), `sensor`, `mover` (the character mover), `physics_world`. Tests: `test_body`, `test_joint`, `test_world`, `test_mover`, `test_determinism`, `test_large_world`. -9. **parallel**: `parallel_for` and the scheduler over Aether's actors; +10. **parallel**: `parallel_for` and the scheduler over Aether's actors; the benchmarks by thread count as the original records them. -10. **recording and replay**, `world_snapshot`: last, since they are the +11. **recording and replay**, `world_snapshot`: last, since they are the tooling and not the engine. -11. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run +12. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run against the C build on the same machine, recorded under `benchmark/`. ## Measures