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