From b3e63559eca888d7f7e1f94b5623f12103c68f0b Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Sat, 19 Sep 2026 17:23:57 -0300 Subject: [PATCH] aephysics.distance: GJK, the shape cast, the time of impact Box3D's distance.c in Aether: the simplex solved by Voronoi regions with barycentric coordinates, the support search warm-started from a cache whose metric is checked, the duplicate support as the termination; the shape cast by conservative advancement on the distance; the time of impact that builds a separation function (vertices, edges, a face of either) from the cached simplex and root-finds on it with false position and bisection in turn. The simplex's vertices are named fields, the cache's index pairs named ints; CastOutput moves here from the hull, which now has overlap_hull, shape_cast_hull and hull_proxy through it. test_distance.ae: the reference's four checks and 1,139 more -- spheres and capsules against boxes at analytic distances, overlap, the warm cache agreeing with a cold one over 200 poses, witness points inside their shapes and as far apart as the distance, no axis separating more than it, a rotating box falling on a slab in the time of impact (hit, miss, overlapped), and the hull's queries. bench/distance.ae against bench/distance_box3d.c: the same results (distance, cast and impact sums equal to the reference's float precision, GJK iterations within 0.01%) at 1.3-1.5x its time. --- README.md | 3 +- aephysics/distance/module.ae | 1124 ++++++++++++++++++++++++++++++++++ aephysics/hull/module.ae | 38 +- aephysics/test_distance.ae | 311 ++++++++++ bench/RESULTS.md | 23 + bench/distance.ae | 114 ++++ bench/distance_box3d.c | 119 ++++ design.md | 28 +- 8 files changed, 1736 insertions(+), 24 deletions(-) create mode 100644 aephysics/distance/module.ae create mode 100644 aephysics/test_distance.ae create mode 100644 bench/distance.ae create mode 100644 bench/distance_box3d.c diff --git a/README.md b/README.md index 5e6b311..307c684 100644 --- a/README.md +++ b/README.md @@ -29,7 +29,8 @@ so a test written against the reference reads the same here. | `aephysics.core` | bit set, id pool, hash set, arrays, the stack and arena allocators | done, `test_core.ae` (100k checks) | | `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.collision` | GJK distance and shape cast, contact manifolds, triangle mesh, height field, shapes with mass properties, ray and shape casts | next | +| `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.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/distance/module.ae b/aephysics/distance/module.ae new file mode 100644 index 0000000..42ff178 --- /dev/null +++ b/aephysics/distance/module.ae @@ -0,0 +1,1124 @@ +// aephysics.distance -- the distance between convex point clouds by GJK, +// the shape cast by conservative advancement on it, and the time of +// impact of two sweeps by separating-axis root finding. +// +// The shape is Box3D's distance.c (Erin Catto, with portions by Dirk +// Gregorius, MIT), the reference this engine is measured against: the +// simplex solved by Voronoi regions with barycentric coordinates, the +// support search warm-started from a cache whose metric is checked, the +// duplicate support as the termination, and the time of impact that +// builds a separation function (vertices, edges, or a face of either) +// from the cached simplex and root-finds on it. Names are the reference's +// without its prefix, in snake case: b3ShapeDistance is shape_distance. +// +// Differences: the simplex's four vertices are named fields rather than +// an array (a simplex is passed by value and never allocated); the cache +// holds its four index pairs as named ints; the debug simplex trail is a +// block of Simplex structs the caller may pass. +import std.string +import aephysics.math + +exports ( + ShapeProxy, SimplexCache, SimplexVertex, Simplex, DistanceInput, DistanceOutput, + ShapeCastPairInput, CastOutput, Sweep, TOIInput, TOIOutput, + MAX_SIMPLEX_VERTICES, MAX_GJK_ITERATIONS, MAX_SHAPE_CAST_POINTS, NULL_INDEX, + TOI_STATE_UNKNOWN, TOI_STATE_FAILED, TOI_STATE_OVERLAPPED, TOI_STATE_HIT, TOI_STATE_SEPARATED, + shape_proxy, empty_cache, empty_cast_output, + get_proxy_support, get_point_support, shape_distance, shape_cast, get_sweep_transform, + time_of_impact, simplex_vertex, cache_index_a, cache_index_b +) + +const NULL_INDEX = 0 - 1 +const MAX_SIMPLEX_VERTICES = 4 +const MAX_GJK_ITERATIONS = 32 +const MAX_SHAPE_CAST_POINTS = 128 + +const TOI_STATE_UNKNOWN = 0 +const TOI_STATE_FAILED = 1 +const TOI_STATE_OVERLAPPED = 2 +const TOI_STATE_HIT = 3 +const TOI_STATE_SEPARATED = 4 + +// The reference's single-precision epsilon and smallest normal, which +// its tolerances are in. +const FLOAT_EPSILON = 0.00000011920929 +const FLOAT_MIN = 0.000000000000000000000000000000000000011754944 + +// A convex shape for GJK: a point cloud with a radius around it. A +// sphere is one point with a radius, a capsule two, a box eight with none. +struct ShapeProxy { + points: ptr // Vec3[] + count: int + radius: float +} + +// The last simplex, to warm-start the next query on nearby transforms. +// Zero it for a first call. +struct SimplexCache { + metric: float // the length, area or volume of the simplex + count: int + index_a0: int + index_a1: int + index_a2: int + index_a3: int + index_b0: int + index_b1: int + index_b2: int + index_b3: int +} + +struct SimplexVertex { + w_a: Vec3 // the support point in proxy A + w_b: Vec3 // the support point in proxy B, in A's frame + w: Vec3 // w_b - w_a + a: float // the barycentric coordinate + index_a: int + index_b: int +} + +struct Simplex { + v0: SimplexVertex + v1: SimplexVertex + v2: SimplexVertex + v3: SimplexVertex + count: int +} + +struct DistanceInput { + proxy_a: ShapeProxy + proxy_b: ShapeProxy + transform: Transform // B in A's frame; the query runs in frame A + use_radii: bool +} + +struct DistanceOutput { + point_a: Vec3 // the closest point on A, in A's frame + point_b: Vec3 // the closest point on B, in A's frame + normal: Vec3 // A to B; meaningless when the distance is zero + distance: float // zero when overlapped + iterations: int + simplex_count: int // simplexes written to the debug trail +} + +struct ShapeCastPairInput { + proxy_a: ShapeProxy + proxy_b: ShapeProxy + transform: Transform // B in A's frame + translation_b: Vec3 // B's translation, in A's frame + max_fraction: float + can_encroach: bool // let shapes with a radius move closer when already touching +} + +// A ray or shape cast's result. +struct CastOutput { + normal: Vec3 + point: Vec3 + fraction: float + iterations: int + triangle_index: int + hit: bool +} + +// The motion of a body over a step: its centre of mass from c1 to c2, +// its rotation from q1 to q2, with the shapes at local_center from it. +struct Sweep { + local_center: Vec3 + c1: Vec3 + c2: Vec3 + q1: Quat + q2: Quat +} + +struct TOIInput { + proxy_a: ShapeProxy + proxy_b: ShapeProxy + sweep_a: Sweep + sweep_b: Sweep + max_fraction: float +} + +struct TOIOutput { + state: int + point: Vec3 + normal: Vec3 + fraction: float + distance: float + distance_iterations: int + push_back_iterations: int + root_iterations: int +} + +shape_proxy(points: ptr, count: int, radius: float) -> ShapeProxy { + return ShapeProxy { points: points, count: count, radius: radius } +} + +empty_cache() -> SimplexCache { + return SimplexCache { metric: 0.0, count: 0, index_a0: 0, index_a1: 0, index_a2: 0, index_a3: 0, + index_b0: 0, index_b1: 0, index_b2: 0, index_b3: 0 } +} + +empty_cast_output() -> CastOutput { + return CastOutput { normal: math.vec3_zero(), point: math.vec3_zero(), fraction: 0.0, iterations: 0, + triangle_index: NULL_INDEX, hit: false } +} + +cache_index_a(c: *SimplexCache, i: int) -> int { + if i == 0 { return c.index_a0 } + if i == 1 { return c.index_a1 } + if i == 2 { return c.index_a2 } + return c.index_a3 +} + +cache_index_b(c: *SimplexCache, i: int) -> int { + if i == 0 { return c.index_b0 } + if i == 1 { return c.index_b1 } + if i == 2 { return c.index_b2 } + return c.index_b3 +} + +simplex_vertex(s: Simplex, i: int) -> SimplexVertex { + if i == 0 { return s.v0 } + if i == 1 { return s.v1 } + if i == 2 { return s.v2 } + return s.v3 +} + +simplex_set(s: *Simplex, i: int, v: SimplexVertex) { + if i == 0 { s.v0 = v + } else if i == 1 { s.v1 = v + } else if i == 2 { s.v2 = v + } else { s.v3 = v } +} + +proxy_point(p: ShapeProxy, i: int) -> Vec3 { + points = p.points as Vec3[] + return points[i] +} + +// --- support ------------------------------------------------------------------ + +// The point of the proxy farthest along the axis. The first point is +// moved to the origin for precision: the points may be far from it. +get_proxy_support(p: ShapeProxy, axis: Vec3) -> int { + return get_point_support(p.points, p.count, axis) +} + +get_point_support(block: ptr, count: int, axis: Vec3) -> int { + points = block as Vec3[] + origin = points[0] + max_index = 0 + max_projection = 0.0 + i = 1 + while i < count { + projection = math.dot(axis, math.sub(points[i], origin)) + if projection > max_projection { + max_index = i + max_projection = projection + } + i = i + 1 + } + return max_index +} + +// --- barycentric coordinates ---------------------------------------------------- + +// The unnormalised barycentric coordinates of the origin on an edge, a +// triangle or a tetrahedron, with the divisor last. +struct Bary { + u: float + v: float + w: float + x: float + divisor: float +} + +bary_edge(a: Vec3, b: Vec3) -> Bary { + ab = math.sub(b, a) + return Bary { u: math.dot(b, ab), v: 0.0 - math.dot(a, ab), w: 0.0, x: 0.0, divisor: math.dot(ab, ab) } +} + +bary_tri(a: Vec3, b: Vec3, c: Vec3) -> Bary { + ab = math.sub(b, a) + ac = math.sub(c, a) + b_x_c = math.cross(b, c) + c_x_a = math.cross(c, a) + a_x_b = math.cross(a, b) + ab_x_ac = math.cross(ab, ac) + return Bary { u: math.dot(b_x_c, ab_x_ac), v: math.dot(c_x_a, ab_x_ac), w: math.dot(a_x_b, ab_x_ac), x: 0.0, + divisor: math.dot(ab_x_ac, ab_x_ac) } +} + +bary_tet(a: Vec3, b: Vec3, c: Vec3, d: Vec3) -> Bary { + ab = math.sub(b, a) + ac = math.sub(c, a) + ad = math.sub(d, a) + // The divisor is forced positive. + divisor = math.scalar_triple_product(ab, ac, ad) + sign = 1.0 + if divisor < 0.0 { sign = 0.0 - 1.0 } + return Bary { u: sign * math.scalar_triple_product(b, c, d), v: sign * math.scalar_triple_product(a, d, c), + w: sign * math.scalar_triple_product(a, b, d), x: sign * math.scalar_triple_product(a, c, b), + divisor: sign * divisor } +} + +// --- the simplex ---------------------------------------------------------------- + +// The length, area or volume of the simplex: what the cache compares. +get_metric(s: Simplex) -> float { + if s.count == 1 { return 0.0 } + if s.count == 2 { return math.distance(s.v0.w, s.v1.w) } + if s.count == 3 { return math.length(math.cross(math.sub(s.v1.w, s.v0.w), math.sub(s.v2.w, s.v0.w))) / 2.0 } + return math.scalar_triple_product(math.sub(s.v1.w, s.v0.w), math.sub(s.v2.w, s.v0.w), math.sub(s.v3.w, s.v0.w)) / 6.0 +} + +write_cache(cache: *SimplexCache, s: Simplex) { + cache.metric = get_metric(s) + cache.count = s.count + cache.index_a0 = s.v0.index_a + cache.index_b0 = s.v0.index_b + cache.index_a1 = s.v1.index_a + cache.index_b1 = s.v1.index_b + cache.index_a2 = s.v2.index_a + cache.index_b2 = s.v2.index_b + cache.index_a3 = s.v3.index_a + cache.index_b3 = s.v3.index_b +} + +with_a(vertex: SimplexVertex, a: float) -> SimplexVertex { + v = vertex + v.a = a + return v +} + +// The closest point of a segment to the origin, by Voronoi region: the +// simplex is reduced to the region's feature with its barycentrics. +// False where a divisor is not positive. +solve_simplex2(s: *Simplex) -> bool { + a = s.v0.w + b = s.v1.w + ab = math.sub(b, a) + divisor = math.dot(ab, ab) + u = math.dot(b, ab) + v = 0.0 - math.dot(a, ab) + if v <= 0.0 { + s.count = 1 + s.v0.a = 1.0 + return true + } + if u <= 0.0 { + s.count = 1 + s.v0 = with_a(s.v1, 1.0) + return true + } + if divisor <= 0.0 { return false } + denominator = 1.0 / divisor + s.v0.a = denominator * u + s.v1.a = denominator * v + return true +} + +solve_simplex3(s: *Simplex) -> bool { + v1 = s.v0 + v2 = s.v1 + v3 = s.v2 + w_ab = bary_edge(v1.w, v2.w) + w_bc = bary_edge(v2.w, v3.w) + w_ca = bary_edge(v3.w, v1.w) + + if w_ab.v <= 0.0 && w_ca.u <= 0.0 { + s.count = 1 + s.v0 = with_a(v1, 1.0) + return true + } + if w_bc.v <= 0.0 && w_ab.u <= 0.0 { + s.count = 1 + s.v0 = with_a(v2, 1.0) + return true + } + if w_ca.v <= 0.0 && w_bc.u <= 0.0 { + s.count = 1 + s.v0 = with_a(v3, 1.0) + return true + } + + w_abc = bary_tri(v1.w, v2.w, v3.w) + + if w_abc.w <= 0.0 && w_ab.u > 0.0 && w_ab.v > 0.0 { + s.count = 2 + if w_ab.divisor <= 0.0 { return false } + s.v0 = with_a(v1, w_ab.u / w_ab.divisor) + s.v1 = with_a(v2, w_ab.v / w_ab.divisor) + return true + } + if w_abc.u <= 0.0 && w_bc.u > 0.0 && w_bc.v > 0.0 { + s.count = 2 + if w_bc.divisor <= 0.0 { return false } + s.v0 = with_a(v2, w_bc.u / w_bc.divisor) + s.v1 = with_a(v3, w_bc.v / w_bc.divisor) + return true + } + if w_abc.v <= 0.0 && w_ca.u > 0.0 && w_ca.v > 0.0 { + s.count = 2 + if w_ca.divisor <= 0.0 { return false } + s.v0 = with_a(v3, w_ca.u / w_ca.divisor) + s.v1 = with_a(v1, w_ca.v / w_ca.divisor) + return true + } + + if w_abc.divisor <= 0.0 { return false } + s.v0.a = w_abc.u / w_abc.divisor + s.v1.a = w_abc.v / w_abc.divisor + s.v2.a = w_abc.w / w_abc.divisor + return true +} + +reduce_to_edge(s: *Simplex, p: SimplexVertex, q: SimplexVertex, bary: Bary) -> bool { + s.count = 2 + if bary.divisor <= 0.0 { return false } + s.v0 = with_a(p, bary.u / bary.divisor) + s.v1 = with_a(q, bary.v / bary.divisor) + return true +} + +reduce_to_face(s: *Simplex, p: SimplexVertex, q: SimplexVertex, r: SimplexVertex, bary: Bary) -> bool { + s.count = 3 + if bary.divisor <= 0.0 { return false } + s.v0 = with_a(p, bary.u / bary.divisor) + s.v1 = with_a(q, bary.v / bary.divisor) + s.v2 = with_a(r, bary.w / bary.divisor) + return true +} + +solve_simplex4(s: *Simplex) -> bool { + va = s.v0 + vb = s.v1 + vc = s.v2 + vd = s.v3 + w_ab = bary_edge(va.w, vb.w) + w_ac = bary_edge(va.w, vc.w) + w_ad = bary_edge(va.w, vd.w) + w_bc = bary_edge(vb.w, vc.w) + w_cd = bary_edge(vc.w, vd.w) + w_db = bary_edge(vd.w, vb.w) + + if w_ab.v <= 0.0 && w_ac.v <= 0.0 && w_ad.v <= 0.0 { + s.count = 1 + s.v0 = with_a(va, 1.0) + return true + } + if w_ab.u <= 0.0 && w_db.u <= 0.0 && w_bc.v <= 0.0 { + s.count = 1 + s.v0 = with_a(vb, 1.0) + return true + } + if w_ac.u <= 0.0 && w_bc.u <= 0.0 && w_cd.v <= 0.0 { + s.count = 1 + s.v0 = with_a(vc, 1.0) + return true + } + if w_ad.u <= 0.0 && w_cd.u <= 0.0 && w_db.v <= 0.0 { + s.count = 1 + s.v0 = with_a(vd, 1.0) + return true + } + + w_acb = bary_tri(va.w, vc.w, vb.w) + w_abd = bary_tri(va.w, vb.w, vd.w) + w_adc = bary_tri(va.w, vd.w, vc.w) + w_bcd = bary_tri(vb.w, vc.w, vd.w) + + if w_abd.w <= 0.0 && w_acb.v <= 0.0 && w_ab.u > 0.0 && w_ab.v > 0.0 { return reduce_to_edge(s, va, vb, w_ab) } + if w_acb.w <= 0.0 && w_adc.v <= 0.0 && w_ac.u > 0.0 && w_ac.v > 0.0 { return reduce_to_edge(s, va, vc, w_ac) } + if w_adc.w <= 0.0 && w_abd.v <= 0.0 && w_ad.u > 0.0 && w_ad.v > 0.0 { return reduce_to_edge(s, va, vd, w_ad) } + if w_acb.u <= 0.0 && w_bcd.w <= 0.0 && w_bc.u > 0.0 && w_bc.v > 0.0 { return reduce_to_edge(s, vb, vc, w_bc) } + if w_adc.u <= 0.0 && w_bcd.u <= 0.0 && w_cd.u > 0.0 && w_cd.v > 0.0 { return reduce_to_edge(s, vc, vd, w_cd) } + if w_abd.u <= 0.0 && w_bcd.v <= 0.0 && w_db.u > 0.0 && w_db.v > 0.0 { return reduce_to_edge(s, vd, vb, w_db) } + + w_abcd = bary_tet(va.w, vb.w, vc.w, vd.w) + + if w_abcd.x < 0.0 && w_acb.u > 0.0 && w_acb.v > 0.0 && w_acb.w > 0.0 { return reduce_to_face(s, va, vc, vb, w_acb) } + if w_abcd.w < 0.0 && w_abd.u > 0.0 && w_abd.v > 0.0 && w_abd.w > 0.0 { return reduce_to_face(s, va, vb, vd, w_abd) } + if w_abcd.v < 0.0 && w_adc.u > 0.0 && w_adc.v > 0.0 && w_adc.w > 0.0 { return reduce_to_face(s, va, vd, vc, w_adc) } + if w_abcd.u < 0.0 && w_bcd.u > 0.0 && w_bcd.v > 0.0 && w_bcd.w > 0.0 { return reduce_to_face(s, vb, vc, vd, w_bcd) } + + // Inside the tetrahedron. + if w_abcd.divisor <= 0.0 { return false } + s.v0.a = w_abcd.u / w_abcd.divisor + s.v1.a = w_abcd.v / w_abcd.divisor + s.v2.a = w_abcd.w / w_abcd.divisor + s.v3.a = w_abcd.x / w_abcd.divisor + return true +} + +blend3(a1: float, v1: Vec3, a2: float, v2: Vec3, a3: float, v3: Vec3) -> Vec3 { + return math.add(math.add(math.mul_sv(a1, v1), math.mul_sv(a2, v2)), math.mul_sv(a3, v3)) +} + +// The closest points on A and B from the solved simplex; identical for a +// tetrahedron (the shapes overlap). +witness_a(s: Simplex) -> Vec3 { + if s.count == 1 { return s.v0.w_a } + if s.count == 2 { return math.blend2(s.v0.a, s.v0.w_a, s.v1.a, s.v1.w_a) } + if s.count == 3 { return blend3(s.v0.a, s.v0.w_a, s.v1.a, s.v1.w_a, s.v2.a, s.v2.w_a) } + return math.add(math.blend2(s.v0.a, s.v0.w_a, s.v1.a, s.v1.w_a), math.blend2(s.v2.a, s.v2.w_a, s.v3.a, s.v3.w_a)) +} + +witness_b(s: Simplex) -> Vec3 { + if s.count == 1 { return s.v0.w_b } + if s.count == 2 { return math.blend2(s.v0.a, s.v0.w_b, s.v1.a, s.v1.w_b) } + if s.count == 3 { return blend3(s.v0.a, s.v0.w_b, s.v1.a, s.v1.w_b, s.v2.a, s.v2.w_b) } + return witness_a(s) +} + +closest_point(s: Simplex) -> Vec3 { + if s.count == 1 { return s.v0.w } + if s.count == 2 { return math.blend2(s.v0.a, s.v0.w, s.v1.a, s.v1.w) } + if s.count == 3 { return blend3(s.v0.a, s.v0.w, s.v1.a, s.v1.w, s.v2.a, s.v2.w) } + return math.add(math.blend2(s.v0.a, s.v0.w, s.v1.a, s.v1.w), math.blend2(s.v2.a, s.v2.w, s.v3.a, s.v3.w)) +} + +make_vertex(pa: Vec3, pb: Vec3, ia: int, ib: int) -> SimplexVertex { + return SimplexVertex { w_a: pa, w_b: pb, w: math.sub(pb, pa), a: 0.0, index_a: ia, index_b: ib } +} + +overlap_output(s: Simplex, iteration: int, simplex_index: int) -> DistanceOutput { + return DistanceOutput { point_a: witness_a(s), point_b: witness_b(s), normal: math.vec3_zero(), distance: 0.0, + iterations: iteration, simplex_count: simplex_index } +} + +// --- GJK ------------------------------------------------------------------------ + +// The distance between two proxies, in A's frame, with the closest +// points and the normal from A to B. The cache warm-starts the simplex +// and receives the result; simplexes (Simplex[] of simplex_capacity, or +// null) receives the trail for debugging. +shape_distance(input: *DistanceInput, cache: *SimplexCache, simplexes: ptr, simplex_capacity: int) -> DistanceOutput { + xf = input.transform + m = math.make_matrix_from_quat(xf.q) + mt = math.transpose(m) + proxy_a = input.proxy_a + proxy_b = input.proxy_b + trail = simplexes as Simplex[] + + // The initial simplex from the cache. + empty_vertex = SimplexVertex { w_a: math.vec3_zero(), w_b: math.vec3_zero(), w: math.vec3_zero(), a: 0.0, index_a: 0, index_b: 0 } + simplex = Simplex { v0: empty_vertex, v1: empty_vertex, v2: empty_vertex, v3: empty_vertex, count: cache.count } + i = 0 + while i < cache.count { + index1 = cache_index_a(cache, i) + index2 = cache_index_b(cache, i) + vertex1 = proxy_point(proxy_a, index1) + vertex2 = math.add(math.mul_mv(m, proxy_point(proxy_b, index2)), xf.p) + simplex_set(&simplex, i, make_vertex(vertex1, vertex2, index1, index2)) + i = i + 1 + } + + // If the new metric differs much from the cached one, flush. + if simplex.count > 0 { + metric1 = cache.metric + metric2 = get_metric(simplex) + if 2.0 * metric1 < metric2 || metric2 < 0.5 * metric1 || metric2 < FLOAT_EPSILON { simplex.count = 0 } + } + if simplex.count == 0 { + vertex1 = proxy_point(proxy_a, 0) + vertex2 = math.add(math.mul_mv(m, proxy_point(proxy_b, 0)), xf.p) + simplex.count = 1 + simplex.v0 = make_vertex(vertex1, vertex2, 0, 0) + } + + backup = simplex + backup.count = 0 + simplex_index = 0 + if simplexes != null && simplex_index < simplex_capacity { + trail[simplex_index] = simplex + simplex_index = simplex_index + 1 + } + + output = DistanceOutput { point_a: math.vec3_zero(), point_b: math.vec3_zero(), normal: math.vec3_zero(), + distance: 0.0, iterations: 0, simplex_count: 0 } + distance_sq = math.MAX_FLOAT + normal = math.vec3_zero() + + iteration = 0 + while iteration < MAX_GJK_ITERATIONS { + solved = false + if simplex.count == 1 { + simplex.v0.a = 1.0 + solved = true + } else if simplex.count == 2 { + solved = solve_simplex2(&simplex) + } else if simplex.count == 3 { + solved = solve_simplex3(&simplex) + } else { + solved = solve_simplex4(&simplex) + } + + if solved == false { + // No progress: the last simplex stands. + simplex = backup + break + } + + if simplexes != null && simplex_index < simplex_capacity { + trail[simplex_index] = simplex + simplex_index = simplex_index + 1 + output.iterations = iteration + output.simplex_count = simplex_index + } + + if simplex.count == MAX_SIMPLEX_VERTICES { + // The origin is inside the tetrahedron: overlap. + return overlap_output(simplex, iteration, simplex_index) + } + + old_distance_sq = distance_sq + closest = closest_point(simplex) + distance_sq = math.dot(closest, closest) + if distance_sq >= old_distance_sq { + simplex = backup + break + } + + // The next search direction, towards the origin. + search = math.vec3_zero() + if simplex.count == 1 { + search = math.neg(simplex.v0.w) + } else if simplex.count == 2 { + a = simplex.v0.w + ab = math.sub(simplex.v1.w, a) + search = math.cross(math.cross(ab, math.neg(a)), ab) + } else { + a = simplex.v0.w + n = math.cross(math.sub(simplex.v1.w, a), math.sub(simplex.v2.w, a)) + search = n + if math.dot(n, a) >= 0.0 { search = math.neg(n) } + } + + if math.length_squared(search) < 1000.0 * FLOAT_MIN { + // The origin is on a segment or triangle: overlap. + return overlap_output(simplex, iteration, simplex_index) + } + + normal = math.neg(search) + + index_a = get_proxy_support(proxy_a, math.neg(search)) + support_a = proxy_point(proxy_a, index_a) + index_b = get_proxy_support(proxy_b, math.mul_mv(mt, search)) + support_b = math.add(math.mul_mv(m, proxy_point(proxy_b, index_b)), xf.p) + + // Save the simplex before adding; a repeated support ends the search. + backup = simplex + duplicate = false + i = 0 + while i < simplex.count { + v = simplex_vertex(simplex, i) + if v.index_a == index_a && v.index_b == index_b { + duplicate = true + break + } + i = i + 1 + } + if duplicate { break } + + simplex_set(&simplex, simplex.count, make_vertex(support_a, support_b, index_a, index_b)) + simplex.count = simplex.count + 1 + iteration = iteration + 1 + } + + output.point_a = witness_a(simplex) + output.point_b = witness_b(simplex) + output.iterations = iteration + output.simplex_count = simplex_index + + normal = math.normalize(normal) + if math.is_normalized(normal) == false { + output.distance = 0.0 + output.normal = math.vec3_zero() + return output + } + + output.distance = math.distance(output.point_a, output.point_b) + output.normal = normal + + if input.use_radii { + r_a = proxy_a.radius + r_b = proxy_b.radius + output.distance = math.max_float(0.0, output.distance - r_a - r_b) + // The points stay on the perimeters even when overlapped, so they move smoothly. + output.point_a = math.add(output.point_a, math.mul_sv(r_a, normal)) + output.point_b = math.sub(output.point_b, math.mul_sv(r_b, normal)) + } + + write_cache(cache, simplex) + return output +} + +// --- shape cast ----------------------------------------------------------------- + +// B swept along its translation in A's frame by conservative advancement +// on the distance: each step moves B up to the point where the current +// distance would close along the current normal, until it is within a +// slop of the target. The hit point is on A's surface. +shape_cast(input: *ShapeCastPairInput) -> CastOutput { + linear_slop = math.LINEAR_SLOP + total_radius = input.proxy_a.radius + input.proxy_b.radius + target = math.max_float(linear_slop, total_radius - linear_slop) + tolerance = 0.25 * linear_slop + + cache = empty_cache() + alpha = 0.0 + distance_input = DistanceInput { proxy_a: input.proxy_a, proxy_b: input.proxy_b, transform: input.transform, use_radii: false } + delta2 = input.translation_b + output = empty_cast_output() + + iteration = 0 + while iteration < 20 { + output.iterations = output.iterations + 1 + d = shape_distance(&distance_input, &cache, null, 0) + + if d.distance < target + tolerance { + if iteration == 0 { + if input.can_encroach && d.distance > 2.0 * linear_slop { + target = d.distance - linear_slop + } else { + // Initial overlap: a common point. + output.hit = true + c1 = math.mul_add(d.point_a, input.proxy_a.radius, d.normal) + c2 = math.mul_add(d.point_b, 0.0 - input.proxy_b.radius, d.normal) + output.point = math.lerp(c1, c2, 0.5) + return output + } + } else { + if d.distance > 0.0 && math.is_normalized(d.normal) == false { + // A numerical problem, likely extreme input. + return output + } + output.fraction = alpha + output.point = math.mul_add(d.point_a, input.proxy_a.radius, d.normal) + output.normal = d.normal + output.hit = true + return output + } + } + + // Are the shapes approaching? + denominator = math.dot(delta2, d.normal) + if denominator >= 0.0 { return output } + + alpha = alpha + (target - d.distance) / denominator + if alpha >= input.max_fraction { return output } + + distance_input.transform.p = math.mul_add(input.transform.p, alpha, delta2) + iteration = iteration + 1 + } + return output +} + +// --- time of impact -------------------------------------------------------------- + +get_sweep_transform(sweep: Sweep, time: float) -> Transform { + q = math.nlerp(sweep.q1, sweep.q2, time) + p = math.sub(math.lerp(sweep.c1, sweep.c2, time), math.rotate_vector(q, sweep.local_center)) + return Transform { p: p, q: q } +} + +get_final_sweep_transform(sweep: Sweep) -> Transform { + return Transform { p: math.sub(sweep.c2, math.rotate_vector(sweep.q2, sweep.local_center)), q: sweep.q2 } +} + +unique_count(count: int, i0: int, i1: int, i2: int) -> int { + if count == 1 { return 1 } + if count == 2 { + if i0 != i1 { return 2 } + return 1 + } + if i0 != i1 && i0 != i2 && i1 != i2 { return 3 } + if i0 == i1 && i0 == i2 { return 1 } + return 2 +} + +// Whether the cross product of two edges flips over the sweep. +check_fast_edges(xf_a: Transform, local_edge_a: Vec3, xf_b: Transform, local_edge_b: Vec3, axis0: Vec3) -> bool { + edge_a = math.rotate_vector(xf_a.q, local_edge_a) + edge_b = math.rotate_vector(xf_b.q, local_edge_b) + return math.dot(math.cross(edge_a, edge_b), axis0) < 0.0 +} + +const SEPARATION_UNKNOWN = 0 +const SEPARATION_VERTICES = 1 +const SEPARATION_EDGES = 2 +const SEPARATION_FACE_A = 3 +const SEPARATION_FACE_B = 4 + +// The separation along an axis from the cached simplex: a world axis +// (vertices), the cross product of two local edges, or a local face of +// A or B. +struct SeparationFunction { + proxy_a: ShapeProxy + proxy_b: ShapeProxy + sweep_a: Sweep + sweep_b: Sweep + witness1: Vec3 + witness2: Vec3 + kind: int +} + +make_separation_function(cache: SimplexCache, proxy_a: ShapeProxy, sweep_a: Sweep, proxy_b: ShapeProxy, sweep_b: Sweep, + world_normal: Vec3, t1: float) -> SeparationFunction { + fcn = SeparationFunction { proxy_a: proxy_a, proxy_b: proxy_b, sweep_a: sweep_a, sweep_b: sweep_b, + witness1: math.vec3_zero(), witness2: math.vec3_zero(), kind: SEPARATION_UNKNOWN } + ia0 = cache.index_a0 + ia1 = cache.index_a1 + ia2 = cache.index_a2 + ib0 = cache.index_b0 + ib1 = cache.index_b1 + ib2 = cache.index_b2 + unique_a = unique_count(cache.count, ia0, ia1, ia2) + unique_b = unique_count(cache.count, ib0, ib1, ib2) + + xf_a1 = get_sweep_transform(sweep_a, t1) + xf_b1 = get_sweep_transform(sweep_b, t1) + q_a = xf_a1.q + q_b = xf_b1.q + delta_p = math.sub(xf_b1.p, xf_a1.p) + + if cache.count == 1 { + fcn.kind = SEPARATION_VERTICES + fcn.witness1 = world_normal + return fcn + } + + if cache.count == 2 { + if unique_a == 2 && unique_b == 2 { + // Edge against edge. + v_a1 = proxy_point(proxy_a, ia0) + local_edge_a = math.normalize(math.sub(proxy_point(proxy_a, ia1), v_a1)) + v_b1 = proxy_point(proxy_b, ib0) + local_edge_b = math.normalize(math.sub(proxy_point(proxy_b, ib1), v_b1)) + return edge_separation(fcn, q_a, q_b, delta_p, v_a1, local_edge_a, v_b1, local_edge_b, world_normal, 0.05 * 0.05) + } + // A vertex against an edge: the world axis. + fcn.kind = SEPARATION_VERTICES + fcn.witness1 = world_normal + return fcn + } + + // Three: a face of A, a face of B, or two edges. + if unique_a == 3 { + v_a1 = proxy_point(proxy_a, ia0) + v_a2 = proxy_point(proxy_a, ia1) + v_a3 = proxy_point(proxy_a, ia2) + local_axis_a = math.normalize(math.cross(math.sub(v_a2, v_a1), math.sub(v_a3, v_a1))) + axis_a = math.rotate_vector(q_a, local_axis_a) + local_point_a = math.mul_sv(1.0 / 3.0, math.add(math.add(v_a1, v_a2), v_a3)) + local_point_b = proxy_point(proxy_b, ib0) + delta = math.add(math.sub(math.rotate_vector(q_b, local_point_b), math.rotate_vector(q_a, local_point_a)), delta_p) + if math.dot(delta, axis_a) < 0.0 { local_axis_a = math.neg(local_axis_a) } + fcn.kind = SEPARATION_FACE_A + fcn.witness1 = local_axis_a + fcn.witness2 = local_point_a + return fcn + } + if unique_b == 3 { + v_b1 = proxy_point(proxy_b, ib0) + v_b2 = proxy_point(proxy_b, ib1) + v_b3 = proxy_point(proxy_b, ib2) + local_axis_b = math.normalize(math.cross(math.sub(v_b2, v_b1), math.sub(v_b3, v_b1))) + axis_b = math.rotate_vector(q_b, local_axis_b) + local_point_a = proxy_point(proxy_a, ia0) + local_point_b = math.mul_sv(1.0 / 3.0, math.add(math.add(v_b1, v_b2), v_b3)) + delta = math.sub(math.sub(math.rotate_vector(q_a, local_point_a), math.rotate_vector(q_b, local_point_b)), delta_p) + if math.dot(delta, axis_b) < 0.0 { local_axis_b = math.neg(local_axis_b) } + fcn.kind = SEPARATION_FACE_B + fcn.witness1 = local_axis_b + fcn.witness2 = local_point_b + return fcn + } + + // Two unique on each: make the first two indices unique. + if ia0 == ia1 { ia1 = ia2 } + if ib0 == ib1 { ib1 = ib2 } + v_a1 = proxy_point(proxy_a, ia0) + local_edge_a = math.normalize(math.sub(proxy_point(proxy_a, ia1), v_a1)) + v_b1 = proxy_point(proxy_b, ib0) + local_edge_b = math.normalize(math.sub(proxy_point(proxy_b, ib1), v_b1)) + return edge_separation(fcn, q_a, q_b, delta_p, v_a1, local_edge_a, v_b1, local_edge_b, world_normal, 0.005 * 0.005) +} + +// The edge-edge case: the cross product axis when the edges are not near +// parallel and it does not flip over the sweep; a fixed axis otherwise. +edge_separation(base: SeparationFunction, q_a: Quat, q_b: Quat, delta_p: Vec3, v_a1: Vec3, local_edge_a: Vec3, + v_b1: Vec3, local_edge_b_in: Vec3, world_normal: Vec3, tolerance_squared: float) -> SeparationFunction { + fcn = base + local_edge_b = local_edge_b_in + edge_a = math.rotate_vector(q_a, local_edge_a) + edge_b = math.rotate_vector(q_b, local_edge_b) + axis = math.cross(edge_a, edge_b) + if math.length_squared(axis) < tolerance_squared { + fcn.kind = SEPARATION_VERTICES + fcn.witness1 = world_normal + return fcn + } + delta = math.add(math.sub(math.rotate_vector(q_b, v_b1), math.rotate_vector(q_a, v_a1)), delta_p) + if math.dot(delta, axis) < 0.0 { + // The axis points from A to B. + axis = math.neg(axis) + local_edge_b = math.neg(local_edge_b) + } + xf_a2 = get_final_sweep_transform(fcn.sweep_a) + xf_b2 = get_final_sweep_transform(fcn.sweep_b) + if check_fast_edges(xf_a2, local_edge_a, xf_b2, local_edge_b, axis) { + fcn.kind = SEPARATION_VERTICES + fcn.witness1 = math.normalize(axis) + return fcn + } + fcn.kind = SEPARATION_EDGES + fcn.witness1 = local_edge_a + fcn.witness2 = local_edge_b + return fcn +} + +// The deepest points at time t along the function's axis and their separation. +struct MinSeparation { + separation: float + index_a: int + index_b: int +} + +find_min_separation(fcn: *SeparationFunction, t: float) -> MinSeparation { + xf_a = get_sweep_transform(fcn.sweep_a, t) + xf_b = get_sweep_transform(fcn.sweep_b, t) + if fcn.kind == SEPARATION_VERTICES { + axis = fcn.witness1 + ia = get_point_support(fcn.proxy_a.points, fcn.proxy_a.count, math.inv_rotate_vector(xf_a.q, axis)) + ib = get_point_support(fcn.proxy_b.points, fcn.proxy_b.count, math.inv_rotate_vector(xf_b.q, math.neg(axis))) + delta = math.add(math.sub(math.rotate_vector(xf_b.q, proxy_point(fcn.proxy_b, ib)), + math.rotate_vector(xf_a.q, proxy_point(fcn.proxy_a, ia))), + math.sub(xf_b.p, xf_a.p)) + return MinSeparation { separation: math.dot(delta, axis), index_a: ia, index_b: ib } + } + if fcn.kind == SEPARATION_EDGES { + edge_a = math.rotate_vector(xf_a.q, fcn.witness1) + edge_b = math.rotate_vector(xf_b.q, fcn.witness2) + axis = math.normalize(math.cross(edge_a, edge_b)) + ia = get_point_support(fcn.proxy_a.points, fcn.proxy_a.count, math.inv_rotate_vector(xf_a.q, axis)) + ib = get_point_support(fcn.proxy_b.points, fcn.proxy_b.count, math.neg(math.inv_rotate_vector(xf_b.q, axis))) + delta = math.add(math.sub(math.rotate_vector(xf_b.q, proxy_point(fcn.proxy_b, ib)), + math.rotate_vector(xf_a.q, proxy_point(fcn.proxy_a, ia))), + math.sub(xf_b.p, xf_a.p)) + return MinSeparation { separation: math.dot(delta, axis), index_a: ia, index_b: ib } + } + if fcn.kind == SEPARATION_FACE_A { + normal = math.rotate_vector(xf_a.q, fcn.witness1) + point_a = math.transform_point(xf_a, fcn.witness2) + ib = get_point_support(fcn.proxy_b.points, fcn.proxy_b.count, math.neg(math.inv_rotate_vector(xf_b.q, normal))) + point_b = math.transform_point(xf_b, proxy_point(fcn.proxy_b, ib)) + return MinSeparation { separation: math.dot(math.sub(point_b, point_a), normal), index_a: NULL_INDEX, index_b: ib } + } + normal = math.rotate_vector(xf_b.q, fcn.witness1) + ia = get_point_support(fcn.proxy_a.points, fcn.proxy_a.count, math.neg(math.inv_rotate_vector(xf_a.q, normal))) + point_a = math.transform_point(xf_a, proxy_point(fcn.proxy_a, ia)) + point_b = math.transform_point(xf_b, fcn.witness2) + return MinSeparation { separation: math.dot(math.sub(point_a, point_b), normal), index_a: ia, index_b: NULL_INDEX } +} + +// The separation of two given points at time beta. +evaluate_separation(fcn: *SeparationFunction, index1: int, index2: int, beta: float) -> float { + xf1 = get_sweep_transform(fcn.sweep_a, beta) + xf2 = get_sweep_transform(fcn.sweep_b, beta) + if fcn.kind == SEPARATION_VERTICES { + point1 = math.transform_point(xf1, proxy_point(fcn.proxy_a, index1)) + point2 = math.transform_point(xf2, proxy_point(fcn.proxy_b, index2)) + return math.dot(math.sub(point2, point1), fcn.witness1) + } + if fcn.kind == SEPARATION_EDGES { + axis = math.normalize(math.cross(math.rotate_vector(xf1.q, fcn.witness1), math.rotate_vector(xf2.q, fcn.witness2))) + point1 = math.transform_point(xf1, proxy_point(fcn.proxy_a, index1)) + point2 = math.transform_point(xf2, proxy_point(fcn.proxy_b, index2)) + return math.dot(math.sub(point2, point1), axis) + } + if fcn.kind == SEPARATION_FACE_A { + axis = math.rotate_vector(xf1.q, fcn.witness1) + point1 = math.transform_point(xf1, fcn.witness2) + point2 = math.transform_point(xf2, proxy_point(fcn.proxy_b, index2)) + return math.dot(math.sub(point2, point1), axis) + } + axis = math.rotate_vector(xf2.q, fcn.witness1) + point1 = math.transform_point(xf1, proxy_point(fcn.proxy_a, index1)) + point2 = math.transform_point(xf2, fcn.witness2) + return math.dot(math.sub(point1, point2), axis) +} + +// Freeze an edge-edge axis at time beta into a fixed world axis. +force_fixed_axis(fcn: *SeparationFunction, beta: float) { + xf1 = get_sweep_transform(fcn.sweep_a, beta) + xf2 = get_sweep_transform(fcn.sweep_b, beta) + axis = math.normalize(math.cross(math.rotate_vector(xf1.q, fcn.witness1), math.rotate_vector(xf2.q, fcn.witness2))) + fcn.kind = SEPARATION_VERTICES + fcn.witness1 = axis + fcn.witness2 = math.vec3_zero() +} + +toi_point(output: *TOIOutput, world_point_a: Vec3, world_point_b: Vec3, world_normal: Vec3, + radius_a: float, radius_b: float, origin: Vec3) { + p_a = math.mul_add(world_point_a, radius_a, world_normal) + p_b = math.mul_add(world_point_b, 0.0 - radius_b, world_normal) + output.point = math.add(math.lerp(p_a, p_b, 0.5), origin) + output.normal = world_normal +} + +// The time in [0, max_fraction] at which the two sweeps first come +// within a slop of touching, by separating axes taken from the distance +// query's simplex and root-found on, each axis resolving the deepest +// points until they are separated at that time. +time_of_impact(input: *TOIInput) -> TOIOutput { + output = TOIOutput { state: TOI_STATE_UNKNOWN, point: math.vec3_zero(), normal: math.vec3_zero(), fraction: 0.0 - 1.0, + distance: 0.0, distance_iterations: 0, push_back_iterations: 0, root_iterations: 0 } + sweep_a = input.sweep_a + sweep_b = input.sweep_b + + // Shift to the origin. + origin = sweep_a.c1 + sweep_a.c1 = math.vec3_zero() + sweep_a.c2 = math.sub(sweep_a.c2, origin) + sweep_b.c1 = math.sub(sweep_b.c1, origin) + sweep_b.c2 = math.sub(sweep_b.c2, origin) + + proxy_a = input.proxy_a + proxy_b = input.proxy_b + max_push_back_iterations = proxy_a.count + proxy_b.count + t_max = input.max_fraction + + linear_slop = math.LINEAR_SLOP + total_radius = proxy_a.radius + proxy_b.radius + target = math.max_float(linear_slop, total_radius - linear_slop) + tolerance = 0.25 * linear_slop + + t1 = 0.0 + max_iterations = 25 + distance_iterations = 0 + cache = empty_cache() + distance_input = DistanceInput { proxy_a: proxy_a, proxy_b: proxy_b, transform: math.transform_identity(), use_radii: false } + + // The outer loop finds new separating axes until one repeats. + while true { + xf_a = get_sweep_transform(sweep_a, t1) + xf_b = get_sweep_transform(sweep_b, t1) + distance_input.transform = math.inv_mul_transforms(xf_a, xf_b) + d = shape_distance(&distance_input, &cache, null, 0) + output.distance = d.distance + + // The query ran in frame A; back to the shifted world. + world_normal = math.rotate_vector(xf_a.q, d.normal) + world_point_a = math.transform_point(xf_a, d.point_a) + world_point_b = math.transform_point(xf_a, d.point_b) + + output.distance_iterations = output.distance_iterations + 1 + distance_iterations = distance_iterations + 1 + + if d.distance <= 0.0 { + // Overlapped: continuous collision gives up. + output.state = TOI_STATE_OVERLAPPED + output.fraction = 0.0 + toi_point(&output, world_point_a, world_point_b, world_normal, proxy_a.radius, proxy_b.radius, origin) + break + } + if d.distance <= target + tolerance { + output.state = TOI_STATE_HIT + output.fraction = t1 + toi_point(&output, world_point_a, world_point_b, world_normal, proxy_a.radius, proxy_b.radius, origin) + break + } + if distance_iterations == max_iterations { + // Too slow: a capsule turning about a triangle vertex, say. + output.state = TOI_STATE_FAILED + output.fraction = t1 + toi_point(&output, world_point_a, world_point_b, world_normal, proxy_a.radius, proxy_b.radius, origin) + break + } + + fcn = make_separation_function(cache, proxy_a, sweep_a, proxy_b, sweep_b, world_normal, t1) + + // Resolve the deepest points on this axis, one after another. + done = false + t2 = t_max + push_back_iterations = 0 + while true { + deepest = find_min_separation(&fcn, t2) + s2 = deepest.separation + index_a = deepest.index_a + index_b = deepest.index_b + + if s2 - target > tolerance { + // Separated at the end. + output.state = TOI_STATE_SEPARATED + output.fraction = input.max_fraction + done = true + break + } + if s2 >= target - tolerance { + // Within tolerance at t2: advance. + t1 = t2 + break + } + + s1 = evaluate_separation(&fcn, index_a, index_b, t1) + if s1 < target - tolerance { + // Overlap: the root finder ran out. + output.state = TOI_STATE_FAILED + output.fraction = t1 + done = true + break + } + if s1 <= target + tolerance { + // t1 holds the time of impact (it may be zero). + output.state = TOI_STATE_HIT + output.fraction = t1 + done = true + break + } + + // The 1D root of s(t) - target, by false position and bisection in turn. + root_iteration_count = 0 + max_root_iterations = 50 + a1 = t1 + a2 = t2 + while true { + t = 0.5 * (a1 + a2) + if (root_iteration_count & 1) != 0 { t = a1 + (target - s1) * (a2 - a1) / (s2 - s1) } + output.root_iterations = output.root_iterations + 1 + root_iteration_count = root_iteration_count + 1 + s = evaluate_separation(&fcn, index_a, index_b, t) + if math.abs_float(s - target) <= tolerance { + t2 = t + break + } + if s > target { + a1 = t + s1 = s + } else { + a2 = t + s2 = s + } + if root_iteration_count == max_root_iterations { break } + } + + // An edge axis that would not converge becomes a fixed one. + if root_iteration_count == max_root_iterations - 1 && fcn.kind == SEPARATION_EDGES { + root_iteration_count = 0 + t2 = input.max_fraction + force_fixed_axis(&fcn, t1) + } + + output.push_back_iterations = output.push_back_iterations + 1 + push_back_iterations = push_back_iterations + 1 + if push_back_iterations == max_push_back_iterations { break } + } + + if done { + toi_point(&output, world_point_a, world_point_b, world_normal, input.proxy_a.radius, input.proxy_b.radius, origin) + break + } + } + return output +} diff --git a/aephysics/hull/module.ae b/aephysics/hull/module.ae index 72668f7..e447f6d 100644 --- a/aephysics/hull/module.ae +++ b/aephysics/hull/module.ae @@ -22,9 +22,10 @@ import std.string import aephysics.math import aephysics.core +import aephysics.distance exports ( - HullData, HalfEdge, MassData, CastOutput, ShapeExtent, Point2D, + HullData, HalfEdge, MassData, ShapeExtent, Point2D, MAX_HULL_VERTICES, MAX_HULL_FACES, MAX_HULL_EDGES, HULL_VERSION, NULL_INDEX, hull_vertices, hull_points, hull_edges, hull_planes, hull_faces, create_hull, destroy_hull, clone_hull, clone_and_transform_hull, is_valid_hull, @@ -34,6 +35,7 @@ exports ( make_scaled_box_hull, scale_box, find_hull_support_vertex, find_hull_support_face, compute_hull_mass, compute_hull_aabb, compute_swept_hull_aabb, ray_cast_hull, + overlap_hull, shape_cast_hull, hull_proxy, compute_hull_extent, compute_hull_projected_area, hull_2d, simplify_hull_2d ) @@ -94,16 +96,6 @@ struct MassData { inertia: Matrix3 } -// A ray or shape cast's result. -struct CastOutput { - normal: Vec3 - point: Vec3 - fraction: float - iterations: int - triangle_index: int - hit: bool -} - // The smallest sphere at the origin inside the shape and the largest box around it. struct ShapeExtent { min_extent: float @@ -1885,7 +1877,7 @@ compute_swept_hull_aabb(h: *HullData, xf1: Transform, xf2: Transform) -> AABB { // the hull's planes: the entering face is the hit; a ray starting inside // hits at its origin with no normal. ray_cast_hull(h: *HullData, origin: Vec3, translation: Vec3, max_fraction: float) -> CastOutput { - output = CastOutput { normal: math.vec3_zero(), point: math.vec3_zero(), fraction: 0.0, iterations: 0, triangle_index: NULL_INDEX, hit: false } + output = distance.empty_cast_output() lower = 0.0 upper = max_fraction best_face = NULL_INDEX @@ -1922,6 +1914,28 @@ ray_cast_hull(h: *HullData, origin: Vec3, translation: Vec3, max_fraction: float return output } +// The hull's points as a GJK proxy, with no radius. +hull_proxy(h: *HullData) -> ShapeProxy { + return distance.shape_proxy((h as ptr) + h.point_offset, h.vertex_count, 0.0) +} + +// Whether the hull, under its transform, comes within the overlap slop +// of a proxy given in world space. +overlap_hull(h: *HullData, shape_transform: Transform, proxy: ShapeProxy) -> bool { + input = DistanceInput { proxy_a: hull_proxy(h), proxy_b: proxy, + transform: math.inv_mul_transforms(shape_transform, math.transform_identity()), use_radii: true } + cache = distance.empty_cache() + output = distance.shape_distance(&input, &cache, null, 0) + return output.distance < 0.1 * math.LINEAR_SLOP +} + +// A proxy swept against the hull, in the hull's frame. +shape_cast_hull(h: *HullData, proxy: ShapeProxy, translation: Vec3, max_fraction: float, can_encroach: bool) -> CastOutput { + input = ShapeCastPairInput { proxy_a: hull_proxy(h), proxy_b: proxy, transform: math.transform_identity(), + translation_b: translation, max_fraction: max_fraction, can_encroach: can_encroach } + return distance.shape_cast(&input) +} + compute_hull_extent(h: *HullData, origin: Vec3) -> ShapeExtent { points = hull_points(h) extent = ShapeExtent { min_extent: h.inner_radius, max_extent: math.vec3_zero() } diff --git a/aephysics/test_distance.ae b/aephysics/test_distance.ae new file mode 100644 index 0000000..0e74985 --- /dev/null +++ b/aephysics/test_distance.ae @@ -0,0 +1,311 @@ +// aephysics.distance against the reference's (Box3D's) test_distance.c +// (the segment distance, the square against a segment, the shape cast, +// the time of impact), and beyond it: sphere and capsule proxies against +// boxes at analytic distances, overlap, the warm-started cache agreeing +// with a cold one over a sweep of poses, witness points inside their +// shapes and as far apart as the distance, rotated sweeps in the time of +// impact, and the hull's overlap and shape cast through it. + +import std.string +import aephysics.math +import aephysics.core +import aephysics.distance +import aephysics.hull + +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("distance: FAIL ${name}") + failures = failures + 1 + } +} + +small(name: string, value: float, tolerance: float) { + ensure("${name} (${value})", math.abs_float(value) < tolerance) +} + +const FLT_EPSILON = 0.00000011920929 + +// A box of half widths as a point cloud. +box_points(hx: float, hy: float, hz: float) -> ptr { + block = calloc(8, sizeof(Vec3)) + p = block as Vec3[] + i = 0 + while i < 8 { + x = hx + y = hy + z = hz + if (i & 1) != 0 { x = 0.0 - hx } + if (i & 2) != 0 { y = 0.0 - hy } + if (i & 4) != 0 { z = 0.0 - hz } + p[i] = math.vec3(x, y, z) + i = i + 1 + } + return block +} + +points4(x0: float, y0: float, x1: float, y1: float, x2: float, y2: float, x3: float, y3: float) -> ptr { + block = calloc(4, sizeof(Vec3)) + p = block as Vec3[] + p[0] = math.vec3(x0, y0, 0.0) + p[1] = math.vec3(x1, y1, 0.0) + p[2] = math.vec3(x2, y2, 0.0) + p[3] = math.vec3(x3, y3, 0.0) + return block +} + +points2(x0: float, y0: float, x1: float, y1: float) -> ptr { + block = calloc(2, sizeof(Vec3)) + p = block as Vec3[] + p[0] = math.vec3(x0, y0, 0.0) + p[1] = math.vec3(x1, y1, 0.0) + return block +} + +exact_quat(axis: Vec3, radians: float) -> Quat { + half = 0.5 * radians + return Quat { v: math.mul_sv(sin(half), axis), s: cos(half) } +} + +// The reference's four. +test_reference() { + r = math.segment_distance(math.vec3(0.0 - 1.0, 0.0 - 1.0, 0.0), math.vec3(0.0 - 1.0, 1.0, 0.0), + math.vec3(2.0, 0.0, 0.0), math.vec3(1.0, 0.0, 0.0)) + small("segment: fraction1", r.fraction1 - 0.5, FLT_EPSILON) + small("segment: fraction2", r.fraction2 - 1.0, FLT_EPSILON) + small("segment: point1 x", r.point1.x + 1.0, FLT_EPSILON) + small("segment: point1 y", r.point1.y, FLT_EPSILON) + small("segment: point2 x", r.point2.x - 1.0, FLT_EPSILON) + small("segment: point2 y", r.point2.y, FLT_EPSILON) + + vas = points4(0.0 - 1.0, 0.0 - 1.0, 1.0, 0.0 - 1.0, 1.0, 1.0, 0.0 - 1.0, 1.0) + vbs = points2(2.0, 0.0 - 1.0, 2.0, 1.0) + input = DistanceInput { proxy_a: distance.shape_proxy(vas, 4, 0.0), proxy_b: distance.shape_proxy(vbs, 2, 0.0), + transform: math.transform_identity(), use_radii: false } + cache = distance.empty_cache() + output = distance.shape_distance(&input, &cache, null, 0) + small("shape distance: the square to the segment", output.distance - 1.0, FLT_EPSILON) + ensure("shape distance: the cache was written", cache.count >= 1) + + cast = ShapeCastPairInput { proxy_a: distance.shape_proxy(vas, 4, 0.0), proxy_b: distance.shape_proxy(vbs, 2, 0.0), + transform: math.transform_identity(), translation_b: math.vec3(0.0 - 2.0, 0.0, 0.0), + max_fraction: 1.0, can_encroach: false } + c = distance.shape_cast(&cast) + ensure("shape cast: hit", c.hit) + small("shape cast: fraction", c.fraction - 0.5, 0.005) + small("shape cast: normal x", c.normal.x - 1.0, 0.001) + + toi = TOIInput { proxy_a: distance.shape_proxy(vas, 4, 0.0), proxy_b: distance.shape_proxy(vbs, 2, 0.0), + sweep_a: Sweep { local_center: math.vec3_zero(), c1: math.vec3_zero(), c2: math.vec3_zero(), q1: math.quat_identity(), q2: math.quat_identity() }, + sweep_b: Sweep { local_center: math.vec3_zero(), c1: math.vec3_zero(), c2: math.vec3(0.0 - 2.0, 0.0, 0.0), q1: math.quat_identity(), q2: math.quat_identity() }, + max_fraction: 1.0 } + t = distance.time_of_impact(&toi) + ensure("toi: hit", t.state == distance.TOI_STATE_HIT) + small("toi: fraction", t.fraction - 0.5, 0.005) + free(vas) + free(vbs) +} + +// Spheres and capsules against boxes, with the radii applied. +test_rounded() { + box = box_points(1.0, 1.0, 1.0) + point = calloc(1, sizeof(Vec3)) + pp = point as Vec3[] + pp[0] = math.vec3_zero() + // A sphere of radius 0.5 centred 3 m along x from a unit box: 1.5 m apart. + input = DistanceInput { proxy_a: distance.shape_proxy(box, 8, 0.0), proxy_b: distance.shape_proxy(point, 1, 0.5), + transform: Transform { p: math.vec3(3.0, 0.0, 0.0), q: math.quat_identity() }, use_radii: true } + cache = distance.empty_cache() + o = distance.shape_distance(&input, &cache, null, 0) + small("sphere-box: distance", o.distance - 1.5, 0.00001) + small("sphere-box: normal x", o.normal.x - 1.0, 0.00001) + small("sphere-box: point on the box face", o.point_a.x - 1.0, 0.00001) + small("sphere-box: point on the sphere", o.point_b.x - 2.5, 0.00001) + + // Along a diagonal the closest feature is a corner. + input.transform.p = math.vec3(3.0, 3.0, 3.0) + cache = distance.empty_cache() + o = distance.shape_distance(&input, &cache, null, 0) + small("sphere-box: corner distance", o.distance - (sqrt(12.0) - 0.5), 0.00001) + small("sphere-box: corner witness", o.point_a.x + o.point_a.y + o.point_a.z - 3.0, 0.00001) + + // Overlapping: distance zero, no normal. + input.transform.p = math.vec3(0.5, 0.0, 0.0) + cache = distance.empty_cache() + o = distance.shape_distance(&input, &cache, null, 0) + ensure("sphere-box: overlap is zero", o.distance == 0.0) + + // A capsule (radius 0.25) lying along z, 2 m above the box: 0.75 m apart. + capsule = calloc(2, sizeof(Vec3)) + cp = capsule as Vec3[] + cp[0] = math.vec3(0.0, 0.0, 0.0 - 1.0) + cp[1] = math.vec3(0.0, 0.0, 1.0) + input.proxy_b = distance.shape_proxy(capsule, 2, 0.25) + input.transform.p = math.vec3(0.0, 3.0, 0.0) + cache = distance.empty_cache() + o = distance.shape_distance(&input, &cache, null, 0) + small("capsule-box: distance", o.distance - 1.75, 0.00001) + small("capsule-box: normal y", o.normal.y - 1.0, 0.00001) + // Without radii the distance is between the cores. + input.use_radii = false + cache = distance.empty_cache() + o = distance.shape_distance(&input, &cache, null, 0) + small("capsule-box: core distance", o.distance - 2.0, 0.00001) + free(box) + free(point) + free(capsule) +} + +// Two rotated boxes over a sweep of poses: the warm cache agrees with a +// cold one, the witness points are as far apart as the distance and lie +// in their shapes, and no direction separates the shapes by more than it. +test_sweep_and_cache() { + a = box_points(1.0, 0.5, 0.75) + b = box_points(0.6, 0.8, 0.4) + ap = a as Vec3[] + bp = b as Vec3[] + warm = distance.empty_cache() + worst = 0.0 + hits = 0 + step = 0 + while step < 200 { + t = (step as float) / 200.0 + angle = 3.0 * t + pos = math.vec3(3.0 - 4.0 * t, 0.5 * sin(4.0 * t), 1.5 * cos(2.0 * t)) + xf = Transform { p: pos, q: exact_quat(math.normalize(math.vec3(1.0, 2.0, 0.5)), angle) } + input = DistanceInput { proxy_a: distance.shape_proxy(a, 8, 0.0), proxy_b: distance.shape_proxy(b, 8, 0.0), + transform: xf, use_radii: false } + cold = distance.empty_cache() + o1 = distance.shape_distance(&input, &cold, null, 0) + o2 = distance.shape_distance(&input, &warm, null, 0) + d = math.abs_float(o1.distance - o2.distance) + if d > worst { worst = d } + ensure("sweep: iterations bounded", o1.iterations < distance.MAX_GJK_ITERATIONS && o2.iterations < distance.MAX_GJK_ITERATIONS) + if o1.distance > 0.0 { + hits = hits + 1 + small("sweep: witnesses as far apart as the distance", math.distance(o1.point_a, o1.point_b) - o1.distance, 0.000001) + ensure("sweep: the normal is unit", math.is_normalized(o1.normal)) + // The witness on A is inside A's box (with a little slack). + ensure("sweep: witness a inside a", math.abs_float(o1.point_a.x) <= 1.0001 && math.abs_float(o1.point_a.y) <= 0.5001 && math.abs_float(o1.point_a.z) <= 0.7501) + // The witness on B, taken back to B's frame, is inside B's box. + local_b = math.inv_transform_point(xf, o1.point_b) + ensure("sweep: witness b inside b", math.abs_float(local_b.x) <= 0.6001 && math.abs_float(local_b.y) <= 0.8001 && math.abs_float(local_b.z) <= 0.4001) + // The separation along the normal is the distance; along any axis, no more. + k = 0 + while k < 8 { + axis = math.normalize(math.vec3(cos(1.7 * (k as float)), sin(2.3 * (k as float)), cos(0.9 * (k as float) + 1.0))) + ia = distance.get_proxy_support(input.proxy_a, axis) + ib = distance.get_proxy_support(input.proxy_b, math.inv_rotate_vector(xf.q, math.neg(axis))) + separation = math.dot(math.sub(math.transform_point(xf, bp[ib]), ap[ia]), axis) + ensure("sweep: no axis separates more than the distance", separation <= o1.distance + 0.000001) + k = k + 1 + } + } + step = step + 1 + } + small("sweep: warm and cold agree (worst ${worst})", worst, 0.0001) + ensure("sweep: some poses were apart (${hits})", hits > 50 && hits < 200) + free(a) + free(b) +} + +// The time of impact of a rotating box falling on a box: the fraction +// where they come within a slop, and the shapes are apart just before it +// and not far apart at it. +test_toi_rotating() { + a = box_points(2.0, 0.25, 2.0) + b = box_points(0.5, 0.5, 0.5) + sweep_a = Sweep { local_center: math.vec3_zero(), c1: math.vec3_zero(), c2: math.vec3_zero(), q1: math.quat_identity(), q2: math.quat_identity() } + q2 = exact_quat(math.vec3_axis_z(), 1.2) + sweep_b = Sweep { local_center: math.vec3_zero(), c1: math.vec3(0.0, 4.0, 0.0), c2: math.vec3(0.5, 0.0 - 1.0, 0.0), q1: math.quat_identity(), q2: q2 } + input = TOIInput { proxy_a: distance.shape_proxy(a, 8, 0.0), proxy_b: distance.shape_proxy(b, 8, 0.0), + sweep_a: sweep_a, sweep_b: sweep_b, max_fraction: 1.0 } + t = distance.time_of_impact(&input) + ensure("toi rotating: hit", t.state == distance.TOI_STATE_HIT) + ensure("toi rotating: fraction in range (${t.fraction})", t.fraction > 0.4 && t.fraction < 0.8) + // At the fraction the shapes are within the target distance; a little earlier they are apart. + xf_a = distance.get_sweep_transform(sweep_a, t.fraction) + xf_b = distance.get_sweep_transform(sweep_b, t.fraction) + d_input = DistanceInput { proxy_a: input.proxy_a, proxy_b: input.proxy_b, transform: math.inv_mul_transforms(xf_a, xf_b), use_radii: false } + cache = distance.empty_cache() + at = distance.shape_distance(&d_input, &cache, null, 0) + ensure("toi rotating: within the slop at impact (${at.distance})", at.distance <= 1.25 * math.LINEAR_SLOP) + xf_b = distance.get_sweep_transform(sweep_b, t.fraction - 0.05) + d_input.transform = math.inv_mul_transforms(xf_a, xf_b) + cache = distance.empty_cache() + before = distance.shape_distance(&d_input, &cache, null, 0) + ensure("toi rotating: apart before impact (${before.distance})", before.distance > math.LINEAR_SLOP) + small("toi rotating: the normal points up", t.normal.y - 1.0, 0.01) + + // A sweep that misses is separated at the end. + sweep_b.c2 = math.vec3(0.0, 3.0, 0.0) + input.sweep_b = sweep_b + m = distance.time_of_impact(&input) + ensure("toi miss: separated", m.state == distance.TOI_STATE_SEPARATED) + small("toi miss: fraction 1", m.fraction - 1.0, 0.000001) + + // A sweep that starts overlapped says so. + sweep_b.c1 = math.vec3(0.0, 0.5, 0.0) + input.sweep_b = sweep_b + ov = distance.time_of_impact(&input) + ensure("toi overlapped: overlapped", ov.state == distance.TOI_STATE_OVERLAPPED) + free(a) + free(b) +} + +// The hull's overlap and shape cast run through the distance layer. +test_hull_queries() { + h = hull.make_box_hull(1.0, 1.0, 1.0) + point = calloc(1, sizeof(Vec3)) + pp = point as Vec3[] + pp[0] = math.vec3(3.0, 0.0, 0.0) + sphere = distance.shape_proxy(point, 1, 0.5) + ensure("hull overlap: apart", hull.overlap_hull(h, math.transform_identity(), sphere) == false) + pp[0] = math.vec3(1.4, 0.0, 0.0) + ensure("hull overlap: touching", hull.overlap_hull(h, math.transform_identity(), sphere)) + // The hull moved 2 m along x is 0.1 m from the sphere: apart. + pp[0] = math.vec3(3.6, 0.0, 0.0) + ensure("hull overlap: transformed apart", hull.overlap_hull(h, Transform { p: math.vec3(2.0, 0.0, 0.0), q: math.quat_identity() }, sphere) == false) + pp[0] = math.vec3(3.49, 0.0, 0.0) + ensure("hull overlap: transformed touching", hull.overlap_hull(h, Transform { p: math.vec3(2.0, 0.0, 0.0), q: math.quat_identity() }, sphere)) + + pp[0] = math.vec3(4.0, 0.0, 0.0) + c = hull.shape_cast_hull(h, sphere, math.vec3(0.0 - 6.0, 0.0, 0.0), 1.0, false) + ensure("hull cast: hit", c.hit) + // The sphere surface reaches the face after 2.5 m of 6: fraction ~0.417, minus the slop. + small("hull cast: fraction", c.fraction - 2.5 / 6.0, 0.002) + small("hull cast: normal", c.normal.x - 1.0, 0.001) + small("hull cast: point on the face", c.point.x - 1.0, 0.01) + miss = hull.shape_cast_hull(h, sphere, math.vec3(0.0, 6.0, 0.0), 1.0, false) + ensure("hull cast: miss", miss.hit == false) + free(point) + hull.destroy_hull(h) +} + +main() { + before = core.alloc_count() + test_reference() + test_rounded() + test_sweep_and_cache() + test_toi_rotating() + test_hull_queries() + ensure("every counted allocation was freed", core.alloc_count() == before && core.bytes_in_use() == 0) + + println("distance: ${checks} checks") + if failures == 0 { + println("distance: all checks passed") + } else { + println("distance: ${failures} failure(s)") + exit(1) + } +} diff --git a/bench/RESULTS.md b/bench/RESULTS.md index 7db172d..7805149 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -124,3 +124,26 @@ place of pointers, structs passed by value); the box hull is a heap block here where the reference's is a stack value, so its cost is the allocation and the hash. Hull construction is a load-time cost, not a per-step one, so this is recorded rather than chased. + +## distance + +`bench/distance.ae` and `bench/distance_box3d.c`: two boxes (1 x 0.5 x +0.75 and 0.6 x 0.8 x 0.4) over 100,000 poses along a sweep that passes +through overlap, each pose a GJK query from a cold cache and then from a +cache warmed by the pose before; 10,000 shape casts of the second box at +the first; 10,000 times of impact of the second box falling and turning +onto a slab. + +| phase | aephysics | Box3D | +|---|---|---| +| 100,000 GJK queries, cold cache | 22.9 ms | **17.3** | +| 100,000 GJK queries, warm cache | 18.2 | **11.9** | +| 10,000 shape casts | 5.0 | **3.5** | +| 10,000 times of impact | 13.8 | **9.4** | + +The sums of the results agree to the precision of the reference's floats +(distances 26,291.3 on both, cast fractions 2,780.06, impact fractions +5,523.1) and the GJK iteration counts within 0.01% (335,952 against +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. diff --git a/bench/distance.ae b/bench/distance.ae new file mode 100644 index 0000000..acb8758 --- /dev/null +++ b/bench/distance.ae @@ -0,0 +1,114 @@ +// GJK, the shape cast and the time of impact on the same scenes as +// bench/distance_box3d.c: two boxes over 100,000 poses along a sweep, +// cold cache then warm; 10,000 shape casts; 10,000 times of impact of a +// falling, turning box. Single thread, wall time per phase. +import std.string +import std.os +import aephysics.math +import aephysics.distance + +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 = 100000 +const CASTS = 10000 +const TOIS = 10000 + +box_points(hx: float, hy: float, hz: float) -> ptr { + block = calloc(8, sizeof(Vec3)) + p = block as Vec3[] + i = 0 + while i < 8 { + x = hx + y = hy + z = hz + if (i & 1) != 0 { x = 0.0 - hx } + if (i & 2) != 0 { y = 0.0 - hy } + if (i & 4) != 0 { z = 0.0 - hz } + p[i] = math.vec3(x, y, z) + i = i + 1 + } + return block +} + +exact_quat(axis: Vec3, radians: float) -> Quat { + h = 0.5 * radians + return Quat { v: math.mul_sv(sin(h), axis), s: cos(h) } +} + +main() { + a = box_points(1.0, 0.5, 0.75) + b = box_points(0.6, 0.8, 0.4) + proxy_a = distance.shape_proxy(a, 8, 0.0) + proxy_b = distance.shape_proxy(b, 8, 0.0) + axis = math.normalize(math.vec3(1.0, 2.0, 0.5)) + + sum = 0.0 + iterations = 0 + t0 = clock() + step = 0 + while step < N { + t = (step as float) / (N as float) + input = DistanceInput { proxy_a: proxy_a, proxy_b: proxy_b, + transform: Transform { p: math.vec3(3.0 - 4.0 * t, 0.5 * sin(4.0 * t), 1.5 * cos(2.0 * t)), q: exact_quat(axis, 3.0 * t) }, + use_radii: false } + cache = distance.empty_cache() + o = distance.shape_distance(&input, &cache, null, 0) + sum = sum + o.distance + iterations = iterations + o.iterations + step = step + 1 + } + t1 = clock() + sum_warm = 0.0 + warm = distance.empty_cache() + step = 0 + while step < N { + t = (step as float) / (N as float) + input = DistanceInput { proxy_a: proxy_a, proxy_b: proxy_b, + transform: Transform { p: math.vec3(3.0 - 4.0 * t, 0.5 * sin(4.0 * t), 1.5 * cos(2.0 * t)), q: exact_quat(axis, 3.0 * t) }, + use_radii: false } + o = distance.shape_distance(&input, &warm, null, 0) + sum_warm = sum_warm + o.distance + step = step + 1 + } + t2 = clock() + + cast_sum = 0.0 + i = 0 + while i < CASTS { + t = (i as float) / (CASTS as float) + cast_input = ShapeCastPairInput { proxy_a: proxy_a, proxy_b: proxy_b, + transform: Transform { p: math.vec3(4.0, 0.5 * sin(4.0 * t), 0.0), q: exact_quat(axis, 3.0 * t) }, + translation_b: math.vec3(0.0 - 8.0, 0.0, 0.0), max_fraction: 1.0, can_encroach: false } + c = distance.shape_cast(&cast_input) + if c.hit { cast_sum = cast_sum + c.fraction } else { cast_sum = cast_sum - 1.0 } + i = i + 1 + } + t3 = clock() + + toi_sum = 0.0 + ground = box_points(2.0, 0.25, 2.0) + proxy_g = distance.shape_proxy(ground, 8, 0.0) + i = 0 + while i < TOIS { + t = (i as float) / (TOIS as float) + toi_input = TOIInput { proxy_a: proxy_g, proxy_b: proxy_b, + sweep_a: Sweep { local_center: math.vec3_zero(), c1: math.vec3_zero(), c2: math.vec3_zero(), q1: math.quat_identity(), q2: math.quat_identity() }, + sweep_b: Sweep { local_center: math.vec3_zero(), c1: math.vec3(0.0, 4.0, 0.0), c2: math.vec3(0.5, 0.0 - 1.0, 0.0), q1: math.quat_identity(), q2: exact_quat(math.vec3_axis_z(), 0.5 + t) }, + max_fraction: 1.0 } + toi = distance.time_of_impact(&toi_input) + toi_sum = toi_sum + toi.fraction + i = i + 1 + } + t4 = clock() + + println("aephysics distance: ${N} cold queries ${ms(t1 - t0)} ms (sum ${sum}, ${iterations} iterations), ${N} warm ${ms(t2 - t1)} ms (sum ${sum_warm}), ${CASTS} casts ${ms(t3 - t2)} ms (sum ${cast_sum}), ${TOIS} tois ${ms(t4 - t3)} ms (sum ${toi_sum})") + free(a) + free(b) + free(ground) +} diff --git a/bench/distance_box3d.c b/bench/distance_box3d.c new file mode 100644 index 0000000..1e8045b --- /dev/null +++ b/bench/distance_box3d.c @@ -0,0 +1,119 @@ +// GJK, the shape cast and the time of impact of the reference on the same +// scenes as bench/distance.ae: two boxes over 100,000 poses along a +// sweep, cold cache then warm; 10,000 shape casts; 10,000 times of +// impact of a falling, turning box. Single thread, wall time per phase. +#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 void box_points( b3Vec3* p, float hx, float hy, float hz ) +{ + for ( int i = 0; i < 8; ++i ) + { + p[i] = (b3Vec3){ ( i & 1 ) ? -hx : hx, ( i & 2 ) ? -hy : hy, ( i & 4 ) ? -hz : hz }; + } +} + +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 100000 +#define CASTS 10000 +#define TOIS 10000 + +int main( void ) +{ + b3Vec3 a[8], b[8]; + box_points( a, 1.0f, 0.5f, 0.75f ); + box_points( b, 0.6f, 0.8f, 0.4f ); + b3ShapeProxy proxyA = { a, 8, 0.0f }; + b3ShapeProxy proxyB = { b, 8, 0.0f }; + b3Vec3 axis = b3Normalize( (b3Vec3){ 1.0f, 2.0f, 0.5f } ); + + double sum = 0.0; + int iterations = 0; + double t0 = now_ms(); + for ( int step = 0; step < N; ++step ) + { + float t = (float)step / (float)N; + b3DistanceInput input = { + .proxyA = proxyA, + .proxyB = proxyB, + .transform = { { 3.0f - 4.0f * t, 0.5f * sinf( 4.0f * t ), 1.5f * cosf( 2.0f * t ) }, exact_quat( axis, 3.0f * t ) }, + .useRadii = false, + }; + b3SimplexCache cache = { 0 }; + b3DistanceOutput o = b3ShapeDistance( &input, &cache, NULL, 0 ); + sum += o.distance; + iterations += o.iterations; + } + double t1 = now_ms(); + double sumWarm = 0.0; + b3SimplexCache warm = { 0 }; + for ( int step = 0; step < N; ++step ) + { + float t = (float)step / (float)N; + b3DistanceInput input = { + .proxyA = proxyA, + .proxyB = proxyB, + .transform = { { 3.0f - 4.0f * t, 0.5f * sinf( 4.0f * t ), 1.5f * cosf( 2.0f * t ) }, exact_quat( axis, 3.0f * t ) }, + .useRadii = false, + }; + b3DistanceOutput o = b3ShapeDistance( &input, &warm, NULL, 0 ); + sumWarm += o.distance; + } + double t2 = now_ms(); + + double castSum = 0.0; + for ( int i = 0; i < CASTS; ++i ) + { + float t = (float)i / (float)CASTS; + b3ShapeCastPairInput input = { + .proxyA = proxyA, + .proxyB = proxyB, + .transform = { { 4.0f, 0.5f * sinf( 4.0f * t ), 0.0f }, exact_quat( axis, 3.0f * t ) }, + .translationB = { -8.0f, 0.0f, 0.0f }, + .maxFraction = 1.0f, + .canEncroach = false, + }; + b3CastOutput o = b3ShapeCast( &input ); + castSum += o.hit ? o.fraction : -1.0; + } + double t3 = now_ms(); + + double toiSum = 0.0; + b3Vec3 ground[8]; + box_points( ground, 2.0f, 0.25f, 2.0f ); + b3ShapeProxy proxyG = { ground, 8, 0.0f }; + for ( int i = 0; i < TOIS; ++i ) + { + float t = (float)i / (float)TOIS; + b3TOIInput input = { + .proxyA = proxyG, + .proxyB = proxyB, + .sweepA = { b3Vec3_zero, b3Vec3_zero, b3Vec3_zero, b3Quat_identity, b3Quat_identity }, + .sweepB = { b3Vec3_zero, { 0.0f, 4.0f, 0.0f }, { 0.5f, -1.0f, 0.0f }, b3Quat_identity, exact_quat( b3Vec3_axisZ, 0.5f + t ) }, + .maxFraction = 1.0f, + }; + b3TOIOutput o = b3TimeOfImpact( &input ); + toiSum += o.fraction; + } + double t4 = now_ms(); + + printf( "box3d distance: %d cold queries %.2f ms (sum %.3f, %d iterations), %d warm %.2f ms (sum %.3f), %d casts %.2f ms (sum %.3f), %d tois %.2f ms (sum %.3f)\n", + N, t1 - t0, sum, iterations, N, t2 - t1, sumWarm, CASTS, t3 - t2, castSum, TOIS, t4 - t3, toiSum ); + return 0; +} diff --git a/design.md b/design.md index 139329a..c564911 100644 --- a/design.md +++ b/design.md @@ -40,25 +40,31 @@ started until its tests pass. SOA mirrors, the box hull a heap block. 438 checks from test_hull.c at the reference's tolerances; the same hulls on the benchmark scenes at 1.6-2x its time. -5. **collision, static**: `aabb`, - `distance` (GJK, shape cast, segment distance), `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_distance`, `test_hull`, `test_manifold`, - `test_sat`, `test_shape`, `test_mesh`, `test_height_field`. -6. **dynamics**: `body`, `contact`, `constraint_graph` (graph colouring), +5. **distance** (done): GJK, the shape cast and the time of impact as + `aephysics.distance`, the simplex's vertices as named fields and the + cache's index pairs as named ints. 1,143 checks: the reference's four + plus spheres and capsules at analytic distances, warm against cold + caches over a sweep of poses, witness points inside their shapes and + 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), `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`. -7. **parallel**: `parallel_for` and the scheduler over Aether's actors; +8. **parallel**: `parallel_for` and the scheduler over Aether's actors; the benchmarks by thread count as the original records them. -8. **recording and replay**, `world_snapshot`: last, since they are the +9. **recording and replay**, `world_snapshot`: last, since they are the tooling and not the engine. -9. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run +10. **benchmarks**: `reference/benchmark/main.c`'s nine scenes ported, run against the C build on the same machine, recorded under `benchmark/`. ## Measures