From 27ba972d2340bcf44bf430f948cd97cab950c676 Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Sat, 19 Sep 2026 18:04:48 -0300 Subject: [PATCH] aephysics.manifold: contact manifolds for spheres, capsules and hulls Box3D's manifold.c and convex_manifold.c in Aether: the separating axis test over two hulls (A's faces, B's faces, the edge pairs whose Gauss-map arcs cross, the reference's scalar path exactly), the SAT cache that answers when the last axis still separates or still builds the same contact, the incident face from the closest vertex, Sutherland-Hodgman clipping to the reference face's side planes with feature pairs on every point, the reduction to four points with the reference's tie-break, the face contact by preference and the edge contact when the face finds nothing or the edge axis is clearly better; and the sphere-sphere, capsule-sphere, hull-sphere (GJK shallow, deepest face deep), capsule-capsule (two points when parallel) and hull-capsule (GJK shallow with the segment clipped to the support face; face and edge axes deep) colliders. The clip buffers are module scratch until the parallel layer gives each worker its own. test_manifold.ae: 43,090 checks from test_sat.c (the axis against a brute-force oracle over 7,000 random box pairs, offset hulls included) and the hull, sphere and capsule parts of test_manifold.c (crossed edges into deep overlap, the edge cache, the endpoint, parallel edges at the noise floor, overlap never empty, the ridge-crossing policy, the edge axis oracles, the capsule through an edge, the sphere and capsule seams, the capsule's deep face contact). On the way: aether#2119, an array literal of float expressions is typed as int. bench/manifold.ae against bench/manifold_box3d.c: the same manifolds (point counts, cache hits and separation sums equal on every phase); warm-cache hull-hull at parity, cold SAT 1.6x, capsule and sphere 1.5x. --- README.md | 3 +- aephysics/manifold/module.ae | 1276 ++++++++++++++++++++++++++++++++++ aephysics/test_manifold.ae | 901 ++++++++++++++++++++++++ bench/RESULTS.md | 22 + bench/manifold.ae | 114 +++ bench/manifold_box3d.c | 105 +++ design.md | 24 +- 7 files changed, 2435 insertions(+), 10 deletions(-) create mode 100644 aephysics/manifold/module.ae create mode 100644 aephysics/test_manifold.ae create mode 100644 bench/manifold.ae create mode 100644 bench/manifold_box3d.c diff --git a/README.md b/README.md index 307c684..176bd04 100644 --- a/README.md +++ b/README.md @@ -30,7 +30,8 @@ so a test written against the reference reads the same here. | `aephysics.dynamic_tree` | the bounding volume hierarchy under the broad phase: SAH insertion, rotations, enlarge, sweep refit, partial rebuild in depth-first order, box / closest / ray / swept-box queries | done, `test_dynamic_tree.ae` (12k checks); [same tree as the reference, ray cast 1.9x its time](bench/RESULTS.md#dynamic_tree) | | `aephysics.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.collision` | contact manifolds, triangle mesh, height field, shapes with mass properties, ray and shape casts | next | +| `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.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/manifold/module.ae b/aephysics/manifold/module.ae new file mode 100644 index 0000000..ddd6034 --- /dev/null +++ b/aephysics/manifold/module.ae @@ -0,0 +1,1276 @@ +// aephysics.manifold -- contact manifolds between convex shapes: sphere, +// capsule and hull in every pairing, the separating axis test over two +// hulls with its cache, reference-face clipping and the reduction of the +// clipped polygon to four points. +// +// The shape is Box3D's manifold.c and convex_manifold.c (Erin Catto, with +// portions by Dirk Gregorius, MIT), the reference this engine is measured +// against: GJK for the shallow cases, the separating axis test (face +// normals of each hull, then the edge pairs whose Gauss-map arcs cross) +// for the deep ones, the incident face found from the closest vertex, +// Sutherland-Hodgman clipping against the reference face's side planes, +// the feature pair on every point so the solver can match it across +// steps, and the SAT cache that skips the full test when the last axis +// still holds. Names are the reference's without its prefix, in snake +// case: b3CollideHulls is collide_hulls. +// +// Differences: the axis test is the reference's scalar path (its SIMD +// path is the same arithmetic four edges at a time); the clip buffers +// are module scratch rather than the C stack, so one thread collides at +// a time until the parallel layer gives each worker its own; the reduction +// to four points is the reference's default (the 2D-hull alternative is +// there for larger manifolds). +import std.string +import aephysics.math +import aephysics.core +import aephysics.distance +import aephysics.hull + +exports ( + Sphere, Capsule, FeaturePair, LocalManifoldPoint, LocalManifold, SATCache, SeparatingAxis, AxisQuery, + ClipVertex, + MAX_MANIFOLD_POINTS, MAX_CLIP_POINTS, SPECULATIVE_DISTANCE, PARALLEL_EDGE_TOL, NULL_INDEX, + AXIS_INVALID, AXIS_BACKSIDE, AXIS_FACE_A, AXIS_FACE_B, AXIS_EDGE_PAIR, AXIS_CLOSEST_POINTS, + AXIS_MANUAL_FACE_A, AXIS_MANUAL_FACE_B, AXIS_MANUAL_EDGE_PAIR, + FEATURE_SHAPE_A, FEATURE_SHAPE_B, + sphere, capsule, make_feature_pair, feature_pair_single, flip_pair, make_feature_id, + local_manifold, empty_sat_cache, clear_sat_cache, copy_sat_cache, empty_point, manifold_point, set_manifold_point, + find_incident_face, clip_polygon, validate_polygon, compute_separating_axis, get_best_axis, + collide_spheres, collide_capsule_and_sphere, collide_hull_and_sphere, collide_capsules, + collide_hull_and_capsule, collide_hulls, reduce_manifold_points +) + +const NULL_INDEX = 0 - 1 +const MAX_MANIFOLD_POINTS = 4 +const MAX_CLIP_POINTS = 64 +// Four linear slops: contacts start this far apart, so the solver sees them coming. +const SPECULATIVE_DISTANCE = 0.02 +// Edge pairs whose sine is below this are not an axis. +const PARALLEL_EDGE_TOL = 0.005 + +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_CLOSEST_POINTS = 5 +// For testing: force the axis. +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 FLOAT_EPSILON = 0.00000011920929 +const FLOAT_MIN = 0.000000000000000000000000000000000000011754944 +const MIN_CAPSULE_LENGTH = 0.005 + +struct Sphere { + center: Vec3 + radius: float +} + +// Two hemispheres joined by a cylinder. +struct Capsule { + center1: Vec3 + center2: Vec3 + radius: float +} + +// A contact point is where two edges cross: two of one shape (a vertex) +// or one of each. The pair identifies the point across steps. +struct FeaturePair { + owner1: int + index1: int + owner2: int + index2: int +} + +// A contact point in frame A. +struct LocalManifoldPoint { + point: Vec3 + separation: float // negative when overlapping + pair: FeaturePair + triangle_index: int +} + +// A manifold in frame A, with the points in a buffer the caller owns. +struct LocalManifold { + normal: Vec3 // from A to B, in frame A + triangle_normal: Vec3 + points: ptr // LocalManifoldPoint[] + point_count: int + triangle_index: int + i1: int + i2: int + i3: int + squared_distance: float + feature: int + triangle_flags: int +} + +// The last separating axis, for temporal coherence. +struct SATCache { + separation: float + kind: int // AXIS_* + index_a: int + index_b: int + hit: int // the cache answered +} + +struct SeparatingAxis { + normal: Vec3 + separation: float + index_a: int + index_b: int + kind: int +} + +struct AxisQuery { + face_a: SeparatingAxis + face_b: SeparatingAxis + edge: SeparatingAxis + separated_feature: int +} + +struct ClipVertex { + position: Vec3 + separation: float + pair: FeaturePair +} + +sphere(center: Vec3, radius: float) -> Sphere { return Sphere { center: center, radius: radius } } + +capsule(center1: Vec3, center2: Vec3, radius: float) -> Capsule { + return Capsule { center1: center1, center2: center2, radius: radius } +} + +make_feature_pair(owner1: int, index1: int, owner2: int, index2: int) -> FeaturePair { + return FeaturePair { owner1: owner1, index1: index1, owner2: owner2, index2: index2 } +} + +// For a single point contact: sphere against anything. +feature_pair_single() -> FeaturePair { return FeaturePair { owner1: 0, index1: 0, owner2: 0, index2: 0 } } + +// Choosing face A or B as the reference does not change the pair, so +// impulses persist through a reference-face flip-flop. +flip_pair(pair: FeaturePair) -> FeaturePair { + return FeaturePair { owner1: 1 - pair.owner2, index1: pair.index2, owner2: 1 - pair.owner1, index2: pair.index1 } +} + +make_feature_id(pair: FeaturePair) -> int { + return (pair.owner1 << 24) | (pair.index1 << 16) | (pair.owner2 << 8) | pair.index2 +} + +empty_point() -> LocalManifoldPoint { + return LocalManifoldPoint { point: math.vec3_zero(), separation: 0.0, pair: feature_pair_single(), triangle_index: NULL_INDEX } +} + +// A manifold over a point buffer of the given capacity, which the caller allocated. +local_manifold(points: ptr) -> LocalManifold { + return LocalManifold { normal: math.vec3_zero(), triangle_normal: math.vec3_zero(), points: points, point_count: 0, + triangle_index: NULL_INDEX, i1: 0, i2: 0, i3: 0, squared_distance: 0.0, feature: 0, triangle_flags: 0 } +} + +empty_sat_cache() -> SATCache { return SATCache { separation: 0.0, kind: AXIS_INVALID, index_a: 0, index_b: 0, hit: 0 } } + +clear_sat_cache(c: *SATCache) { + c.separation = 0.0 + c.kind = AXIS_INVALID + c.index_a = 0 + c.index_b = 0 + c.hit = 0 +} + +copy_sat_cache(dst: *SATCache, src: SATCache) { + dst.separation = src.separation + dst.kind = src.kind + dst.index_a = src.index_a + dst.index_b = src.index_b + dst.hit = src.hit +} + +clear_simplex_cache(c: *SimplexCache) { + c.metric = 0.0 + c.count = 0 +} + +manifold_point(m: *LocalManifold, i: int) -> LocalManifoldPoint { + points = m.points as LocalManifoldPoint[] + return points[i] +} + +set_manifold_point(m: *LocalManifold, i: int, p: LocalManifoldPoint) { + points = m.points as LocalManifoldPoint[] + points[i] = p +} + +// --- scratch ------------------------------------------------------------------------ + +// The clip polygons and the unreduced points, allocated once. One collision +// at a time until the parallel layer gives each worker its own. +var g_clip1: ptr = null +var g_clip2: ptr = null +var g_clip_points: ptr = null + +scratch_ready() { + if g_clip1 == null { + g_clip1 = core.alloc(MAX_CLIP_POINTS * sizeof(ClipVertex)) + g_clip2 = core.alloc(MAX_CLIP_POINTS * sizeof(ClipVertex)) + g_clip_points = core.alloc(MAX_CLIP_POINTS * sizeof(LocalManifoldPoint)) + } +} + +// --- the incident face and clipping ---------------------------------------------------- + +// The face of the hull most anti-parallel to the reference normal, found +// from the closest vertex: the edge out of it most perpendicular to the +// normal, then the more anti-parallel of that edge's two faces. Extended +// from the plain search so a wedge gets the right incident face. +find_incident_face(h: *HullData, ref_normal: Vec3, vertex_index: int) -> int { + vertices = hull.hull_vertices(h) + edges = hull.hull_edges(h) + planes = hull.hull_planes(h) + points = hull.hull_points(h) + min_edge = NULL_INDEX + min_projection = math.MAX_FLOAT + start = vertices[vertex_index] + edge_index = start + edge_origin = points[edges[start].origin] + while true { + twin = edges[edge_index].twin + axis = math.normalize(math.sub(points[edges[twin].origin], edge_origin)) + projection = math.abs_float(math.dot(axis, ref_normal)) + if projection < min_projection { + min_edge = edge_index + min_projection = projection + } + edge_index = edges[twin].next + if edge_index == start { break } + } + face1 = edges[min_edge].face + face2 = edges[edges[min_edge].twin].face + if math.dot(planes[face1].normal, ref_normal) < math.dot(planes[face2].normal, ref_normal) { return face1 } + return face2 +} + +// Every vertex's outgoing edge is the next vertex's incoming one. +validate_polygon(polygon: ClipVertex[], count: int) -> bool { + if count == 0 { return true } + vertex1 = polygon[count - 1] + i = 0 + while i < count { + vertex2 = polygon[i] + if vertex1.pair.owner2 != vertex2.pair.owner1 || vertex1.pair.index2 != vertex2.pair.index1 { return false } + vertex1 = vertex2 + i = i + 1 + } + return true +} + +// Sutherland-Hodgman: the polygon against one side plane of the reference +// face, whose edge tags the new vertices; separations are from the +// reference plane. Returns the output count. +clip_polygon(out: ClipVertex[], polygon: ClipVertex[], count: int, clip_plane: Plane, edge: int, ref_plane: Plane) -> int { + vertex1 = polygon[count - 1] + distance1 = math.plane_separation(clip_plane, vertex1.position) + out_count = 0 + i = 0 + while i < count { + vertex2 = polygon[i] + distance2 = math.plane_separation(clip_plane, vertex2.position) + if distance1 <= 0.0 && distance2 <= 0.0 { + // Both behind: keep vertex2. + out[out_count] = vertex2 + out_count = out_count + 1 + } else if distance1 <= 0.0 && distance2 > 0.0 { + // Leaving: the intersection, with its outgoing edge adjusted. + fraction = distance1 / (distance1 - distance2) + position = math.mul_add(vertex1.position, fraction, math.sub(vertex2.position, vertex1.position)) + v = ClipVertex { position: position, separation: math.plane_separation(ref_plane, position), pair: vertex2.pair } + v.pair.owner2 = FEATURE_SHAPE_A + v.pair.index2 = edge + out[out_count] = v + out_count = out_count + 1 + } else if distance2 <= 0.0 && distance1 > 0.0 { + // Entering: the intersection with its incoming edge adjusted, then vertex2. + fraction = distance1 / (distance1 - distance2) + position = math.mul_add(vertex1.position, fraction, math.sub(vertex2.position, vertex1.position)) + v = ClipVertex { position: position, separation: math.plane_separation(ref_plane, position), pair: vertex1.pair } + v.pair.owner1 = FEATURE_SHAPE_A + v.pair.index1 = edge + out[out_count] = v + out_count = out_count + 1 + out[out_count] = vertex2 + out_count = out_count + 1 + } + vertex1 = vertex2 + distance1 = distance2 + i = i + 1 + } + return out_count +} + +// A segment (segment[0..1]) against a plane, in place; the count kept. +clip_segment(segment: ClipVertex[], plane: Plane) -> int { + count = 0 + vertex1 = segment[0] + vertex2 = segment[1] + distance1 = math.plane_separation(plane, vertex1.position) + distance2 = math.plane_separation(plane, vertex2.position) + if distance1 <= 0.0 { + segment[count] = vertex1 + count = count + 1 + } + if distance2 <= 0.0 { + segment[count] = vertex2 + count = count + 1 + } + if distance1 * distance2 < 0.0 { + t = distance1 / (distance1 - distance2) + pair = vertex2.pair + if distance1 > 0.0 { pair = vertex1.pair } + segment[count] = ClipVertex { position: math.add(math.mul_sv(1.0 - t, vertex1.position), math.mul_sv(t, vertex2.position)), + separation: 0.0, pair: pair } + count = count + 1 + } + return count +} + +// The segment against the side planes of a hull face: 2 if it survives, else 0. +clip_segment_to_hull_face(segment: ClipVertex[], h: *HullData, ref_face: int) -> int { + faces = hull.hull_faces(h) + planes = hull.hull_planes(h) + edges = hull.hull_edges(h) + points = hull.hull_points(h) + ref_plane = planes[ref_face] + start = faces[ref_face] + 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) + if clip_segment(segment, math.make_plane_from_normal_and_point(binormal, vertex1)) < 2 { return 0 } + edge_index = next_index + if edge_index == start { break } + } + return 2 +} + +// --- reduction ------------------------------------------------------------------------- + +// Four points from the clipped polygon: the deepest touching point (with +// a tangent tie-break), the farthest from it, the largest triangle, then +// the point adding the most area outside it. The 0.95 bias keeps the +// choice stable across steps. Consumes the point array. +reduce_manifold_points(m: *LocalManifold, capacity: int, points: LocalManifoldPoint[], count: int) { + if capacity < 4 { return } + if count <= 4 { + i = 0 + while i < count { + set_manifold_point(m, i, points[i]) + i = i + 1 + } + m.point_count = count + return + } + normal = m.normal + tol_sqr = SPECULATIVE_DISTANCE * SPECULATIVE_DISTANCE + bias = 0.95 + + // 1: the extreme touching point. + best_index = NULL_INDEX + best_score = 0.0 - math.MAX_FLOAT + search = arbitrary_perp(normal) + i = 0 + while i < count { + if points[i].separation <= SPECULATIVE_DISTANCE { + score = 0.0 - points[i].separation + math.dot(search, points[i].point) + if bias * score > best_score { + best_index = i + best_score = score + } + } + i = i + 1 + } + if best_index == NULL_INDEX { + m.point_count = 0 + return + } + set_manifold_point(m, 0, points[best_index]) + m.point_count = 1 + points[best_index] = points[count - 1] + count = count - 1 + a = manifold_point(m, 0).point + + // 2: the farthest point in the plane. + best_score = 0.0 + best_index = NULL_INDEX + i = 0 + while i < count { + d = math.sub(points[i].point, a) + v = math.mul_sub(d, math.dot(d, normal), normal) + separation = math.max_float(0.0, 0.0 - points[i].separation) + score = math.length_squared(v) + 4.0 * separation * separation + if bias * score > best_score { + best_score = score + best_index = i + } + i = i + 1 + } + if best_score < tol_sqr { return } + set_manifold_point(m, 1, points[best_index]) + m.point_count = 2 + points[best_index] = points[count - 1] + count = count - 1 + b = manifold_point(m, 1).point + + // 3: the largest triangle. + best_score = tol_sqr + best_index = NULL_INDEX + best_signed_area = 0.0 + ba = math.sub(b, a) + i = 0 + while i < count { + signed_area = math.dot(normal, math.cross(ba, math.sub(points[i].point, a))) + score = math.abs_float(signed_area) + if bias * score >= best_score { + best_score = score + best_index = i + best_signed_area = signed_area + } + i = i + 1 + } + if best_index == NULL_INDEX { return } + set_manifold_point(m, 2, points[best_index]) + m.point_count = 3 + points[best_index] = points[count - 1] + count = count - 1 + c = manifold_point(m, 2).point + + // 4: the most area outside the triangle. + best_score = tol_sqr + best_index = NULL_INDEX + sign = 1.0 + if best_signed_area < 0.0 { sign = 0.0 - 1.0 } + i = 0 + while i < count { + p = points[i].point + u1 = sign * math.dot(normal, math.cross(math.sub(p, a), ba)) + u2 = sign * math.dot(normal, math.cross(math.sub(p, b), math.sub(c, b))) + u3 = sign * math.dot(normal, math.cross(math.sub(p, c), math.sub(a, c))) + score = math.max_float(u1, math.max_float(u2, u3)) + if bias * score > best_score { + best_score = score + best_index = i + } + i = i + 1 + } + if best_index != NULL_INDEX { + set_manifold_point(m, m.point_count, points[best_index]) + m.point_count = m.point_count + 1 + } +} + +// A fixed perpendicular to a unit vector, the reference's, so the +// reduction's tie-break is the same on every platform. +arbitrary_perp(v: Vec3) -> Vec3 { + a = 0.67 + b = 0.0 - 0.42 + p = math.vec3_zero() + if v.x < 0.0 - 0.5 || 0.5 < v.x { + p = math.vec3(a * v.y + b * v.z, 0.0 - a * v.x, 0.0 - b * v.x) + } else if v.y < 0.0 - 0.5 || 0.5 < v.y { + p = math.vec3(a * v.y, 0.0 - a * v.x + b * v.z, 0.0 - b * v.y) + } else { + p = math.vec3(a * v.z, b * v.z, 0.0 - a * v.x - b * v.y) + } + return math.normalize(p) +} + +// --- spheres and capsules ---------------------------------------------------------------- + +one_point(m: *LocalManifold, normal: Vec3, point: Vec3, separation: float, pair: FeaturePair) { + m.normal = normal + m.point_count = 1 + set_manifold_point(m, 0, LocalManifoldPoint { point: point, separation: separation, pair: pair, triangle_index: NULL_INDEX }) +} + +two_points(m: *LocalManifold, normal: Vec3, p1: Vec3, s1: float, pair1: FeaturePair, p2: Vec3, s2: float, pair2: FeaturePair) { + m.normal = normal + m.point_count = 2 + set_manifold_point(m, 0, LocalManifoldPoint { point: p1, separation: s1, pair: pair1, triangle_index: NULL_INDEX }) + set_manifold_point(m, 1, LocalManifoldPoint { point: p2, separation: s2, pair: pair2, triangle_index: NULL_INDEX }) +} + +collide_spheres(m: *LocalManifold, capacity: int, sphere_a: Sphere, sphere_b: Sphere, transform_b_to_a: Transform) { + m.point_count = 0 + if capacity == 0 { return } + center1 = sphere_a.center + center2 = math.transform_point(transform_b_to_a, sphere_b.center) + total_radius = sphere_a.radius + sphere_b.radius + offset = math.sub(center2, center1) + distance_sq = math.length_squared(offset) + if distance_sq > total_radius * total_radius { return } + normal = math.vec3(0.0, 1.0, 0.0) + d = math.sqrt_float(distance_sq) + if d * d > 1000.0 * FLOAT_MIN { normal = math.mul_sv(1.0 / d, offset) } + // The contact at the midpoint of the two surfaces. + point = math.mul_sv(0.5, math.mul_sub(math.add(math.mul_add(center1, sphere_a.radius, normal), center2), sphere_b.radius, normal)) + one_point(m, normal, point, d - total_radius, feature_pair_single()) +} + +collide_capsule_and_sphere(m: *LocalManifold, capacity: int, capsule_a: Capsule, sphere_b: Sphere, transform_b_to_a: Transform) { + m.point_count = 0 + if capacity < 1 { return } + center = math.transform_point(transform_b_to_a, sphere_b.center) + total_radius = sphere_b.radius + capsule_a.radius + closest = math.point_to_segment_distance(capsule_a.center1, capsule_a.center2, center) + offset = math.sub(center, closest) + distance_sq = math.length_squared(offset) + if distance_sq > total_radius * total_radius { return } + normal = math.vec3(0.0, 1.0, 0.0) + d = math.sqrt_float(distance_sq) + if d * d > 1000.0 * FLOAT_MIN { normal = math.mul_sv(1.0 / d, offset) } + point = math.mul_sv(0.5, math.mul_add(math.add(math.mul_sub(center, sphere_b.radius, normal), closest), capsule_a.radius, normal)) + one_point(m, normal, point, d - total_radius, feature_pair_single()) +} + +// GJK from the sphere's centre for the shallow case; the deepest face for +// the deep one. +collide_hull_and_sphere(m: *LocalManifold, capacity: int, hull_a: *HullData, sphere_b: Sphere, transform_b_to_a: Transform, + cache: *SimplexCache) { + m.point_count = 0 + if capacity == 0 { return } + center = math.transform_point(transform_b_to_a, sphere_b.center) + input = DistanceInput { proxy_a: hull.hull_proxy(hull_a), proxy_b: distance.shape_proxy((¢er) as ptr, 1, 0.0), + transform: math.transform_identity(), use_radii: false } + radius_a = 0.0 + radius_b = sphere_b.radius + radius = radius_a + radius_b + output = distance.shape_distance(&input, cache, null, 0) + + if output.distance > radius + SPECULATIVE_DISTANCE { + clear_simplex_cache(cache) + return + } + if output.distance > 100.0 * FLOAT_EPSILON { + // Shallow. + normal = math.normalize(math.sub(output.point_b, output.point_a)) + c_a = math.mul_add(center, radius_a - math.dot(math.sub(center, output.point_a), normal), normal) + c_b = math.mul_sub(center, radius_b, normal) + one_point(m, normal, math.lerp(c_a, c_b, 0.5), output.distance - radius, feature_pair_single()) + return + } + // Deep: the face the centre is least below. + planes = hull.hull_planes(hull_a) + best_index = NULL_INDEX + best_distance = 0.0 - math.MAX_FLOAT + i = 0 + while i < hull_a.face_count { + d = math.plane_separation(planes[i], center) + if d > best_distance { + best_index = i + best_distance = d + } + i = i + 1 + } + normal = planes[best_index].normal + c_a = math.mul_add(center, radius_a - math.dot(math.sub(center, output.point_a), normal), normal) + c_b = math.mul_sub(center, radius_b, normal) + one_point(m, normal, math.lerp(c_a, c_b, 0.5), best_distance - radius, feature_pair_single()) +} + +// Two points when the capsules are nearly parallel and B's clipped ends +// both touch A; otherwise the closest points of the segments. +collide_capsules(m: *LocalManifold, capacity: int, capsule_a: Capsule, capsule_b: Capsule, transform_b_to_a: Transform) { + m.point_count = 0 + if capacity < 2 { return } + center_a1 = capsule_a.center1 + center_a2 = capsule_a.center2 + center_b1 = math.transform_point(transform_b_to_a, capsule_b.center1) + center_b2 = math.transform_point(transform_b_to_a, capsule_b.center2) + radius = capsule_a.radius + capsule_b.radius + max_distance = radius + SPECULATIVE_DISTANCE + result = math.segment_distance(center_a1, center_a2, center_b1, center_b2) + offset = math.sub(result.point2, result.point1) + distance_squared = math.length_squared(offset) + min_distance = 0.01 * math.LINEAR_SLOP + if distance_squared > max_distance * max_distance || distance_squared < min_distance * min_distance { return } + + segment_a = math.sub(center_a2, center_a1) + length_a = math.length(segment_a) + if length_a < MIN_CAPSULE_LENGTH { return } + edge_a = math.mul_sv(1.0 / length_a, segment_a) + segment_b = math.sub(center_b2, center_b1) + length_b = math.length(segment_b) + if length_b < MIN_CAPSULE_LENGTH { return } + edge_b = math.mul_sv(1.0 / length_b, segment_b) + + // Nearly parallel: |eA x eB| = sin(alpha). + alpha_tol_sqr = 0.05 * 0.05 + axis = math.cross(edge_a, edge_b) + if math.length_squared(axis) < alpha_tol_sqr { + scratch_ready() + vertices_b = g_clip1 as ClipVertex[] + vertices_b[0] = ClipVertex { position: center_b1, separation: 0.0, pair: make_feature_pair(FEATURE_SHAPE_A, 0, FEATURE_SHAPE_A, 0) } + vertices_b[1] = ClipVertex { position: center_b2, separation: 0.0, pair: make_feature_pair(FEATURE_SHAPE_A, 1, FEATURE_SHAPE_A, 1) } + plane1 = Plane { normal: math.neg(edge_a), offset: 0.0 - math.dot(edge_a, capsule_a.center1) } + plane2 = Plane { normal: edge_a, offset: math.dot(edge_a, capsule_a.center2) } + point_count = clip_segment(vertices_b, plane1) + if point_count == 2 { point_count = clip_segment(vertices_b, plane2) } + if point_count == 2 { + closest1 = math.point_to_segment_distance(center_a1, center_a2, vertices_b[0].position) + closest2 = math.point_to_segment_distance(center_a1, center_a2, vertices_b[1].position) + distance1 = math.distance(closest1, vertices_b[0].position) + distance2 = math.distance(closest2, vertices_b[1].position) + if distance1 <= radius && distance2 <= radius { + if distance1 < min_distance || distance2 < min_distance { return } + normal1 = math.mul_sv(1.0 / distance1, math.sub(vertices_b[0].position, closest1)) + normal2 = math.mul_sv(1.0 / distance2, math.sub(vertices_b[1].position, closest2)) + normal = math.normalize(math.add(normal1, normal2)) + radius_a = capsule_a.radius + radius_b = capsule_b.radius + point1 = math.mul_sv(0.5, math.mul_sub(math.add(math.mul_add(vertices_b[0].position, radius_a, normal1), closest1), radius_b, normal)) + point2 = math.mul_sv(0.5, math.mul_sub(math.add(math.mul_add(vertices_b[1].position, radius_a, normal2), closest2), radius_b, normal)) + two_points(m, normal, point1, distance1 - radius, vertices_b[0].pair, point2, distance2 - radius, vertices_b[1].pair) + return + } + } + } + + d = math.length(offset) + normal = math.mul_sv(1.0 / d, offset) + point = math.mul_sv(0.5, math.mul_sub(math.add(math.mul_add(result.point1, capsule_a.radius, normal), result.point2), capsule_b.radius, normal)) + one_point(m, normal, point, d - radius, feature_pair_single()) +} + +// --- hull and capsule ------------------------------------------------------------------------ + +// The hull face the capsule is least below, and the capsule end that is. +query_face_direction_hull_and_capsule(h: *HullData, c: Capsule, capsule_transform: Transform) -> SeparatingAxis { + planes = hull.hull_planes(h) + p1 = math.transform_point(capsule_transform, c.center1) + p2 = math.transform_point(capsule_transform, c.center2) + max_face = NULL_INDEX + max_vertex = NULL_INDEX + max_separation = 0.0 - math.MAX_FLOAT + i = 0 + while i < h.face_count { + plane = planes[i] + vertex_index = 0 + support = p1 + if math.dot(math.neg(plane.normal), math.sub(p2, p1)) > 0.0 { + vertex_index = 1 + support = p2 + } + separation = math.plane_separation(plane, support) + if separation > max_separation { + max_vertex = vertex_index + max_face = i + max_separation = separation + } + i = i + 1 + } + return SeparatingAxis { normal: planes[max_face].normal, separation: max_separation, index_a: max_face, index_b: max_vertex, kind: AXIS_FACE_A } +} + +// The hull edge whose Gauss-map arc the capsule's circle crosses with the +// most separation; index_a stays 0, index_b is the hull's half-edge. +query_edge_direction_hull_and_capsule(h: *HullData, c: Capsule, capsule_transform: Transform) -> SeparatingAxis { + max_normal = math.vec3_zero() + max_separation = 0.0 - math.MAX_FLOAT + max_index_a = NULL_INDEX + max_index_b = NULL_INDEX + p_a = math.transform_point(capsule_transform, c.center1) + q_a = math.transform_point(capsule_transform, c.center2) + e_a = math.sub(q_a, p_a) + edges = hull.hull_edges(h) + points = hull.hull_points(h) + planes = hull.hull_planes(h) + squared_tolerance = PARALLEL_EDGE_TOL * PARALLEL_EDGE_TOL + index = 0 + while index < h.edge_count { + twin = index + 1 + q_b = points[edges[twin].origin] + u_b = planes[edges[index].face].normal + v_b = planes[edges[twin].face].normal + // An isolated edge is a circle through the origin on the Gauss map, + // so the overlap with the arc u-v is a plane test. + cba = math.dot(u_b, e_a) + dba = math.dot(v_b, e_a) + if cba * dba < 0.0 { + if math.max_float(cba * cba, dba * dba) >= squared_tolerance * math.length_squared(e_a) { + t = cba / (cba - dba) + axis = math.normalize(math.lerp(u_b, v_b, t)) + separation = math.dot(axis, math.sub(q_a, q_b)) + if separation > max_separation { + max_normal = axis + max_separation = separation + max_index_a = 0 + max_index_b = index + } + } + } + index = index + 2 + } + return SeparatingAxis { normal: max_normal, separation: max_separation, index_a: max_index_a, index_b: max_index_b, kind: AXIS_EDGE_PAIR } +} + +build_hull_face_and_capsule_contact(m: *LocalManifold, hull_a: *HullData, capsule_b: Capsule, transform_b_to_a: Transform, + query: SeparatingAxis) -> bool { + planes = hull.hull_planes(hull_a) + ref_face = query.index_a + ref_plane = planes[ref_face] + scratch_ready() + segment = g_clip1 as ClipVertex[] + segment[0] = ClipVertex { position: math.transform_point(transform_b_to_a, capsule_b.center1), separation: 0.0, + pair: make_feature_pair(FEATURE_SHAPE_A, 0, FEATURE_SHAPE_A, 0) } + segment[1] = ClipVertex { position: math.transform_point(transform_b_to_a, capsule_b.center2), separation: 0.0, + pair: make_feature_pair(FEATURE_SHAPE_A, 1, FEATURE_SHAPE_A, 1) } + if clip_segment_to_hull_face(segment, hull_a, ref_face) < 2 { return false } + distance1 = math.plane_separation(ref_plane, segment[0].position) + distance2 = math.plane_separation(ref_plane, segment[1].position) + if distance1 <= SPECULATIVE_DISTANCE || distance2 <= SPECULATIVE_DISTANCE { + normal = ref_plane.normal + point1 = math.mul_sub(segment[0].position, 0.5 * (distance1 + capsule_b.radius), normal) + point2 = math.mul_sub(segment[1].position, 0.5 * (distance2 + capsule_b.radius), normal) + two_points(m, normal, point1, distance1 - capsule_b.radius, segment[0].pair, point2, distance2 - capsule_b.radius, segment[1].pair) + return true + } + return false +} + +is_within_segments(r: SegmentDistanceResult) -> bool { + return 0.0 <= r.fraction1 && r.fraction1 <= 1.0 && 0.0 <= r.fraction2 && r.fraction2 <= 1.0 +} + +build_hull_and_capsule_edge_contact(m: *LocalManifold, capacity: int, hull_a: *HullData, capsule_b: Capsule, + transform_b_to_a: Transform, query: SeparatingAxis) -> bool { + if capacity < 1 { return false } + p_c = math.transform_point(transform_b_to_a, capsule_b.center1) + q_c = math.transform_point(transform_b_to_a, capsule_b.center2) + e_c = math.sub(q_c, p_c) + edges = hull.hull_edges(hull_a) + points = hull.hull_points(hull_a) + edge2 = query.index_b + p_h = points[edges[edge2].origin] + q_h = points[edges[edges[edge2].twin].origin] + e_h = math.sub(q_h, p_h) + normal = query.normal + result = math.line_distance(p_h, e_h, p_c, e_c) + if is_within_segments(result) == false { return false } + point = math.mul_sv(0.5, math.add(math.mul_sub(result.point1, capsule_b.radius, normal), result.point2)) + separation = math.dot(normal, math.sub(result.point2, result.point1)) + one_point(m, normal, point, separation - capsule_b.radius, make_feature_pair(FEATURE_SHAPE_A, query.index_a, FEATURE_SHAPE_B, query.index_b)) + return true +} + +// GJK for the shallow case, clipping the capsule to the support face when +// it lies along it; the face and edge axes for the deep case. +collide_hull_and_capsule(m: *LocalManifold, capacity: int, hull_a: *HullData, capsule_b: Capsule, transform_b_to_a: Transform, + cache: *SimplexCache) { + m.point_count = 0 + if capacity < 2 { return } + input = DistanceInput { proxy_a: hull.hull_proxy(hull_a), proxy_b: distance.shape_proxy((&capsule_b) as ptr, 2, 0.0), + transform: transform_b_to_a, use_radii: false } + output = distance.shape_distance(&input, cache, null, 0) + if output.distance > capsule_b.radius + SPECULATIVE_DISTANCE { + clear_simplex_cache(cache) + return + } + + if output.distance > 100.0 * FLOAT_EPSILON { + // Shallow. + planes = hull.hull_planes(hull_a) + delta = output.normal + ref_face = hull.find_hull_support_face(hull_a, delta) + ref_plane = planes[ref_face] + // Two points when the closest-point direction is nearly the face normal. + if math.abs_float(math.dot(ref_plane.normal, delta)) > 0.998 { + scratch_ready() + vertices_b = g_clip1 as ClipVertex[] + vertices_b[0] = ClipVertex { position: math.transform_point(transform_b_to_a, capsule_b.center1), separation: 0.0, + pair: make_feature_pair(FEATURE_SHAPE_A, 0, FEATURE_SHAPE_A, 0) } + vertices_b[1] = ClipVertex { position: math.transform_point(transform_b_to_a, capsule_b.center2), separation: 0.0, + pair: make_feature_pair(FEATURE_SHAPE_A, 1, FEATURE_SHAPE_A, 1) } + if clip_segment_to_hull_face(vertices_b, hull_a, ref_face) == 2 { + distance1 = math.plane_separation(ref_plane, vertices_b[0].position) + distance2 = math.plane_separation(ref_plane, vertices_b[1].position) + if distance1 <= capsule_b.radius + SPECULATIVE_DISTANCE || distance2 <= capsule_b.radius + SPECULATIVE_DISTANCE { + normal = ref_plane.normal + point1 = math.mul_sub(vertices_b[0].position, 0.5 * (capsule_b.radius + distance1), normal) + point2 = math.mul_sub(vertices_b[1].position, 0.5 * (capsule_b.radius + distance2), normal) + two_points(m, normal, point1, distance1 - capsule_b.radius, vertices_b[0].pair, point2, distance2 - capsule_b.radius, vertices_b[1].pair) + return + } + } + } + point = math.mul_sv(0.5, math.add(math.mul_sub(output.point_a, capsule_b.radius, delta), output.point_b)) + one_point(m, delta, point, output.distance - capsule_b.radius, feature_pair_single()) + return + } + + // Deep. + face_query = query_face_direction_hull_and_capsule(hull_a, capsule_b, transform_b_to_a) + if face_query.separation > capsule_b.radius { return } + edge_query = query_edge_direction_hull_and_capsule(hull_a, capsule_b, transform_b_to_a) + if edge_query.separation > capsule_b.radius { return } + + face_separation = face_query.separation - capsule_b.radius + build_hull_face_and_capsule_contact(m, hull_a, capsule_b, transform_b_to_a, face_query) + if m.point_count == 2 { + face_separation = math.min_float(manifold_point(m, 0).separation, manifold_point(m, 1).separation) + } + if edge_query.index_a == NULL_INDEX { return } + // The face contact can be empty when it does not realise the axis of + // least penetration; the edge takes over then, or when clearly better. + edge_separation = edge_query.separation - capsule_b.radius + if m.point_count == 0 || edge_separation > face_separation + math.LINEAR_SLOP { + build_hull_and_capsule_edge_contact(m, capacity, hull_a, capsule_b, transform_b_to_a, edge_query) + } +} + +// --- hull and hull ----------------------------------------------------------------------------- + +// The separating axis test: A's faces against B's vertices, B's faces +// against A's, then the edge pairs whose Gauss-map arcs cross (Gregorius), +// with the axis from the arc intersection. Normals point from A to B in +// frame A. With early_return the scan stops at the first axis past the +// speculative distance. The reference's scalar path, exactly. +compute_separating_axis(hull_a: *HullData, hull_b: *HullData, xf_b: Transform, early_return: bool) -> AxisQuery { + r = math.make_matrix_from_quat(xf_b.q) + inv_r = math.transpose(r) + res = AxisQuery { + face_a: SeparatingAxis { normal: math.vec3_zero(), separation: 0.0 - math.MAX_FLOAT, index_a: NULL_INDEX, index_b: NULL_INDEX, kind: AXIS_FACE_A }, + face_b: SeparatingAxis { normal: math.vec3_zero(), separation: 0.0 - math.MAX_FLOAT, index_a: NULL_INDEX, index_b: NULL_INDEX, kind: AXIS_FACE_B }, + edge: SeparatingAxis { normal: math.vec3_zero(), separation: 0.0 - math.MAX_FLOAT, index_a: NULL_INDEX, index_b: NULL_INDEX, kind: AXIS_EDGE_PAIR }, + separated_feature: AXIS_INVALID + } + planes_a = hull.hull_planes(hull_a) + points_a = hull.hull_points(hull_a) + planes_b = hull.hull_planes(hull_b) + points_b = hull.hull_points(hull_b) + edges_a = hull.hull_edges(hull_a) + edges_b = hull.hull_edges(hull_b) + + // A's face planes against B's vertices. + i = 0 + while i < hull_a.face_count { + plane = planes_a[i] + direction = math.neg(math.mul_mv(inv_r, plane.normal)) + plane_separation = math.dot(plane.normal, xf_b.p) - plane.offset + vertex_index = support_index(points_b, hull_b.vertex_count, direction) + support = math.dot(direction, points_b[vertex_index]) + separation = plane_separation - support + if separation > res.face_a.separation { + res.face_a.normal = plane.normal + res.face_a.separation = separation + res.face_a.index_a = i + res.face_a.index_b = vertex_index + if separation > SPECULATIVE_DISTANCE && early_return { + res.separated_feature = AXIS_FACE_A + return res + } + } + i = i + 1 + } + + // B's face planes against A's vertices. + i = 0 + while i < hull_b.face_count { + plane = planes_b[i] + direction = math.neg(math.mul_mv(r, plane.normal)) + plane_separation = math.dot(direction, xf_b.p) - plane.offset + vertex_index = support_index(points_a, hull_a.vertex_count, direction) + support = math.dot(direction, points_a[vertex_index]) + separation = plane_separation - support + if separation > res.face_b.separation { + res.face_b.normal = direction + res.face_b.separation = separation + res.face_b.index_a = vertex_index + res.face_b.index_b = i + if separation > SPECULATIVE_DISTANCE && early_return { + res.separated_feature = AXIS_FACE_B + return res + } + } + i = i + 1 + } + + // The edge pairs. B's data is taken into A's frame and negated once: + // C and D are its two face normals, DC its edge, bv0 its first vertex. + squared_tol = PARALLEL_EDGE_TOL * PARALLEL_EDGE_TOL + eps = 0.0 - 0.0001 + j = 0 + while j < hull_b.edge_count { + twin_b = j + 1 + c = math.neg(math.mul_mv(r, planes_b[edges_b[j].face].normal)) + d = math.neg(math.mul_mv(r, planes_b[edges_b[twin_b].face].normal)) + bv0 = math.neg(math.add(math.mul_mv(r, points_b[edges_b[j].origin]), xf_b.p)) + bv1 = math.neg(math.add(math.mul_mv(r, points_b[edges_b[twin_b].origin]), xf_b.p)) + dc = math.sub(bv1, bv0) + i = 0 + while i < hull_a.edge_count { + twin_a = i + 1 + n0 = planes_a[edges_a[i].face].normal + n1 = planes_a[edges_a[twin_a].face].normal + av0 = points_a[edges_a[i].origin] + ad = math.sub(points_a[edges_a[twin_a].origin], av0) + cba = math.dot(c, ad) + dba = math.dot(d, ad) + if cba * dba < eps { + adc = math.dot(n0, dc) + bdc = math.dot(n1, dc) + if adc * bdc < eps && cba * bdc < eps { + // Near-parallel edges are not an axis. + max_cd = math.max_float(cba * cba, dba * dba) + if max_cd > squared_tol * math.length_squared(ad) { + t = (0.0 - cba) / (dba - cba) + n = math.normalize(math.add(c, math.mul_sv(t, math.sub(d, c)))) + separation = 0.0 - math.dot(math.add(av0, bv0), n) + if separation > res.edge.separation { + res.edge.normal = n + res.edge.separation = separation + res.edge.index_a = i + res.edge.index_b = j + if separation > SPECULATIVE_DISTANCE && early_return { + res.separated_feature = AXIS_EDGE_PAIR + return res + } + } + } + } + } + i = i + 2 + } + j = j + 2 + } + return res +} + +// The vertex farthest along the direction. +support_index(points: Vec3[], count: int, direction: Vec3) -> int { + best = 0 + best_dot = math.dot(direction, points[0]) + i = 1 + while i < count { + d = math.dot(direction, points[i]) + if d > best_dot { + best = i + best_dot = d + } + i = i + 1 + } + return best +} + +// The axis of least penetration among the three. +get_best_axis(query: *AxisQuery) -> SeparatingAxis { + if query.face_a.separation > query.face_b.separation { + if query.edge.separation > query.face_a.separation { return query.edge } + return query.face_a + } + if query.edge.separation > query.face_b.separation { return query.edge } + return query.face_b +} + +// The incident face of the hull, in frame A, as a clip polygon with each +// vertex tagged by its edges; separations from the reference plane. +build_polygon(out: ClipVertex[], transform: Transform, h: *HullData, inc_face: int, ref_plane: Plane) -> int { + faces = hull.hull_faces(h) + edges = hull.hull_edges(h) + points = hull.hull_points(h) + start = faces[inc_face] + edge_index = start + out_count = 0 + m = math.make_matrix_from_quat(transform.q) + while true { + next_index = edges[edge_index].next + position = math.add(math.mul_mv(m, points[edges[next_index].origin]), transform.p) + out[out_count] = ClipVertex { position: position, separation: math.plane_separation(ref_plane, position), + pair: make_feature_pair(FEATURE_SHAPE_B, edge_index, FEATURE_SHAPE_B, next_index) } + out_count = out_count + 1 + edge_index = next_index + if edge_index == start || out_count >= MAX_CLIP_POINTS { break } + } + return out_count +} + +// The face contact with A's face as the reference: B's incident face +// clipped to A's side planes, the points halfway between the surfaces, +// reduced to four. +build_face_a_contact(m: *LocalManifold, capacity: int, hull_a: *HullData, hull_b: *HullData, transform_b_to_a: Transform, + query: SeparatingAxis, cache: *SATCache) -> bool { + faces_a = hull.hull_faces(hull_a) + edges_a = hull.hull_edges(hull_a) + planes_a = hull.hull_planes(hull_a) + points_a = hull.hull_points(hull_a) + ref_face = query.index_a + ref_plane = planes_a[ref_face] + + ref_normal_in_b = math.inv_rotate_vector(transform_b_to_a.q, ref_plane.normal) + inc_face = find_incident_face(hull_b, ref_normal_in_b, query.index_b) + + scratch_ready() + input = g_clip1 as ClipVertex[] + output = g_clip2 as ClipVertex[] + point_count = build_polygon(input, transform_b_to_a, hull_b, inc_face, ref_plane) + + start = faces_a[ref_face] + edge_index = start + while true { + next_index = edges_a[edge_index].next + vertex1 = points_a[edges_a[edge_index].origin] + vertex2 = points_a[edges_a[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 = clip_polygon(output, input, point_count, clip_plane, edge_index, ref_plane) + swap = input + input = output + output = swap + if point_count < 3 { + clear_sat_cache(cache) + return false + } + edge_index = next_index + if edge_index == start { break } + } + point_count = math.min_int(point_count, MAX_CLIP_POINTS) + + points = g_clip_points as LocalManifoldPoint[] + min_separation = math.MAX_FLOAT + m.normal = ref_plane.normal + i = 0 + while i < point_count { + cp = input[i] + // Halfway between the surfaces, so the points stay put when the + // reference face swaps from A to B. + points[i] = LocalManifoldPoint { point: math.mul_sub(cp.position, 0.5 * cp.separation, ref_plane.normal), + separation: cp.separation, pair: cp.pair, triangle_index: NULL_INDEX } + min_separation = math.min_float(min_separation, cp.separation) + i = i + 1 + } + if min_separation >= SPECULATIVE_DISTANCE { + clear_sat_cache(cache) + return false + } + reduce_manifold_points(m, capacity, points, point_count) + cache.separation = min_separation + cache.kind = AXIS_FACE_A + cache.index_a = query.index_a + cache.index_b = query.index_b + return true +} + +// The same with B's face as the reference, built in B's frame and taken +// back to A's, the normal flipped to point from A to B and the pairs flipped. +build_face_b_contact(m: *LocalManifold, capacity: int, hull_a: *HullData, hull_b: *HullData, transform_b_to_a: Transform, + query: SeparatingAxis, cache: *SATCache) -> bool { + transform_a_to_b = math.invert_transform(transform_b_to_a) + flipped = SeparatingAxis { normal: math.neg(query.normal), separation: query.separation, index_a: query.index_b, + index_b: query.index_a, kind: AXIS_FACE_A } + if build_face_a_contact(m, capacity, hull_b, hull_a, transform_a_to_b, flipped, cache) == false { + clear_sat_cache(cache) + return false + } + matrix = math.make_matrix_from_quat(transform_b_to_a.q) + m.normal = math.neg(math.mul_mv(matrix, m.normal)) + i = 0 + while i < m.point_count { + p = manifold_point(m, i) + p.point = math.add(math.mul_mv(matrix, p.point), transform_b_to_a.p) + p.pair = flip_pair(p.pair) + set_manifold_point(m, i, p) + i = i + 1 + } + cache.kind = AXIS_FACE_B + cache.index_a = query.index_a + cache.index_b = query.index_b + return true +} + +// The edge contact: the closest points of the two edge lines, if within +// both segments; halfway between them. +build_edge_contact(m: *LocalManifold, hull_a: *HullData, hull_b: *HullData, transform_b_to_a: Transform, + query: SeparatingAxis, cache: *SATCache) -> bool { + edges_a = hull.hull_edges(hull_a) + points_a = hull.hull_points(hull_a) + edges_b = hull.hull_edges(hull_b) + points_b = hull.hull_points(hull_b) + p_a = points_a[edges_a[query.index_a].origin] + q_a = points_a[edges_a[edges_a[query.index_a].twin].origin] + e_a = math.sub(q_a, p_a) + p_b = math.transform_point(transform_b_to_a, points_b[edges_b[query.index_b].origin]) + q_b = math.transform_point(transform_b_to_a, points_b[edges_b[edges_b[query.index_b].twin].origin]) + e_b = math.sub(q_b, p_b) + normal = query.normal + result = math.line_distance(p_a, e_a, p_b, e_b) + if is_within_segments(result) == false { + clear_sat_cache(cache) + return false + } + // From a cache this can have slid off the end. + separation = math.dot(normal, math.sub(result.point2, result.point1)) + point = math.mul_sv(0.5, math.add(result.point1, result.point2)) + one_point(m, normal, point, separation, 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 + return true +} + +// Two hulls, B in A's frame. The cache's last axis is tried first: if it +// still separates, or still builds the same contact, that is the answer. +// Otherwise the full axis test; a face contact by preference (a one-point +// edge contact is weak for stacking), the edge contact when the face +// finds nothing or the edge axis is clearly better. +collide_hulls(m: *LocalManifold, capacity: int, hull_a: *HullData, hull_b: *HullData, transform_b_to_a: Transform, cache: *SATCache) { + m.point_count = 0 + if capacity < 4 { return } + edges_a = hull.hull_edges(hull_a) + planes_a = hull.hull_planes(hull_a) + points_a = hull.hull_points(hull_a) + edges_b = hull.hull_edges(hull_b) + planes_b = hull.hull_planes(hull_b) + points_b = hull.hull_points(hull_b) + cache.hit = 0 + + if cache.kind == AXIS_FACE_A { + plane = planes_a[cache.index_a] + search_in_b = math.neg(math.inv_rotate_vector(transform_b_to_a.q, plane.normal)) + vertex_index = hull.find_hull_support_vertex(hull_b, search_in_b) + support = math.transform_point(transform_b_to_a, points_b[vertex_index]) + separation = math.plane_separation(plane, support) + if separation >= SPECULATIVE_DISTANCE { + cache.hit = 1 + return + } + face_query = SeparatingAxis { normal: plane.normal, separation: 0.0, index_a: cache.index_a, index_b: vertex_index, kind: AXIS_FACE_A } + local_cache = empty_sat_cache() + touching = build_face_a_contact(m, capacity, hull_a, hull_b, transform_b_to_a, face_query, &local_cache) + if touching && math.abs_float(cache.separation - local_cache.separation) < math.LINEAR_SLOP { + cache.hit = 1 + return + } + } else if cache.kind == AXIS_FACE_B { + plane = planes_b[cache.index_b] + search_in_a = math.neg(math.rotate_vector(transform_b_to_a.q, plane.normal)) + vertex_index = hull.find_hull_support_vertex(hull_a, search_in_a) + support = math.inv_transform_point(transform_b_to_a, points_a[vertex_index]) + separation = math.plane_separation(plane, support) + if separation >= SPECULATIVE_DISTANCE { + cache.hit = 1 + return + } + face_query = SeparatingAxis { normal: math.neg(plane.normal), separation: 0.0, index_a: vertex_index, index_b: cache.index_b, kind: AXIS_FACE_B } + local_cache = empty_sat_cache() + touching = build_face_b_contact(m, capacity, hull_a, hull_b, transform_b_to_a, face_query, &local_cache) + if touching && math.abs_float(cache.separation - local_cache.separation) < math.LINEAR_SLOP { + cache.hit = 1 + return + } + } else if cache.kind == AXIS_EDGE_PAIR { + index_a = cache.index_a + p_a = points_a[edges_a[index_a].origin] + q_a = points_a[edges_a[index_a + 1].origin] + e_a = math.sub(q_a, p_a) + u_a = planes_a[edges_a[index_a].face].normal + v_a = planes_a[edges_a[index_a + 1].face].normal + index_b = cache.index_b + p_b = math.transform_point(transform_b_to_a, points_b[edges_b[index_b].origin]) + q_b = math.transform_point(transform_b_to_a, points_b[edges_b[index_b + 1].origin]) + e_b = math.sub(q_b, p_b) + u_b = math.rotate_vector(transform_b_to_a.q, planes_b[edges_b[index_b].face].normal) + v_b = math.rotate_vector(transform_b_to_a.q, planes_b[edges_b[index_b + 1].face].normal) + cba = math.dot(u_b, e_a) + dba = math.dot(v_b, e_a) + adc = 0.0 - math.dot(u_a, e_b) + bdc = 0.0 - math.dot(v_a, e_b) + if cba * dba < 0.0 && adc * bdc < 0.0 && cba * bdc > 0.0 { + squared_tolerance = PARALLEL_EDGE_TOL * PARALLEL_EDGE_TOL + if math.max_float(cba * cba, dba * dba) >= squared_tolerance * math.length_squared(e_a) { + t = cba / (cba - dba) + axis = math.normalize(math.lerp(u_b, v_b, t)) + separation = math.dot(axis, math.sub(q_a, q_b)) + if separation > SPECULATIVE_DISTANCE { + cache.hit = 1 + return + } + edge_query = SeparatingAxis { normal: math.neg(axis), separation: 0.0, index_a: cache.index_a, index_b: cache.index_b, kind: AXIS_EDGE_PAIR } + local_cache = empty_sat_cache() + touching = build_edge_contact(m, hull_a, hull_b, transform_b_to_a, edge_query, &local_cache) + if touching && math.abs_float(cache.separation - local_cache.separation) < math.LINEAR_SLOP { + cache.hit = 1 + return + } + } + } + } else if cache.kind == AXIS_MANUAL_FACE_A { + axis_query = compute_separating_axis(hull_a, hull_b, transform_b_to_a, false) + build_face_a_contact(m, capacity, hull_a, hull_b, transform_b_to_a, axis_query.face_a, cache) + return + } else if cache.kind == AXIS_MANUAL_FACE_B { + axis_query = compute_separating_axis(hull_a, hull_b, transform_b_to_a, false) + build_face_b_contact(m, capacity, hull_a, hull_b, transform_b_to_a, axis_query.face_b, cache) + return + } else if cache.kind == AXIS_MANUAL_EDGE_PAIR { + axis_query = compute_separating_axis(hull_a, hull_b, transform_b_to_a, false) + if axis_query.edge.index_a != NULL_INDEX { + build_edge_contact(m, hull_a, hull_b, transform_b_to_a, axis_query.edge, cache) + } + return + } + + m.point_count = 0 + clear_sat_cache(cache) + + axis_query = compute_separating_axis(hull_a, hull_b, transform_b_to_a, true) + if axis_query.separated_feature != AXIS_INVALID { + cache.kind = axis_query.separated_feature + if axis_query.separated_feature == AXIS_FACE_A { + cache.separation = axis_query.face_a.separation + cache.index_a = axis_query.face_a.index_a + cache.index_b = axis_query.face_a.index_b + } else if axis_query.separated_feature == AXIS_FACE_B { + cache.separation = axis_query.face_b.separation + cache.index_a = axis_query.face_b.index_a + cache.index_b = axis_query.face_b.index_b + } else { + cache.separation = axis_query.edge.separation + cache.index_a = axis_query.edge.index_a + cache.index_b = axis_query.edge.index_b + } + return + } + + if axis_query.face_a.separation > axis_query.face_b.separation { + build_face_a_contact(m, capacity, hull_a, hull_b, transform_b_to_a, axis_query.face_a, cache) + } else { + build_face_b_contact(m, capacity, hull_a, hull_b, transform_b_to_a, axis_query.face_b, cache) + } + + edge_query = axis_query.edge + if edge_query.index_a == NULL_INDEX { return } + + clip_separation = cache.separation + if m.point_count == 0 || edge_query.separation > clip_separation + math.LINEAR_SLOP { + // The edge contact into its own point, kept only if it built one. + edge_block = core.alloc(sizeof(LocalManifoldPoint)) + edge_manifold = local_manifold(edge_block) + edge_cache = empty_sat_cache() + build_edge_contact(&edge_manifold, hull_a, hull_b, transform_b_to_a, edge_query, &edge_cache) + if edge_manifold.point_count == 1 { + m.normal = edge_manifold.normal + m.point_count = 1 + set_manifold_point(m, 0, manifold_point(&edge_manifold, 0)) + copy_sat_cache(cache, edge_cache) + } + core.free_bytes(edge_block, sizeof(LocalManifoldPoint)) + } +} diff --git a/aephysics/test_manifold.ae b/aephysics/test_manifold.ae new file mode 100644 index 0000000..bee214a --- /dev/null +++ b/aephysics/test_manifold.ae @@ -0,0 +1,901 @@ +// aephysics.manifold against the reference's (Box3D's) test_sat.c and the +// hull, sphere and capsule parts of test_manifold.c: the separating axis +// against a brute-force oracle over 7,000 random box pairs, crossed edges +// from speculative contact into deep overlap, the edge cache, the edge +// endpoint, parallel edges at the noise floor, overlap never empty, the +// ridge crossing policy, the edge axis oracles, the capsule stabbed +// through an edge, the sphere and capsule seams, the capsule's deep face +// contact, and sphere and capsule pairs. The triangle parts wait for the +// triangle manifold. + +import std.string +import aephysics.math +import aephysics.core +import aephysics.distance +import aephysics.hull +import aephysics.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("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 + +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) } +} + +slide_x(x: float) -> Transform { return Transform { p: math.vec3(x, 0.0, 0.0), q: math.quat_identity() } } + +// A point buffer with room for eight points. +new_manifold() -> 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 separating axis against a brute-force oracle ----------------------------- + +// The projection gap min over B minus max over A along a unit axis from A to B. +sep_along(hull_a: *HullData, hull_b: *HullData, xf_b: Transform, n: Vec3) -> float { + points_a = hull.hull_points(hull_a) + max_a = 0.0 - math.MAX_FLOAT + i = 0 + while i < hull_a.vertex_count { + max_a = math.max_float(max_a, math.dot(n, points_a[i])) + i = i + 1 + } + points_b = hull.hull_points(hull_b) + min_b = math.MAX_FLOAT + i = 0 + while i < hull_b.vertex_count { + min_b = math.min_float(min_b, math.dot(n, math.transform_point(xf_b, points_b[i]))) + i = i + 1 + } + return min_b - max_a +} + +// Every face normal and every edge cross product, both ways: the true maximum. +oracle_separation(hull_a: *HullData, hull_b: *HullData, xf_b: Transform) -> float { + best = 0.0 - math.MAX_FLOAT + planes_a = hull.hull_planes(hull_a) + i = 0 + while i < hull_a.face_count { + best = math.max_float(best, sep_along(hull_a, hull_b, xf_b, planes_a[i].normal)) + best = math.max_float(best, sep_along(hull_a, hull_b, xf_b, math.neg(planes_a[i].normal))) + i = i + 1 + } + planes_b = hull.hull_planes(hull_b) + i = 0 + while i < hull_b.face_count { + n = math.rotate_vector(xf_b.q, planes_b[i].normal) + best = math.max_float(best, sep_along(hull_a, hull_b, xf_b, n)) + best = math.max_float(best, sep_along(hull_a, hull_b, xf_b, math.neg(n))) + i = i + 1 + } + edges_a = hull.hull_edges(hull_a) + points_a = hull.hull_points(hull_a) + edges_b = hull.hull_edges(hull_b) + points_b = hull.hull_points(hull_b) + i = 0 + while i < hull_a.edge_count { + dir_a = math.sub(points_a[edges_a[i + 1].origin], points_a[edges_a[i].origin]) + j = 0 + while j < hull_b.edge_count { + dir_b = math.rotate_vector(xf_b.q, math.sub(points_b[edges_b[j + 1].origin], points_b[edges_b[j].origin])) + c = math.cross(dir_a, dir_b) + if math.dot(c, c) >= 0.0000000001 { + c = math.normalize(c) + best = math.max_float(best, sep_along(hull_a, hull_b, xf_b, c)) + best = math.max_float(best, sep_along(hull_a, hull_b, xf_b, math.neg(c))) + } + j = j + 2 + } + i = i + 2 + } + return best +} + +var seed = 987654321 + +next_float(lower: float, upper: float) -> float { + seed = 1664525 * seed + 1013904223 + t = (((seed >> 8) & 0xFFFFFF) as float) / 16777216.0 + return lower + t * (upper - lower) +} + +next_direction() -> Vec3 { + return math.normalize(math.vec3(next_float(0.0 - 1.0, 1.0), next_float(0.0 - 1.0, 1.0), next_float(0.0 - 1.0, 1.0))) +} + +test_sat_fixed() { + // Two cubes apart along x: A's +x face, the gap 0.2, B's -x vertex. + a = hull.make_box_hull(0.5, 0.5, 0.5) + b = hull.make_box_hull(0.5, 0.5, 0.5) + xf = slide_x(1.2) + aq = manifold.compute_separating_axis(a, b, xf, true) + q = manifold.get_best_axis(&aq) + ensure("sat face a: kind", q.kind == manifold.AXIS_FACE_A) + small("sat face a: separation", q.separation - 0.2, 0.00001) + small("sat face a: normal x", q.normal.x - 1.0, 0.000001) + small("sat face a: normal y", q.normal.y, 0.000001) + planes_a = hull.hull_planes(a) + small("sat face a: the +x face", planes_a[q.index_a].normal.x - 1.0, 0.000001) + points_b = hull.hull_points(b) + small("sat face a: B's -x vertex", points_b[q.index_b].x + 0.5, 0.000001) + small("sat face a: consistent", sep_along(a, b, xf, q.normal) - q.separation, 0.00001) + hull.destroy_hull(b) + + // A rotated so no face of it faces B: B's face wins. + a_rot = hull.make_transformed_box_hull(0.5, 0.5, 0.5, exact_rotation(math.vec3_axis_y(), 0.25 * math.PI)) + a_extent = 0.5 * ROOT2 + b = hull.make_box_hull(0.5, 0.5, 0.5) + gap = 0.2 + xf = slide_x(a_extent + 0.5 + gap) + aq = manifold.compute_separating_axis(a_rot, b, xf, true) + q = manifold.get_best_axis(&aq) + ensure("sat face b: kind", q.kind == manifold.AXIS_FACE_B) + small("sat face b: separation", q.separation - gap, 0.00001) + small("sat face b: normal x", q.normal.x - 1.0, 0.00001) + planes_b = hull.hull_planes(b) + small("sat face b: B's -x face", planes_b[q.index_b].normal.x + 1.0, 0.00001) + points_a = hull.hull_points(a_rot) + small("sat face b: A's +x vertex", points_a[q.index_a].x - a_extent, 0.0001) + small("sat face b: consistent", sep_along(a_rot, b, xf, q.normal) - q.separation, 0.0001) + hull.destroy_hull(b) + + // Far apart: some separating axis, the true one here. + b = hull.make_box_hull(0.5, 0.5, 0.5) + xf = slide_x(3.0) + aq = manifold.compute_separating_axis(a, b, xf, true) + q = manifold.get_best_axis(&aq) + ensure("sat far: kind", q.kind == manifold.AXIS_FACE_A) + small("sat far: separation", q.separation - 2.0, 0.00001) + small("sat far: oracle", q.separation - oracle_separation(a, b, xf), 0.00001) + hull.destroy_hull(b) + + // B offset in its own frame: the support has to cope. + b_off = hull.make_offset_box_hull(0.5, 0.5, 0.5, math.vec3(3.0, 0.0, 0.0)) + xf = Transform { p: math.vec3(a_extent + 0.5 + gap - 3.0, 0.0, 0.0), q: math.quat_identity() } + aq = manifold.compute_separating_axis(a_rot, b_off, xf, true) + q = manifold.get_best_axis(&aq) + ensure("sat offset: kind", q.kind == manifold.AXIS_FACE_B) + small("sat offset: separation", q.separation - gap, 0.0001) + small("sat offset: normal x", q.normal.x - 1.0, 0.00001) + small("sat offset: oracle", q.separation - oracle_separation(a_rot, b_off, xf), 0.0001) + small("sat offset: consistent", sep_along(a_rot, b_off, xf, q.normal) - q.separation, 0.0001) + hull.destroy_hull(b_off) + hull.destroy_hull(a) + + // Crossed edges: A yawed 45 about y, B rolled 45 about z, slid along x. + b_rot = hull.make_transformed_box_hull(0.5, 0.5, 0.5, exact_rotation(math.vec3_axis_z(), 0.25 * math.PI)) + distances = [ 1.43, 1.42, ROOT2, 1.40, 1.38, 1.35, 1.30 ] + i = 0 + while i < 7 { + d = distances[i] + xf = slide_x(d) + aq = manifold.compute_separating_axis(a_rot, b_rot, xf, true) + q = manifold.get_best_axis(&aq) + ensure("sat edge sweep: kind (${d})", q.kind == manifold.AXIS_EDGE_PAIR) + small("sat edge sweep: separation", q.separation - (d - ROOT2), 0.0001) + small("sat edge sweep: normal x", q.normal.x - 1.0, 0.0001) + small("sat edge sweep: normal y", q.normal.y, 0.0001) + small("sat edge sweep: normal z", q.normal.z, 0.0001) + ensure("sat edge sweep: even half edges", (q.index_a & 1) == 0 && (q.index_b & 1) == 0) + small("sat edge sweep: oracle", q.separation - oracle_separation(a_rot, b_rot, xf), 0.0001) + small("sat edge sweep: consistent", sep_along(a_rot, b_rot, xf, q.normal) - q.separation, 0.0001) + i = i + 1 + } + hull.destroy_hull(a_rot) + hull.destroy_hull(b_rot) +} + +// A wide net of random box pairs, thin crossed beams among them, from +// clearly separated to deep overlap. The axis must reproduce its own +// separation, never exceed the true maximum, and within the speculative +// band never fall short of it. +test_sat_oracle() { + seed = 987654321 + separated = 0 + penetrating = 0 + edge_wins = 0 + worst_shortfall = 0.0 + worst_excess = 0.0 + i = 0 + while i < 4000 { + half_a = math.vec3(next_float(0.3, 0.8), next_float(0.3, 0.8), next_float(0.3, 0.8)) + half_b = math.vec3(next_float(0.3, 0.8), next_float(0.3, 0.8), next_float(0.3, 0.8)) + reach = next_float(0.0, 1.7) + if (i & 1) != 0 { + half_a = math.vec3(next_float(1.0, 1.6), next_float(0.08, 0.15), next_float(0.08, 0.15)) + half_b = math.vec3(next_float(1.0, 1.6), next_float(0.08, 0.15), next_float(0.08, 0.15)) + reach = next_float(0.0, 0.4) + } + a = hull.make_transformed_box_hull(half_a.x, half_a.y, half_a.z, exact_rotation(next_direction(), next_float(0.0, math.PI))) + b = hull.make_box_hull(half_b.x, half_b.y, half_b.z) + xf = Transform { p: math.mul_sv(reach, next_direction()), q: exact_quat(next_direction(), next_float(0.0, math.PI)) } + aq = manifold.compute_separating_axis(a, b, xf, true) + q = manifold.get_best_axis(&aq) + oracle = oracle_separation(a, b, xf) + small("oracle: unit normal", math.length(q.normal) - 1.0, 0.001) + small("oracle: consistent", sep_along(a, b, xf, q.normal) - q.separation, 0.002) + worst_excess = math.max_float(worst_excess, q.separation - oracle) + ensure("oracle: never above the true maximum", q.separation <= oracle + 0.002) + if q.separation <= manifold.SPECULATIVE_DISTANCE { + worst_shortfall = math.max_float(worst_shortfall, oracle - q.separation) + ensure("oracle: in band, never short of it", q.separation >= oracle - 0.004) + } + if oracle > 0.0 { separated = separated + 1 } else { penetrating = penetrating + 1 } + if q.kind == manifold.AXIS_EDGE_PAIR { edge_wins = edge_wins + 1 } + hull.destroy_hull(a) + hull.destroy_hull(b) + i = i + 1 + } + println("manifold: oracle separated=${separated} penetrating=${penetrating} edge wins=${edge_wins} worst shortfall=${worst_shortfall} worst excess=${worst_excess}") + ensure("oracle: both regimes and the edge path", separated > 100 && penetrating > 100 && edge_wins > 20) + + // Hulls whose vertices sit far from their own origin. + seed = 24681012 + separated = 0 + penetrating = 0 + edge_wins = 0 + i = 0 + while i < 3000 { + half_a = math.vec3(next_float(0.3, 0.8), next_float(0.3, 0.8), next_float(0.3, 0.8)) + half_b = math.vec3(next_float(0.3, 0.8), next_float(0.3, 0.8), next_float(0.3, 0.8)) + offset = math.mul_sv(next_float(2.0, 6.0), next_direction()) + reach = next_float(0.0, 1.7) + dir = next_direction() + a = null + b = null + xf = math.transform_identity() + if (i & 1) != 0 { + place_a = Transform { p: offset, q: exact_quat(next_direction(), next_float(0.0, math.PI)) } + a = hull.make_transformed_box_hull(half_a.x, half_a.y, half_a.z, place_a) + b = hull.make_box_hull(half_b.x, half_b.y, half_b.z) + xf = Transform { p: math.mul_add(offset, reach, dir), q: exact_quat(next_direction(), next_float(0.0, math.PI)) } + } else { + q_b = exact_quat(next_direction(), next_float(0.0, math.PI)) + a = hull.make_box_hull(half_a.x, half_a.y, half_a.z) + b = hull.make_offset_box_hull(half_b.x, half_b.y, half_b.z, offset) + xf = Transform { p: math.sub(math.mul_sv(reach, dir), math.rotate_vector(q_b, offset)), q: q_b } + } + aq = manifold.compute_separating_axis(a, b, xf, true) + q = manifold.get_best_axis(&aq) + oracle = oracle_separation(a, b, xf) + consistency = math.abs_float(sep_along(a, b, xf, q.normal) - q.separation) + small("offset oracle: unit normal", math.length(q.normal) - 1.0, 0.001) + ensure("offset oracle: consistent (${consistency})", consistency < 0.001) + ensure("offset oracle: never above the true maximum", q.separation <= oracle + 0.003) + if q.separation <= manifold.SPECULATIVE_DISTANCE { + ensure("offset oracle: in band, never short", q.separation >= oracle - 0.008) + } + if oracle > 0.0 { separated = separated + 1 } else { penetrating = penetrating + 1 } + if q.kind == manifold.AXIS_EDGE_PAIR { edge_wins = edge_wins + 1 } + hull.destroy_hull(a) + hull.destroy_hull(b) + i = i + 1 + } + ensure("offset oracle: both regimes and the edge path (${separated} ${penetrating} ${edge_wins})", separated > 100 && penetrating > 100 && edge_wins > 20) +} + +// --- hull against hull -------------------------------------------------------------- + +test_crossed_edge() { + a = hull.make_transformed_box_hull(0.5, 0.5, 0.5, exact_rotation(math.vec3_axis_y(), 0.25 * math.PI)) + b = hull.make_transformed_box_hull(0.5, 0.5, 0.5, exact_rotation(math.vec3_axis_z(), 0.25 * math.PI)) + points = new_manifold() + distances = [ 1.42, ROOT2, 1.41, 1.3 ] + i = 0 + while i < 4 { + d = distances[i] + expected = d - ROOT2 + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + manifold.collide_hulls(&m, 8, a, b, slide_x(d), &cache) + ensure("crossed: one point (${d})", m.point_count == 1) + ensure("crossed: edge pair", cache.kind == manifold.AXIS_EDGE_PAIR) + small("crossed: normal x", m.normal.x - 1.0, 0.000001) + small("crossed: normal y", m.normal.y, 0.000001) + small("crossed: normal z", m.normal.z, 0.000001) + if m.point_count == 1 { + p = manifold.manifold_point(&m, 0) + small("crossed: separation", p.separation - expected, 0.00001) + small("crossed: point x", p.point.x - 0.5 * d, 0.00001) + small("crossed: point y", p.point.y, 0.00001) + small("crossed: point z", p.point.z, 0.00001) + } + // The forced edge query agrees with the full solver. + manual = manifold.local_manifold(points) + manual_cache = manifold.empty_sat_cache() + manual_cache.kind = manifold.AXIS_MANUAL_EDGE_PAIR + manifold.collide_hulls(&manual, 8, a, b, slide_x(d), &manual_cache) + ensure("crossed manual: one point", manual.point_count == 1) + if manual.point_count == 1 { small("crossed manual: separation", manifold.manifold_point(&manual, 0).separation - expected, 0.00001) } + i = i + 1 + } + // Beyond the speculative distance: the axis, no contact. + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + manifold.collide_hulls(&m, 8, a, b, slide_x(1.5), &cache) + ensure("crossed far: no points", m.point_count == 0) + ensure("crossed far: edge pair cached", cache.kind == manifold.AXIS_EDGE_PAIR) + small("crossed far: cached separation", cache.separation - (1.5 - ROOT2), 0.00001) + + // The parallel-edge rejection is a sine threshold: it holds at any scale. + scales = [ 100.0, 1.0, 0.2 ] + i = 0 + while i < 3 { + s = scales[i] + a_s = hull.make_transformed_box_hull(0.5 * s, 0.5 * s, 0.5 * s, exact_rotation(math.vec3_axis_y(), 0.25 * math.PI)) + b_s = hull.make_transformed_box_hull(0.5 * s, 0.5 * s, 0.5 * s, exact_rotation(math.vec3_axis_z(), 0.25 * math.PI)) + expected = 0.0 - 0.002 + d = s * ROOT2 + expected + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + manifold.collide_hulls(&m, 8, a_s, b_s, slide_x(d), &cache) + tolerance = 0.00001 * s + 0.000001 + ensure("scale ${s}: one point", m.point_count == 1) + ensure("scale ${s}: edge pair", cache.kind == manifold.AXIS_EDGE_PAIR) + small("scale ${s}: normal x", m.normal.x - 1.0, 0.000001) + if m.point_count == 1 { + p = manifold.manifold_point(&m, 0) + small("scale ${s}: separation", p.separation - expected, tolerance) + small("scale ${s}: point x", p.point.x - 0.5 * d, tolerance) + small("scale ${s}: point y", p.point.y, tolerance) + small("scale ${s}: point z", p.point.z, tolerance) + } + hull.destroy_hull(a_s) + hull.destroy_hull(b_s) + i = i + 1 + } + + // The cached edge pair rebuilds the axis without a fresh query. + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + manifold.collide_hulls(&m, 8, a, b, slide_x(1.41), &cache) + ensure("edge cache: one point", m.point_count == 1) + ensure("edge cache: edge pair", cache.kind == manifold.AXIS_EDGE_PAIR) + ensure("edge cache: even half edges in range", (cache.index_a & 1) == 0 && cache.index_a < a.edge_count && (cache.index_b & 1) == 0 && cache.index_b < b.edge_count) + seeded = cache.separation + small("edge cache: seeded separation", seeded - (1.41 - ROOT2), 0.00001) + manifold.collide_hulls(&m, 8, a, b, slide_x(1.4105), &cache) + ensure("edge cache: still one point", m.point_count == 1) + ensure("edge cache: the cache answered", cache.separation == seeded && cache.hit == 1) + if m.point_count == 1 { small("edge cache: new separation", manifold.manifold_point(&m, 0).separation - (1.4105 - ROOT2), 0.00001) } + small("edge cache: normal x", m.normal.x - 1.0, 0.000001) + manifold.collide_hulls(&m, 8, a, b, slide_x(1.5), &cache) + ensure("edge cache: past the band, no points", m.point_count == 0) + ensure("edge cache: the cached axis alone answered", cache.separation == seeded) + + // Sliding B along edge A walks the closest point off the end. + inside = Transform { p: math.vec3(1.41, 0.49, 0.0), q: math.quat_identity() } + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + cache.kind = manifold.AXIS_MANUAL_EDGE_PAIR + manifold.collide_hulls(&m, 8, a, b, inside, &cache) + ensure("endpoint inside: one point", m.point_count == 1) + if m.point_count == 1 { + small("endpoint inside: separation", manifold.manifold_point(&m, 0).separation - (1.41 - ROOT2), 0.00001) + small("endpoint inside: point y", manifold.manifold_point(&m, 0).point.y - 0.49, 0.00001) + } + off = Transform { p: math.vec3(1.41, 0.55, 0.0), q: math.quat_identity() } + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + cache.kind = manifold.AXIS_MANUAL_EDGE_PAIR + manifold.collide_hulls(&m, 8, a, b, off, &cache) + ensure("endpoint off: no points", m.point_count == 0) + ensure("endpoint off: cache cleared", cache.kind == manifold.AXIS_INVALID) + fresh = manifold.empty_sat_cache() + manifold.collide_hulls(&m, 8, a, b, off, &fresh) + ensure("endpoint off: fresh, no points", m.point_count == 0) + ensure("endpoint off: fresh separation positive", fresh.separation > 0.0) + + free(points) + hull.destroy_hull(a) + hull.destroy_hull(b) +} + +// Cubes stacked face to face and tipped by a hair: the near-parallel +// edge pairs carry no information; the face contact has to survive. +test_parallel_edges() { + a = hull.make_box_hull(0.5, 0.5, 0.5) + b = hull.make_box_hull(0.5, 0.5, 0.5) + points = new_manifold() + overlap = 0.01 + 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 ] + half_diagonal = 0.87 + i = 0 + while i < 4 { + axis = math.vec3(axes[3 * i], axes[3 * i + 1], axes[3 * i + 2]) + j = 0 + while j < 11 { + angle = angles[j] + xf = Transform { p: math.vec3(0.0, 1.0 - overlap, 0.0), q: exact_quat(axis, angle) } + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + manifold.collide_hulls(&m, 8, a, b, xf, &cache) + ensure("parallel: four points (${i} ${j})", m.point_count == 4) + ensure("parallel: a face axis", cache.kind == manifold.AXIS_FACE_A || cache.kind == manifold.AXIS_FACE_B) + ensure("parallel: normal up", math.dot(m.normal, math.vec3_axis_y()) > 0.998) + bound = half_diagonal * angle + 0.00001 + k = 0 + while k < m.point_count { + small("parallel: separation", manifold.manifold_point(&m, k).separation + overlap, bound) + k = k + 1 + } + // Forced edge: exactly parallel finds nothing; otherwise never positive. + manual = manifold.local_manifold(points) + manual_cache = manifold.empty_sat_cache() + manual_cache.kind = manifold.AXIS_MANUAL_EDGE_PAIR + manifold.collide_hulls(&manual, 8, a, b, xf, &manual_cache) + if angle == 0.0 { + ensure("parallel manual: nothing at zero", manual.point_count == 0 && manual_cache.kind == manifold.AXIS_MANUAL_EDGE_PAIR) + } else if manual.point_count > 0 { + ensure("parallel manual: one point", manual.point_count == 1) + ensure("parallel manual: normal up", math.dot(manual.normal, math.vec3_axis_y()) > 0.99) + s = manifold.manifold_point(&manual, 0).separation + ensure("parallel manual: not positive (${s})", s <= 0.0 && s >= 0.0 - overlap - half_diagonal * angle - 0.0001) + } + j = j + 1 + } + i = i + 1 + } + free(points) + hull.destroy_hull(a) + hull.destroy_hull(b) +} + +// Overlapping hulls admit no separating axis: a manifold always comes out. +test_overlap_never_empty() { + seed = 12345 + a = hull.make_box_hull(0.5, 0.5, 0.5) + b = hull.make_box_hull(0.4, 0.6, 0.5) + points = new_manifold() + i = 0 + while i < 2000 { + axis = next_direction() + angle = next_float(0.0 - math.PI, math.PI) + if (i & 1) != 0 { angle = next_float(0.0 - 0.01, 0.01) } + offset = math.mul_sv(0.4, next_direction()) + xf = Transform { p: offset, q: exact_quat(axis, angle) } + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + manifold.collide_hulls(&m, 8, a, b, xf, &cache) + ensure("overlap: points", m.point_count > 0) + small("overlap: unit normal", math.length(m.normal) - 1.0, 0.00001) + ensure("overlap: the deepest penetrates", min_separation(&m) < 0.0) + i = i + 1 + } + free(points) + hull.destroy_hull(a) + hull.destroy_hull(b) +} + +check_roof_face(name: string, m: *LocalManifold, cache: *SATCache, overlap: float) { + ensure("${name}: four points", m.point_count == 4) + ensure("${name}: a face axis", cache.kind == manifold.AXIS_FACE_A || cache.kind == manifold.AXIS_FACE_B) + small("${name}: roof normal", m.normal.y - HALF_ROOT2, 0.0001) + s = min_separation(m) + ensure("${name}: separation in range (${s})", s < 0.0 - HALF_ROOT2 * overlap + 0.0001 && s > 0.0 - ROOT2 * overlap - 0.0001) +} + +// Two roof ridges across each other: the face contact by preference, the +// edge contact only when its axis beats the clipped face by more than the slop. +test_ridge_crossing() { + a = hull.make_transformed_box_hull(1.5, 0.1, 0.1, exact_rotation(math.vec3_axis_x(), 0.25 * math.PI)) + b = hull.make_transformed_box_hull(1.5, 0.1, 0.1, exact_rotation(math.vec3_axis_x(), 0.25 * math.PI)) + ridge_y = 0.1 * ROOT2 + points = new_manifold() + overlap = 0.01 + lift = 2.0 * ridge_y - overlap + shallow = [ 0.0, 0.001, 0.02, 0.1, 0.5 ] + i = 0 + while i < 5 { + xf = Transform { p: math.vec3(0.0, lift, 0.0), q: exact_quat(math.vec3_axis_y(), shallow[i]) } + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + manifold.collide_hulls(&m, 8, a, b, xf, &cache) + check_roof_face("ridge shallow ${i}", &m, &cache, overlap) + i = i + 1 + } + overlap = 0.05 + lift = 2.0 * ridge_y - overlap + deep = [ 0.05, 0.1, 0.2, 0.5 ] + i = 0 + while i < 4 { + xf = Transform { p: math.vec3(0.0, lift, 0.0), q: exact_quat(math.vec3_axis_y(), deep[i]) } + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + manifold.collide_hulls(&m, 8, a, b, xf, &cache) + ensure("ridge deep ${i}: one point", m.point_count == 1) + ensure("ridge deep ${i}: edge pair", cache.kind == manifold.AXIS_EDGE_PAIR) + small("ridge deep ${i}: normal x", m.normal.x, 0.0001) + small("ridge deep ${i}: normal y", m.normal.y - 1.0, 0.0001) + small("ridge deep ${i}: normal z", m.normal.z, 0.0001) + if m.point_count == 1 { + p = manifold.manifold_point(&m, 0) + small("ridge deep ${i}: separation", p.separation + overlap, 0.0001) + small("ridge deep ${i}: point y", p.point.y - (ridge_y - 0.5 * overlap), 0.0001) + small("ridge deep ${i}: point x", p.point.x, 0.01) + small("ridge deep ${i}: point z", p.point.z, 0.01) + } + i = i + 1 + } + near = [ 0.0, 0.0001, 0.001, 0.003 ] + i = 0 + while i < 4 { + xf = Transform { p: math.vec3(0.0, lift, 0.0), q: exact_quat(math.vec3_axis_y(), near[i]) } + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + manifold.collide_hulls(&m, 8, a, b, xf, &cache) + check_roof_face("ridge near ${i}", &m, &cache, overlap) + i = i + 1 + } + free(points) + hull.destroy_hull(a) + hull.destroy_hull(b) +} + +// The edge pair axis must match the classic cross product: rebuild the +// axis, the separation and the point from the two edges and compare. +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, t: Transform) -> Vec3 { + edges = hull.hull_edges(h) + points = hull.hull_points(h) + return math.transform_point(t, points[edges[edge_index].origin]) +} + +hull_edge_vector(h: *HullData, edge_index: int, t: Transform) -> Vec3 { + edges = hull.hull_edges(h) + points = hull.hull_points(h) + tail = math.transform_point(t, points[edges[edge_index].origin]) + head = math.transform_point(t, points[edges[edges[edge_index].twin].origin]) + return math.sub(head, tail) +} + +test_edge_axis_oracle() { + a = hull.make_transformed_box_hull(0.5, 0.5, 0.5, exact_rotation(math.vec3_axis_y(), 0.25 * math.PI)) + points = new_manifold() + // (An array literal of expressions such as 0.18 * PI is mis-typed by + // aetherc, so the rolls are computed.) + yaws = [ 0.0 - 0.35, 0.0 - 0.15, 0.0, 0.15, 0.35 ] + distances = [ 1.38, 1.40, ROOT2, 1.44 ] + edge_contacts = 0 + i = 0 + while i < 3 { + roll = (0.18 + 0.07 * (i as float)) * math.PI + b = hull.make_transformed_box_hull(0.5, 0.5, 0.5, exact_rotation(math.vec3_axis_z(), roll)) + j = 0 + while j < 5 { + k = 0 + while k < 4 { + xf = Transform { p: math.vec3(distances[k], 0.0, 0.0), q: exact_quat(math.vec3_axis_y(), yaws[j]) } + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + manifold.collide_hulls(&m, 8, a, b, xf, &cache) + if cache.kind == manifold.AXIS_EDGE_PAIR && m.point_count == 1 { + p1 = hull_edge_tail(a, cache.index_a, math.transform_identity()) + e1 = hull_edge_vector(a, cache.index_a, math.transform_identity()) + p2 = hull_edge_tail(b, cache.index_b, xf) + e2 = hull_edge_vector(b, cache.index_b, xf) + orient = math.sub(math.transform_point(xf, b.center), a.center) + check_edge_contact("edge oracle", &m, p1, e1, p2, e2, orient, 0.0, 0.0002, 0.0002, 0.002) + edge_contacts = edge_contacts + 1 + } + k = k + 1 + } + j = j + 1 + } + hull.destroy_hull(b) + i = i + 1 + } + ensure("edge oracle: drove the edge path (${edge_contacts})", edge_contacts >= 15) + + // Random pairs, the same check. + seed = 246813579 + edge_contacts = 0 + i = 0 + while i < 2000 { + angle_a = next_float(0.2, 0.5) * math.PI + angle_b = next_float(0.2, 0.5) * math.PI + a_r = hull.make_transformed_box_hull(0.5, 0.5, 0.5, exact_rotation(next_direction(), angle_a)) + b_r = hull.make_box_hull(0.5, 0.5, 0.5) + d = next_float(1.2, 1.55) + xf = Transform { p: math.mul_sv(d, next_direction()), q: exact_quat(next_direction(), angle_b) } + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + manifold.collide_hulls(&m, 8, a_r, b_r, xf, &cache) + if cache.kind == manifold.AXIS_EDGE_PAIR && m.point_count == 1 { + p1 = hull_edge_tail(a_r, cache.index_a, math.transform_identity()) + e1 = hull_edge_vector(a_r, cache.index_a, math.transform_identity()) + p2 = hull_edge_tail(b_r, cache.index_b, xf) + e2 = hull_edge_vector(b_r, cache.index_b, xf) + sine = math.length(math.cross(math.normalize(e1), math.normalize(e2))) + if sine >= 0.1 { + orient = math.sub(math.transform_point(xf, b_r.center), a_r.center) + check_edge_contact("random edge oracle", &m, p1, e1, p2, e2, orient, 0.0, 0.001, 0.001, 0.005) + edge_contacts = edge_contacts + 1 + } + } + hull.destroy_hull(a_r) + hull.destroy_hull(b_r) + i = i + 1 + } + ensure("random edge oracle: drove the edge path (${edge_contacts})", edge_contacts >= 100) + free(points) + hull.destroy_hull(a) +} + +// --- hull against capsule and sphere ----------------------------------------------------- + +// A thin capsule stabbed through the +x +y edge of a box: the isolated +// edge axis, nearly parallel to a face normal. +test_hull_capsule_edge_deep() { + h = hull.make_box_hull(0.5, 0.5, 0.5) + points = new_manifold() + edge_point = math.vec3(0.5, 0.5, 0.0) + edge_dir = math.vec3(0.0, 0.0, 1.0) + outward = math.normalize(math.vec3(1.0, 1.0, 0.0)) + depths = [ 0.12, 0.18, 0.25 ] + radii = [ 0.05, 0.1, 0.2 ] + tilts = [ 0.0, 0.25, 0.0 - 0.25 ] + count = 0 + i = 0 + while i < 3 { + j = 0 + while j < 3 { + k = 0 + while k < 3 { + dir = math.normalize(math.vec3(1.0, 0.0 - 1.0, tilts[k])) + mid = math.mul_add(edge_point, 0.0 - depths[i], outward) + c1 = math.mul_add(mid, 0.0 - 0.5, dir) + c2 = math.mul_add(mid, 0.5, dir) + c = manifold.capsule(c1, c2, radii[j]) + m = manifold.local_manifold(points) + cache = distance.empty_cache() + manifold.collide_hull_and_capsule(&m, 8, h, c, math.transform_identity(), &cache) + ensure("capsule edge deep: one point (${i} ${j} ${k})", m.point_count == 1) + if m.point_count == 1 { + ensure("capsule edge deep: penetrating", manifold.manifold_point(&m, 0).separation < 0.0) + check_edge_contact("capsule edge deep", &m, edge_point, edge_dir, c1, math.sub(c2, c1), outward, radii[j], 0.0001, 0.0001, 0.0001) + } + count = count + 1 + k = k + 1 + } + j = j + 1 + } + i = i + 1 + } + ensure("capsule edge deep: all cases", count == 27) + free(points) + hull.destroy_hull(h) +} + +// A sphere lowered through a box face: the separation is the analytic +// gap on both sides of the seam between the GJK and the SAT branch. +test_sphere_hull_seam() { + h = hull.make_box_hull(0.5, 0.5, 0.5) + points = new_manifold() + radius = 0.15 + y_start = 0.5 + radius + 0.4 * manifold.SPECULATIVE_DISTANCE + y_end = 0.1 + steps = 400 + dy = (y_start - y_end) / (steps as float) + previous = 0.0 + shallow = 0 + deep = 0 + i = 0 + while i <= steps { + y = y_start - (i as float) * dy + s = manifold.sphere(math.vec3(0.0, y, 0.0), radius) + m = manifold.local_manifold(points) + cache = distance.empty_cache() + manifold.collide_hull_and_sphere(&m, 8, h, s, math.transform_identity(), &cache) + ensure("sphere seam: one point", m.point_count == 1) + if m.point_count == 1 { + separation = manifold.manifold_point(&m, 0).separation + expected = (y - 0.5) - radius + small("sphere seam: separation", separation - expected, 0.00001) + small("sphere seam: normal x", m.normal.x, 0.00001) + small("sphere seam: normal y", m.normal.y - 1.0, 0.00001) + small("sphere seam: normal z", m.normal.z, 0.00001) + if i > 0 { small("sphere seam: no jump", (previous - separation) - dy, 0.00001) } + previous = separation + } + if y > 0.5 { shallow = shallow + 1 } else { deep = deep + 1 } + i = i + 1 + } + ensure("sphere seam: both branches", shallow > 0 && deep > 0) + + // The same seam for a capsule laid along the face: two points, at the gap. + half_length = 0.3 + previous = 0.0 + shallow = 0 + deep = 0 + i = 0 + while i <= steps { + y = y_start - (i as float) * dy + c = manifold.capsule(math.vec3(0.0 - half_length, y, 0.0), math.vec3(half_length, y, 0.0), radius) + m = manifold.local_manifold(points) + cache = distance.empty_cache() + manifold.collide_hull_and_capsule(&m, 8, h, c, math.transform_identity(), &cache) + ensure("capsule seam: points", m.point_count >= 1) + expected = (y - 0.5) - radius + k = 0 + while k < m.point_count { + small("capsule seam: separation", manifold.manifold_point(&m, k).separation - expected, 0.00001) + k = k + 1 + } + small("capsule seam: normal x", m.normal.x, 0.00001) + small("capsule seam: normal y", m.normal.y - 1.0, 0.00001) + small("capsule seam: normal z", m.normal.z, 0.00001) + ms = min_separation(&m) + if i > 0 { small("capsule seam: no jump", (previous - ms) - dy, 0.00001) } + previous = ms + if y > 0.5 { shallow = shallow + 1 } else { deep = deep + 1 } + i = i + 1 + } + ensure("capsule seam: both branches", shallow > 0 && deep > 0) + + // A capsule core inside the box: two points on the top face. + depths = [ 0.1, 0.2, 0.3, 0.4, 0.45 ] + yaws = [ 0.0, 0.4, 0.8, 1.2 ] + radii = [ 0.1, 0.15, 0.2 ] + offsets = [ 0.0 - 0.1, 0.0, 0.1 ] + face_contacts = 0 + i = 0 + while i < 5 { + j = 0 + while j < 4 { + r = 0 + while r < 3 { + o = 0 + while o < 3 { + y = depths[i] + dir = math.vec3(cos(yaws[j]), 0.0, sin(yaws[j])) + center = math.vec3(offsets[o], y, 0.0) + c = manifold.capsule(math.mul_add(center, 0.0 - half_length, dir), math.mul_add(center, half_length, dir), radii[r]) + m = manifold.local_manifold(points) + cache = distance.empty_cache() + manifold.collide_hull_and_capsule(&m, 8, h, c, math.transform_identity(), &cache) + ensure("capsule face deep: two points (${i} ${j} ${r} ${o})", m.point_count == 2) + 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) + expected = (y - 0.5) - radii[r] + if m.point_count == 2 { + small("capsule face deep: separation 0", manifold.manifold_point(&m, 0).separation - expected, 0.00001) + small("capsule face deep: separation 1", manifold.manifold_point(&m, 1).separation - expected, 0.00001) + } + ensure("capsule face deep: penetrating", expected < 0.0) + face_contacts = face_contacts + 1 + o = o + 1 + } + r = r + 1 + } + j = j + 1 + } + i = i + 1 + } + ensure("capsule face deep: all cases", face_contacts == 180) + free(points) + hull.destroy_hull(h) +} + +// Sphere and capsule pairs at analytic distances. +test_rounded_pairs() { + points = new_manifold() + a = manifold.sphere(math.vec3_zero(), 0.5) + b = manifold.sphere(math.vec3_zero(), 0.25) + m = manifold.local_manifold(points) + manifold.collide_spheres(&m, 8, a, b, Transform { p: math.vec3(0.7, 0.0, 0.0), q: math.quat_identity() }) + ensure("spheres: one point", m.point_count == 1) + if m.point_count == 1 { + small("spheres: separation", manifold.manifold_point(&m, 0).separation + 0.05, 0.00001) + small("spheres: point at the midpoint", manifold.manifold_point(&m, 0).point.x - 0.475, 0.00001) + } + small("spheres: normal", m.normal.x - 1.0, 0.00001) + m = manifold.local_manifold(points) + manifold.collide_spheres(&m, 8, a, b, Transform { p: math.vec3(0.8, 0.0, 0.0), q: math.quat_identity() }) + ensure("spheres: apart is empty", m.point_count == 0) + + c = manifold.capsule(math.vec3(0.0 - 1.0, 0.0, 0.0), math.vec3(1.0, 0.0, 0.0), 0.2) + m = manifold.local_manifold(points) + manifold.collide_capsule_and_sphere(&m, 8, c, b, Transform { p: math.vec3(0.3, 0.4, 0.0), q: math.quat_identity() }) + ensure("capsule-sphere: one point", m.point_count == 1) + if m.point_count == 1 { small("capsule-sphere: separation", manifold.manifold_point(&m, 0).separation + 0.05, 0.00001) } + small("capsule-sphere: normal y", m.normal.y - 1.0, 0.00001) + + // Parallel capsules: two points; crossed: one. + d = manifold.capsule(math.vec3(0.0 - 0.5, 0.0, 0.0), math.vec3(0.5, 0.0, 0.0), 0.2) + m = manifold.local_manifold(points) + manifold.collide_capsules(&m, 8, c, d, Transform { p: math.vec3(0.2, 0.39, 0.0), q: math.quat_identity() }) + ensure("capsules parallel: two points", m.point_count == 2) + small("capsules parallel: normal y", m.normal.y - 1.0, 0.00001) + if m.point_count == 2 { + small("capsules parallel: separation 0", manifold.manifold_point(&m, 0).separation + 0.01, 0.00001) + small("capsules parallel: separation 1", manifold.manifold_point(&m, 1).separation + 0.01, 0.00001) + } + m = manifold.local_manifold(points) + manifold.collide_capsules(&m, 8, c, d, Transform { p: math.vec3(0.0, 0.39, 0.0), q: exact_quat(math.vec3_axis_y(), 0.5 * math.PI) }) + ensure("capsules crossed: one point", m.point_count == 1) + if m.point_count == 1 { small("capsules crossed: separation", manifold.manifold_point(&m, 0).separation + 0.01, 0.00001) } + m = manifold.local_manifold(points) + manifold.collide_capsules(&m, 8, c, d, Transform { p: math.vec3(0.0, 0.5, 0.0), q: math.quat_identity() }) + ensure("capsules apart: empty", m.point_count == 0) + free(points) +} + +main() { + before = core.alloc_count() + test_sat_fixed() + test_sat_oracle() + test_crossed_edge() + test_parallel_edges() + test_overlap_never_empty() + test_ridge_crossing() + test_edge_axis_oracle() + test_hull_capsule_edge_deep() + test_sphere_hull_seam() + test_rounded_pairs() + // The module's clip scratch stays allocated: three blocks. + ensure("every other counted allocation was freed", core.alloc_count() == before + 3) + + println("manifold: ${checks} checks") + if failures == 0 { + println("manifold: all checks passed") + } else { + println("manifold: ${failures} failure(s)") + exit(1) + } +} diff --git a/bench/RESULTS.md b/bench/RESULTS.md index 7805149..7ffcdcc 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -147,3 +147,25 @@ The sums of the results agree to the precision of the reference's floats 335,990): the same algorithm taking the same paths. The port runs at 1.3-1.5x the reference's time; the simplex is passed by value here where the reference works on it in place. + +## manifold + +`bench/manifold.ae` and `bench/manifold_box3d.c`: 20,000 hull-hull +collisions of two boxes (1 x 0.5 x 0.75 and 0.6 x 0.8 x 0.4) along a sweep +through overlap with a cold SAT cache each time; the same 20,000 with the +cache carried from pose to pose (a stack's steady state); 20,000 +hull-capsule and 20,000 hull-sphere collisions along a sweep. + +| phase | aephysics | Box3D | +|---|---|---| +| 20,000 hull-hull, cold cache | 15.3 ms | **9.5** | +| 20,000 hull-hull, warm cache | 3.7 | **3.6** | +| 20,000 hull-capsule | 7.2 | **5.0** | +| 20,000 hull-sphere | 4.0 | **2.6** | + +The same manifolds come out: 52,386 / 52,074 / 23,482 / 11,500 contact +points, 19,706 cache hits, and equal separation sums on every phase. With +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. diff --git a/bench/manifold.ae b/bench/manifold.ae new file mode 100644 index 0000000..a31edb9 --- /dev/null +++ b/bench/manifold.ae @@ -0,0 +1,114 @@ +// The contact manifolds on the same scenes as bench/manifold_box3d.c: +// 20,000 hull-hull collisions of two rotated boxes along a sweep through +// overlap, cold cache; the same 20,000 with the cache carried between +// poses; 20,000 hull-capsule and 20,000 hull-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 + +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() { + hull_a = hull.make_box_hull(1.0, 0.5, 0.75) + hull_b = hull.make_box_hull(0.6, 0.8, 0.4) + axis = math.normalize(math.vec3(1.0, 2.0, 0.5)) + points = calloc(8, sizeof(LocalManifoldPoint)) + + points_cold = 0 + sep_cold = 0.0 + t0 = clock() + i = 0 + while i < N { + t = (i as float) / (N as float) + xf = Transform { p: math.vec3(2.2 - 2.0 * t, 0.3 * sin(4.0 * t), 0.4 * cos(2.0 * t)), q: exact_quat(axis, 3.0 * t) } + m = manifold.local_manifold(points) + cache = manifold.empty_sat_cache() + manifold.collide_hulls(&m, 8, hull_a, hull_b, xf, &cache) + 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() + i = 0 + while i < N { + t = (i as float) / (N as float) + xf = Transform { p: math.vec3(2.2 - 2.0 * t, 0.3 * sin(4.0 * t), 0.4 * cos(2.0 * t)), q: exact_quat(axis, 3.0 * t) } + m = manifold.local_manifold(points) + manifold.collide_hulls(&m, 8, hull_a, hull_b, xf, &warm) + 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 + i = 0 + while i < N { + t = (i as float) / (N as float) + c = manifold.capsule(math.vec3(0.0 - 0.4, 0.0, 0.0), math.vec3(0.4, 0.0, 0.0), 0.15) + xf = Transform { p: math.vec3(0.3 * sin(3.0 * t), 1.4 - 1.6 * t, 0.2), q: exact_quat(axis, 2.0 * t) } + m = manifold.local_manifold(points) + simplex_cache = distance.empty_cache() + manifold.collide_hull_and_capsule(&m, 8, hull_a, c, xf, &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_zero(), 0.2) + xf = Transform { p: math.vec3(0.5 * sin(5.0 * t), 1.4 - 1.6 * t, 0.3 * cos(3.0 * t)), q: math.quat_identity() } + m = manifold.local_manifold(points) + simplex_cache = distance.empty_cache() + manifold.collide_hull_and_sphere(&m, 8, hull_a, s, xf, &simplex_cache) + points_sphere = points_sphere + m.point_count + sep_sphere = sep_sphere + sum_separations(&m) + i = i + 1 + } + t4 = clock() + + println("aephysics manifold: ${N} hull-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} hull-capsule ${ms(t3 - t2)} ms (${points_capsule} points, sum ${sep_capsule}), ${N} hull-sphere ${ms(t4 - t3)} ms (${points_sphere} points, sum ${sep_sphere})") + free(points) + hull.destroy_hull(hull_a) + hull.destroy_hull(hull_b) +} diff --git a/bench/manifold_box3d.c b/bench/manifold_box3d.c new file mode 100644 index 0000000..3b4f4ec --- /dev/null +++ b/bench/manifold_box3d.c @@ -0,0 +1,105 @@ +// The contact manifolds of the reference on the same scenes as +// bench/manifold.ae: 20,000 hull-hull collisions of two rotated boxes +// along a sweep through overlap, cold cache; the same 20,000 with the +// cache carried between poses; 20,000 hull-capsule and 20,000 +// hull-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 ) +{ + b3BoxHull hullA = b3MakeBoxHull( 1.0f, 0.5f, 0.75f ); + b3BoxHull hullB = b3MakeBoxHull( 0.6f, 0.8f, 0.4f ); + b3Vec3 axis = b3Normalize( (b3Vec3){ 1.0f, 2.0f, 0.5f } ); + b3LocalManifoldPoint points[8]; + + int pointsCold = 0; + double sepCold = 0.0; + double t0 = now_ms(); + for ( int i = 0; i < N; ++i ) + { + float t = (float)i / (float)N; + b3Transform xf = { { 2.2f - 2.0f * t, 0.3f * sinf( 4.0f * t ), 0.4f * cosf( 2.0f * t ) }, exact_quat( axis, 3.0f * t ) }; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SATCache cache = { 0 }; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, xf, &cache ); + 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 }; + for ( int i = 0; i < N; ++i ) + { + float t = (float)i / (float)N; + b3Transform xf = { { 2.2f - 2.0f * t, 0.3f * sinf( 4.0f * t ), 0.4f * cosf( 2.0f * t ) }, exact_quat( axis, 3.0f * t ) }; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3CollideHulls( &manifold, 8, &hullA.base, &hullB.base, xf, &warm ); + 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; + for ( int i = 0; i < N; ++i ) + { + float t = (float)i / (float)N; + b3Capsule capsule = { { -0.4f, 0.0f, 0.0f }, { 0.4f, 0.0f, 0.0f }, 0.15f }; + b3Transform xf = { { 0.3f * sinf( 3.0f * t ), 1.4f - 1.6f * t, 0.2f }, exact_quat( axis, 2.0f * t ) }; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SimplexCache cache = { 0 }; + b3CollideHullAndCapsule( &manifold, 8, &hullA.base, &capsule, xf, &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 = { { 0.0f, 0.0f, 0.0f }, 0.2f }; + b3Transform xf = { { 0.5f * sinf( 5.0f * t ), 1.4f - 1.6f * t, 0.3f * cosf( 3.0f * t ) }, b3Quat_identity }; + b3LocalManifold manifold = { 0 }; + manifold.points = points; + b3SimplexCache cache = { 0 }; + b3CollideHullAndSphere( &manifold, 8, &hullA.base, &sphere, xf, &cache ); + pointsSphere += manifold.pointCount; + for ( int k = 0; k < manifold.pointCount; ++k ) sepSphere += points[k].separation; + } + double t4 = now_ms(); + + printf( "box3d manifold: %d hull-hull cold %.2f ms (%d points, sum %.3f), warm %.2f ms (%d points, sum %.3f, %d cache hits), " + "%d hull-capsule %.2f ms (%d points, sum %.3f), %d hull-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 c564911..a135589 100644 --- a/design.md +++ b/design.md @@ -48,23 +48,29 @@ started until its tests pass. no axis separating more than the distance, rotating sweeps in the time of impact, the hull's overlap and cast through it. The same results as the reference at 1.3-1.5x its time. -6. **collision, static**: `manifold` and `convex_manifold` - (sphere/capsule/hull contact manifolds), `triangle_manifold`, `mesh`, - `height_field`, `shape` (mass properties, ray and shape casts per - shape). Tests: `test_collision`, `test_manifold`, `test_sat`, - `test_shape`, `test_mesh`, `test_height_field`. -7. **dynamics**: `body`, `contact`, `constraint_graph` (graph colouring), +6. **manifold** (done): manifold.c and convex_manifold.c as + `aephysics.manifold`, the reference's scalar SAT path, module scratch + for the clip buffers (per-worker in the parallel layer). 43,090 checks: + test_sat.c's oracle over 7,000 random pairs and test_manifold.c's hull, + 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), `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`. -8. **parallel**: `parallel_for` and the scheduler over Aether's actors; +9. **parallel**: `parallel_for` and the scheduler over Aether's actors; the benchmarks by thread count as the original records them. -9. **recording and replay**, `world_snapshot`: last, since they are the +10. **recording and replay**, `world_snapshot`: last, since they are the tooling and not the engine. -10. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run +11. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run against the C build on the same machine, recorded under `benchmark/`. ## Measures