From 2e1f524a4829401f96dc68cd98a9d78e1e8ebe28 Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Sat, 19 Sep 2026 23:56:39 -0300 Subject: [PATCH] broad_phase: the broad phase, and an overflow-free key hash aephysics.broad_phase, Box3D's broad_phase.c in Aether: a dynamic tree per body type with the shapes' proxies keyed by type in the low bits, and the pair update after a step: the dynamic tree against itself through the sibling pairs a moved node touched (cross-subtree only, so no duplicates) and against the static and kinematic trees from a breadth-first seeding; the candidates culled by the pair set, filtered by the client's visitor, a compound's pair expanded to one per overlapping child through the client's compound lookups, and the keys sorted so the contacts come out in an order the trees cannot change. The reference filters and creates contacts inside its world; here the update leaves the keys for the client, which adopts them into the set. test_broad_phase.ae (ours; the reference covers this through its world tests): keys and the sort, a grid of dynamic boxes over a static ground with a kinematic platform against a brute force across the first update, a quiet update, moves, a removed pair, the filter, one body's shapes, a forced static proxy, a destroyed proxy, and a compound's pairs one per child. 36 checks. The hash under every set and map, core's key_hash, multiplied signed longs: undefined in the C underneath, and gcc at -O2 made two inlined copies disagree, so a key stored by one copy was not found by the other (the broad phase test caught it; the mesh's maps had passed by luck). It now mixes both halves in 32-bit products. The mesh builds move from 199 to 221 ms with it. bench/broad_phase.ae runs alone (the reference's update has no free-standing counterpart): 10,000 moving boxes at 4.6 ms a step. scripts/bench.sh runs a layer without a reference pair. --- README.md | 1 + aephysics/broad_phase/module.ae | 566 ++++++++++++++++++++++++++++++++ aephysics/core/module.ae | 24 +- aephysics/test_broad_phase.ae | 384 ++++++++++++++++++++++ bench/RESULTS.md | 33 +- bench/broad_phase.ae | 96 ++++++ design.md | 17 +- scripts/bench.sh | 9 +- 8 files changed, 1111 insertions(+), 19 deletions(-) create mode 100644 aephysics/broad_phase/module.ae create mode 100644 aephysics/test_broad_phase.ae create mode 100644 bench/broad_phase.ae diff --git a/README.md b/README.md index 8c723b0..c20d4fd 100644 --- a/README.md +++ b/README.md @@ -40,6 +40,7 @@ so a test written against the reference reads the same here. | `aephysics.compound` | the baked compound: capsules, hulls, meshes and spheres in one block under a static tree, hulls and meshes shared by content, materials by value; bounds, overlap, ray and shape casts, the box query, the mover's planes | done, `test_compound.ae` (159 checks); [same results as the reference, build 0.7x, queries 1.3-1.6x](bench/RESULTS.md#compound) | | `aephysics.shape` | the shape of any kind (sphere, capsule, hull, mesh, height field, compound) with the dispatch over every kind under a transform: bounds, swept and fat bounds, centroid, areas, mass, extent, ray and shape casts, overlap, the mover's planes, the proxy; the collision filters | done, `test_shape.ae` (440 checks); [same results as the reference, rays and masses at parity, radius casts 2.5x](bench/RESULTS.md#shape) | | `aephysics.mover` | the character mover's plane solver: pushes accumulated and clamped over twenty sweeps, the velocity clip | done, `test_mover.ae` (56 checks); [same results as the reference, 0.9x its time](bench/RESULTS.md#mover) | +| `aephysics.broad_phase` | the broad phase: a tree per body type, proxies keyed by type, the pair update through the moved siblings and cross-tree seeds with the filter and compound lookups as visitors, the pair set, the keys sorted | done, `test_broad_phase.ae` (36 checks against a brute force); [10,000 moving boxes at 4.6 ms a step](bench/RESULTS.md#broad_phase) | | `aephysics.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/broad_phase/module.ae b/aephysics/broad_phase/module.ae new file mode 100644 index 0000000..6abcaea --- /dev/null +++ b/aephysics/broad_phase/module.ae @@ -0,0 +1,566 @@ +// aephysics.broad_phase -- the broad phase: one dynamic tree per body +// type (static, kinematic, dynamic), the shapes' proxies keyed by their +// type in the low two bits, and the pair update that finds every new +// overlap after a step: the dynamic tree against itself through the +// sibling pairs a moved node touched (cross-subtree only, so no +// duplicates), and against the static and kinematic trees from a +// breadth-first seeding; the candidates culled by the set of pairs that +// already have a contact, filtered by the client's visitor, a compound's +// pair expanded to one per overlapping child, and the keys sorted so +// the contacts come out in an order the trees cannot change. +// +// Box3D's broad_phase.c (Erin Catto, MIT), the reference this engine is +// measured against. Names are the reference's without its prefix, in +// snake case: b3UpdateBroadPhasePairs is update_broad_phase_pairs. +// +// Differences: the reference filters pairs and creates contacts inside +// the update through its world; here the filter and the compound +// lookups are visitors and the update leaves the sorted keys for the +// client to turn into contacts, adding each to the pair set as it does +// (the reference's contact creation does the same). Single-threaded: +// the seeds and the moved siblings are walked in order here, where the +// reference spreads them over workers. +import std.string +import aephysics.math +import aephysics.core +import aephysics.dynamic_tree + +exports ( + BroadPhase, PairVisitors, Capacity, + BODY_STATIC, BODY_KINEMATIC, BODY_DYNAMIC, BODY_TYPE_COUNT, NULL_INDEX, + proxy_type, proxy_id, proxy_key, + create_broad_phase, destroy_broad_phase, broad_phase_tree, + broad_phase_create_proxy, broad_phase_destroy_proxy, broad_phase_move_proxy, + broad_phase_mark_proxy_moved, broad_phase_mark_proxy_moved_serial, broad_phase_get_shape_index, + broad_phase_test_overlap, broad_phase_get_aabb, + update_broad_phase_pairs, pair_key_count, pair_key, pair_key_shape_a, pair_key_shape_b, pair_key_child, + add_pair, remove_pair, has_pair, pair_count, + validate_broad_phase, validate_no_moved, sort_longs +) + +extern memset(block: ptr, value: int, size: int) -> ptr +extern memcpy(dst: ptr, src: ptr, size: int) -> ptr + +const NULL_INDEX = 0 - 1 +const BODY_STATIC = 0 +const BODY_KINEMATIC = 1 +const BODY_DYNAMIC = 2 +const BODY_TYPE_COUNT = 3 +const CROSS_SEED_COUNT = 64 +const STACK_SIZE = 512 + +// What a world expects to hold, for the initial capacities. +struct Capacity { + static_shape_count: int + dynamic_shape_count: int + contact_count: int +} + +struct BroadPhase { + trees: ptr // DynamicTree[BODY_TYPE_COUNT] + pair_set: HashSet // the pairs that have a contact + moved_siblings: ptr // int[]: the dynamic tree's sibling pairs a moved node touched + moved_capacity: int + pair_keys: ptr // long[]: the keys the last update found, sorted + pair_key_count: int + pair_key_capacity: int + stack: ptr // int[STACK_SIZE]: the proxy-against-subtree walk + pair_stack: ptr // int[2 * STACK_SIZE]: the subtree-against-subtree walk + seeds: ptr // int[2 * 2 * CROSS_SEED_COUNT]: node index pairs + queue: ptr // int[2 * 2 * CROSS_SEED_COUNT]: the seeding's ring +} + +// What the client answers about its shapes during an update: whether a +// pair may collide, and which shapes are compounds (the compound's tree +// and the body's transform, so the other shape's box can be pulled into +// the compound's frame). compound_tree_of returns null for a plain +// shape and a *DynamicTree for a compound. +struct PairVisitors { + should_pair: fn(int, int, ptr) -> bool + compound_tree_of: fn(int, ptr) -> ptr + compound_transform: fn(int, ptr) -> Transform + context: ptr +} + +// The type sits in the low two bits of a key, the id above. +proxy_type(key: int) -> int { return key & 3 } +proxy_id(key: int) -> int { return key >> 2 } +proxy_key(id: int, kind: int) -> int { return (id << 2) | kind } + +broad_phase_tree(bp: *BroadPhase, kind: int) -> *DynamicTree { return (bp.trees + kind * sizeof(DynamicTree)) as *DynamicTree } + +create_broad_phase(bp: *BroadPhase, capacity: Capacity) { + bp.trees = core.alloc(BODY_TYPE_COUNT * sizeof(DynamicTree)) + made = dynamic_tree.tree_create(math.max_int(16, capacity.static_shape_count)) + copy_tree(broad_phase_tree(bp, BODY_STATIC), made) + made = dynamic_tree.tree_create(16) + copy_tree(broad_phase_tree(bp, BODY_KINEMATIC), made) + made = dynamic_tree.tree_create(math.max_int(16, capacity.dynamic_shape_count)) + copy_tree(broad_phase_tree(bp, BODY_DYNAMIC), made) + bp.pair_set = core.set_create(math.max_int(32, 2 * capacity.contact_count)) + bp.moved_siblings = null + bp.moved_capacity = 0 + bp.pair_keys = null + bp.pair_key_count = 0 + bp.pair_key_capacity = 0 + bp.stack = core.alloc(STACK_SIZE * 4) + bp.pair_stack = core.alloc(2 * STACK_SIZE * 4) + bp.seeds = core.alloc(4 * CROSS_SEED_COUNT * 4) + bp.queue = core.alloc(4 * CROSS_SEED_COUNT * 4) +} + +copy_tree(dst: *DynamicTree, src: DynamicTree) { + dst.nodes = src.nodes + dst.parents = src.parents + dst.proxies = src.proxies + dst.node_end = src.node_end + dst.node_capacity = src.node_capacity + dst.pair_free_list = src.pair_free_list + dst.proxy_count = src.proxy_count + dst.proxy_capacity = src.proxy_capacity + dst.proxy_free_list = src.proxy_free_list + dst.swap_nodes = src.swap_nodes + dst.leaf_indices = src.leaf_indices + dst.leaf_nodes = src.leaf_nodes + dst.leaf_centers = src.leaf_centers + dst.rebuild_capacity = src.rebuild_capacity + dst.dfs_ordered = src.dfs_ordered + dst.stack = src.stack + dst.stack_dist = src.stack_dist + dst.copy_stack = src.copy_stack +} + +destroy_broad_phase(bp: *BroadPhase) { + i = 0 + while i < BODY_TYPE_COUNT { + dynamic_tree.tree_destroy(broad_phase_tree(bp, i)) + i = i + 1 + } + core.free_bytes(bp.trees, BODY_TYPE_COUNT * sizeof(DynamicTree)) + core.set_destroy(&bp.pair_set) + core.free_bytes(bp.moved_siblings, bp.moved_capacity * 4) + core.free_bytes(bp.pair_keys, bp.pair_key_capacity * 8) + core.free_bytes(bp.stack, STACK_SIZE * 4) + core.free_bytes(bp.pair_stack, 2 * STACK_SIZE * 4) + core.free_bytes(bp.seeds, 4 * CROSS_SEED_COUNT * 4) + core.free_bytes(bp.queue, 4 * CROSS_SEED_COUNT * 4) + memset(bp as ptr, 0, sizeof(BroadPhase)) +} + +// A shape's proxy in the tree of its body's type. A static proxy is not +// marked moved (no pairs are sought for it) unless the client forces it. +broad_phase_create_proxy(bp: *BroadPhase, kind: int, box: AABB, category_bits: long, shape_index: int, force_pair_creation: bool) -> int { + mark = kind != BODY_STATIC || force_pair_creation + id = dynamic_tree.tree_create_proxy_internal(broad_phase_tree(bp, kind), box, category_bits, shape_index as long, mark) + return proxy_key(id, kind) +} + +broad_phase_destroy_proxy(bp: *BroadPhase, key: int) { + dynamic_tree.tree_destroy_proxy(broad_phase_tree(bp, proxy_type(key)), proxy_id(key)) +} + +// The proxy re-inserted at a new box and marked moved. +broad_phase_move_proxy(bp: *BroadPhase, key: int, box: AABB) { + dynamic_tree.tree_move_proxy_internal(broad_phase_tree(bp, proxy_type(key)), proxy_id(key), box, true) +} + +broad_phase_mark_proxy_moved_serial(bp: *BroadPhase, key: int) { + dynamic_tree.tree_mark_proxy_moved_serial(broad_phase_tree(bp, proxy_type(key)), proxy_id(key)) +} + +broad_phase_mark_proxy_moved(bp: *BroadPhase, key: int, box: AABB) { + dynamic_tree.tree_mark_proxy_moved(broad_phase_tree(bp, proxy_type(key)), proxy_id(key), box) +} + +broad_phase_get_shape_index(bp: *BroadPhase, key: int) -> int { + return dynamic_tree.tree_get_user_data(broad_phase_tree(bp, proxy_type(key)), proxy_id(key)) as int +} + +broad_phase_get_aabb(bp: *BroadPhase, key: int) -> AABB { + return dynamic_tree.tree_get_aabb(broad_phase_tree(bp, proxy_type(key)), proxy_id(key)) +} + +broad_phase_test_overlap(bp: *BroadPhase, key_a: int, key_b: int) -> bool { + return math.aabb_overlaps(broad_phase_get_aabb(bp, key_a), broad_phase_get_aabb(bp, key_b)) +} + +// --- the pair set and the keys ------------------------------------------------------------------- + +add_pair(bp: *BroadPhase, key: long) -> bool { return core.set_add(&bp.pair_set, key) } +remove_pair(bp: *BroadPhase, key: long) -> bool { return core.set_remove(&bp.pair_set, key) } +has_pair(bp: *BroadPhase, key: long) -> bool { return core.set_contains(&bp.pair_set, key) } +pair_count(bp: *BroadPhase) -> int { return core.set_count(&bp.pair_set) } + +pair_key_count(bp: *BroadPhase) -> int { return bp.pair_key_count } + +pair_key(bp: *BroadPhase, index: int) -> long { + keys = bp.pair_keys as long[] + return keys[index] +} + +pair_key_shape_a(key: long) -> int { return (core.lsr(key, 64 - core.SHAPE_POWER) as int) & (core.MAX_SHAPES - 1) } +pair_key_shape_b(key: long) -> int { return (core.lsr(key, 64 - 2 * core.SHAPE_POWER) as int) & (core.MAX_SHAPES - 1) } +pair_key_child(key: long) -> int { return (key as int) & (core.MAX_CHILD_SHAPES - 1) } + +push_key(bp: *BroadPhase, key: long) { + if bp.pair_key_count == bp.pair_key_capacity { + grown = math.max_int(64, 2 * bp.pair_key_capacity) + block = core.alloc(grown * 8) + if bp.pair_keys != null { + memcpy(block, bp.pair_keys, bp.pair_key_count * 8) + core.free_bytes(bp.pair_keys, bp.pair_key_capacity * 8) + } + bp.pair_keys = block + bp.pair_key_capacity = grown + } + keys = bp.pair_keys as long[] + keys[bp.pair_key_count] = key + bp.pair_key_count = bp.pair_key_count + 1 +} + +// --- candidates ------------------------------------------------------------------------------------ + +// The update's visitors and the walk's stack of subtree pairs. +struct PairContext { + bp: *BroadPhase + visitors: PairVisitors + check_compounds: bool // only the static cross pass can meet a compound + stack_count: int +} + +// One of the two is a compound: the pair becomes one per child whose box +// the other shape's box (pulled into the compound's frame) overlaps. +struct CompoundContext { + bp: *BroadPhase + compound_shape: int + other_shape: int +} + +compound_child_visitor(proxy: int, user_data: long, context: ptr) -> bool { + ctx = context as *CompoundContext + key = core.shape_pair_key(ctx.compound_shape, ctx.other_shape, user_data as int) + if has_pair(ctx.bp, key) == false { push_key(ctx.bp, key) } + return true +} + +emit_compound_pairs(ctx: *PairContext, compound_shape: int, other_shape: int, other_key: int) { + bp = ctx.bp + // A visitor is called through a local copy: a nested field is not callable. + visitors = ctx.visitors + compound_tree = visitors.compound_tree_of(compound_shape, visitors.context) as *DynamicTree + compound_transform = visitors.compound_transform(compound_shape, visitors.context) + other_box = broad_phase_get_aabb(bp, other_key) + local_box = math.aabb_transform(math.invert_transform(compound_transform), other_box) + child_ctx = CompoundContext { bp: bp, compound_shape: compound_shape, other_shape: other_shape } + start = bp.pair_key_count + dynamic_tree.tree_query(compound_tree, local_box, dynamic_tree.default_mask_bits(), false, compound_child_visitor, (&child_ctx) as ptr) + // The gauntlet is a property of the pair, so it runs once for every child. + if bp.pair_key_count > start && visitors.should_pair(compound_shape, other_shape, visitors.context) == false { + bp.pair_key_count = start + } +} + +// A candidate straight from the trees: culled by the pair set, then the +// visitor's gauntlet, then kept as a key. The proxies' keys are known +// (the compound expansion needs the other shape's box). +add_candidate_pair(ctx: *PairContext, shape_a: int, shape_b: int, key_a: int, key_b: int) { + bp = ctx.bp + visitors = ctx.visitors + if ctx.check_compounds { + if visitors.compound_tree_of(shape_a, visitors.context) != null { + emit_compound_pairs(ctx, shape_a, shape_b, key_b) + return + } + if visitors.compound_tree_of(shape_b, visitors.context) != null { + emit_compound_pairs(ctx, shape_b, shape_a, key_a) + return + } + } + key = core.shape_pair_key(shape_a, shape_b, 0) + if has_pair(bp, key) { return } + if visitors.should_pair(math.min_int(shape_a, shape_b), math.max_int(shape_a, shape_b), visitors.context) { + push_key(bp, key) + } +} + +// Did either move, and if so do they overlap? +test_pair(a: TreeNode, b: TreeNode) -> bool { + if ((a.flag_index | b.flag_index) & dynamic_tree.MOVED_NODE) == 0 { return false } + return math.aabb_overlaps(a.aabb, b.aabb) +} + +// A leaf against a subtree of a tree (its own or another): the moved +// flag on either side and the boxes cull. +collide_proxy_and_subtree(ctx: *PairContext, leaf: TreeNode, leaf_key: int, tree: *DynamicTree, tree_kind: int, pair: int) { + nodes = tree.nodes as TreeNode[] + proxies = tree.proxies as TreeProxy[] + leaf_mark = leaf.flag_index & dynamic_tree.MOVED_NODE + leaf_box = leaf.aabb + shape_id = broad_phase_get_shape_index(ctx.bp, leaf_key) + stack = ctx.bp.stack as int[] + stack_count = 1 + stack[0] = pair + while stack_count > 0 { + stack_count = stack_count - 1 + current = stack[stack_count] + i = 0 + while i < 2 { + index = current + i + i = i + 1 + flags = nodes[index].flag_index + if ((flags | leaf_mark) & dynamic_tree.MOVED_NODE) == 0 { continue } + if math.aabb_overlaps(leaf_box, nodes[index].aabb) == false { continue } + if dynamic_tree.is_leaf(flags) { + other_proxy = dynamic_tree.proxy_id_of(flags) + add_candidate_pair(ctx, shape_id, proxies[other_proxy].user_data as int, leaf_key, proxy_key(other_proxy, tree_kind)) + } else if stack_count < STACK_SIZE { + stack[stack_count] = dynamic_tree.left_child(flags) + stack_count = stack_count + 1 + } + } + } +} + +// One node against another: a pair of leaves is a candidate, a leaf +// against a subtree walks it, two subtrees go on the stack. +visit_pair(ctx: *PairContext, tree_a: *DynamicTree, kind_a: int, tree_b: *DynamicTree, kind_b: int, a: int, b: int) { + nodes_a = tree_a.nodes as TreeNode[] + nodes_b = tree_b.nodes as TreeNode[] + node_a = nodes_a[a] + node_b = nodes_b[b] + if test_pair(node_a, node_b) == false { return } + leaf_a = dynamic_tree.is_leaf(node_a.flag_index) + leaf_b = dynamic_tree.is_leaf(node_b.flag_index) + if leaf_a && leaf_b { + proxies_a = tree_a.proxies as TreeProxy[] + proxies_b = tree_b.proxies as TreeProxy[] + proxy_a = dynamic_tree.proxy_id_of(node_a.flag_index) + proxy_b = dynamic_tree.proxy_id_of(node_b.flag_index) + add_candidate_pair(ctx, proxies_a[proxy_a].user_data as int, proxies_b[proxy_b].user_data as int, + proxy_key(proxy_a, kind_a), proxy_key(proxy_b, kind_b)) + } else if leaf_a { + collide_proxy_and_subtree(ctx, node_a, proxy_key(dynamic_tree.proxy_id_of(node_a.flag_index), kind_a), tree_b, kind_b, dynamic_tree.left_child(node_b.flag_index)) + } else if leaf_b { + collide_proxy_and_subtree(ctx, node_b, proxy_key(dynamic_tree.proxy_id_of(node_b.flag_index), kind_b), tree_a, kind_a, dynamic_tree.left_child(node_a.flag_index)) + } else if ctx.stack_count < STACK_SIZE { + stack = ctx.bp.pair_stack as int[] + stack[2 * ctx.stack_count] = dynamic_tree.left_child(node_a.flag_index) + stack[2 * ctx.stack_count + 1] = dynamic_tree.left_child(node_b.flag_index) + ctx.stack_count = ctx.stack_count + 1 + } +} + +// Two subtrees against each other, which may be of one tree: pairs come +// only across the two, never within one, so a moved node's two children +// give no duplicates (Real-Time Collision Detection 6.3.2). +collide_cross_pairs(ctx: *PairContext, tree_a: *DynamicTree, kind_a: int, tree_b: *DynamicTree, kind_b: int, subtree_a: int, subtree_b: int) { + stack = ctx.bp.pair_stack as int[] + ctx.stack_count = 0 + visit_pair(ctx, tree_a, kind_a, tree_b, kind_b, subtree_a, subtree_b) + while ctx.stack_count > 0 { + ctx.stack_count = ctx.stack_count - 1 + pair_a = stack[2 * ctx.stack_count] + pair_b = stack[2 * ctx.stack_count + 1] + i = 0 + while i < 4 { + visit_pair(ctx, tree_a, kind_a, tree_b, kind_b, pair_a + (i >> 1), pair_b + (i & 1)) + i = i + 1 + } + } +} + +// The dynamic tree's sibling pairs a moved node touched, in order. +gather_moved_siblings(tree: *DynamicTree, siblings: int[]) -> int { + nodes = tree.nodes as TreeNode[] + count = 0 + pair = 2 + while pair < tree.node_end { + if ((nodes[pair].flag_index | nodes[pair + 1].flag_index) & dynamic_tree.MOVED_NODE) != 0 { + siblings[count] = pair + count = count + 1 + } + pair = pair + 2 + } + return count +} + +// A breadth-first walk of two trees from their roots until the queue +// holds enough node pairs to seed the cross walk (the reference spreads +// them over workers; here they keep the same order). +gather_cross_seeds(bp: *BroadPhase, tree_a: *DynamicTree, tree_b: *DynamicTree, seeds: int[], seed_offset: int) -> int { + nodes_a = tree_a.nodes as TreeNode[] + nodes_b = tree_b.nodes as TreeNode[] + queue = bp.queue as int[] + mask = 2 * CROSS_SEED_COUNT - 1 + head = 0 + tail = 0 + if tree_a.proxy_count > 0 && tree_b.proxy_count > 0 && test_pair(nodes_a[dynamic_tree.ROOT_NODE], nodes_b[dynamic_tree.ROOT_NODE]) { + queue[2 * (tail & mask)] = dynamic_tree.ROOT_NODE + queue[2 * (tail & mask) + 1] = dynamic_tree.ROOT_NODE + tail = tail + 1 + } + seed_count = 0 + while head < tail && seed_count + (tail - head) + 3 < CROSS_SEED_COUNT { + a = queue[2 * (head & mask)] + b = queue[2 * (head & mask) + 1] + head = head + 1 + if dynamic_tree.is_leaf(nodes_a[a].flag_index) || dynamic_tree.is_leaf(nodes_b[b].flag_index) { + seeds[2 * (seed_offset + seed_count)] = a + seeds[2 * (seed_offset + seed_count) + 1] = b + seed_count = seed_count + 1 + continue + } + child_a = dynamic_tree.left_child(nodes_a[a].flag_index) + child_b = dynamic_tree.left_child(nodes_b[b].flag_index) + i = 0 + while i < 4 { + ca = child_a + (i >> 1) + cb = child_b + (i & 1) + if test_pair(nodes_a[ca], nodes_b[cb]) { + queue[2 * (tail & mask)] = ca + queue[2 * (tail & mask) + 1] = cb + tail = tail + 1 + } + i = i + 1 + } + } + while head < tail { + seeds[2 * (seed_offset + seed_count)] = queue[2 * (head & mask)] + seeds[2 * (seed_offset + seed_count) + 1] = queue[2 * (head & mask) + 1] + seed_count = seed_count + 1 + head = head + 1 + } + return seed_count +} + +// An in-place quicksort of longs, for the keys. +sort_longs(keys: long[], count: int) { + if count < 2 { return } + quick_sort(keys, 0, count - 1) +} + +quick_sort(keys: long[], low: int, high: int) { + while low < high { + if high - low < 16 { + i = low + 1 + while i <= high { + v = keys[i] + j = i - 1 + while j >= low && keys[j] > v { + keys[j + 1] = keys[j] + j = j - 1 + } + keys[j + 1] = v + i = i + 1 + } + return + } + mid = low + (high - low) / 2 + pivot = keys[mid] + i = low + j = high + while i <= j { + while keys[i] < pivot { i = i + 1 } + while keys[j] > pivot { j = j - 1 } + if i <= j { + swap = keys[i] + keys[i] = keys[j] + keys[j] = swap + i = i + 1 + j = j - 1 + } + } + // The smaller side recurses, the larger loops: the stack stays logarithmic. + if j - low < high - i { + if low < j { quick_sort(keys, low, j) } + low = i + } else { + if i < high { quick_sort(keys, i, high) } + high = j + } + } +} + +// The new pairs since the last update, as sorted keys in the broad +// phase (read them with pair_key_count and pair_key; a key names two +// shapes and a compound child). Nothing is found when nothing moved. The +// static tree's moved flags are cleared, the dynamic and kinematic trees +// rebuilt where stale. Returns how many keys. +update_broad_phase_pairs(bp: *BroadPhase, visitors: PairVisitors, has_compounds: bool) -> int { + bp.pair_key_count = 0 + static_tree = broad_phase_tree(bp, BODY_STATIC) + kinematic_tree = broad_phase_tree(bp, BODY_KINEMATIC) + dynamic_tree_ = broad_phase_tree(bp, BODY_DYNAMIC) + need_update = dynamic_tree.tree_has_moved(static_tree) || dynamic_tree.tree_needs_rebuild(kinematic_tree) || + dynamic_tree.tree_needs_rebuild(dynamic_tree_) + if need_update == false { return 0 } + + // The dynamic tree's moved sibling pairs. + pair_capacity = math.max_int(dynamic_tree_.node_end / 2, 1) + if pair_capacity > bp.moved_capacity { + core.free_bytes(bp.moved_siblings, bp.moved_capacity * 4) + bp.moved_capacity = pair_capacity + bp.moved_siblings = core.alloc(pair_capacity * 4) + } + siblings = bp.moved_siblings as int[] + dynamic_move_count = gather_moved_siblings(dynamic_tree_, siblings) + + // The seeds against the static and kinematic trees. + seeds = bp.seeds as int[] + static_seed_count = gather_cross_seeds(bp, dynamic_tree_, static_tree, seeds, 0) + kinematic_seed_count = gather_cross_seeds(bp, dynamic_tree_, kinematic_tree, seeds, static_seed_count) + + ctx = PairContext { bp: bp, visitors: visitors, check_compounds: has_compounds, stack_count: 0 } + i = 0 + while i < static_seed_count { + collide_cross_pairs(&ctx, dynamic_tree_, BODY_DYNAMIC, static_tree, BODY_STATIC, seeds[2 * i], seeds[2 * i + 1]) + i = i + 1 + } + ctx.check_compounds = false + i = 0 + while i < kinematic_seed_count { + k = static_seed_count + i + collide_cross_pairs(&ctx, dynamic_tree_, BODY_DYNAMIC, kinematic_tree, BODY_KINEMATIC, seeds[2 * k], seeds[2 * k + 1]) + i = i + 1 + } + // The dynamic tree against itself. + i = 0 + while i < dynamic_move_count { + collide_cross_pairs(&ctx, dynamic_tree_, BODY_DYNAMIC, dynamic_tree_, BODY_DYNAMIC, siblings[i], siblings[i] + 1) + i = i + 1 + } + dynamic_tree.tree_clear_moved(static_tree) + + // The stale trees, which the reference rebuilds beside the narrow phase. + dynamic_tree.tree_rebuild(dynamic_tree_, false) + dynamic_tree.tree_rebuild(kinematic_tree, false) + + // Sorted, the contacts come out in an order the trees cannot change; + // a duplicate would be a bug in the walk, and is dropped. + keys = bp.pair_keys as long[] + sort_longs(keys, bp.pair_key_count) + kept = 0 + i = 0 + while i < bp.pair_key_count { + if i == 0 || keys[i] != keys[i - 1] { + keys[kept] = keys[i] + kept = kept + 1 + } + i = i + 1 + } + bp.pair_key_count = kept + return kept +} + +validate_broad_phase(bp: *BroadPhase) -> bool { + return dynamic_tree.tree_validate(broad_phase_tree(bp, BODY_DYNAMIC)) && dynamic_tree.tree_validate(broad_phase_tree(bp, BODY_KINEMATIC)) +} + +validate_no_moved(bp: *BroadPhase) -> bool { + i = 0 + while i < BODY_TYPE_COUNT { + if dynamic_tree.tree_validate_no_moved(broad_phase_tree(bp, i)) == false { return false } + i = i + 1 + } + return true +} diff --git a/aephysics/core/module.ae b/aephysics/core/module.ae index 0800748..cea2858 100644 --- a/aephysics/core/module.ae +++ b/aephysics/core/module.ae @@ -565,14 +565,24 @@ const MAX_CHILD_SHAPES = 1048576 // integers and xor alone collides. The murmur3 finaliser, with logical // shifts written out. The low 32 bits are the hash; zero is the empty // slot's mark, so a zero hash is nudged to one. +// A 32-bit mix, overflow-free: the 27-bit constant 0x45d9f3b keeps every +// product under 2^59 (a signed long overflow is undefined in the C +// underneath, and gcc at -O2 made two inlined copies of a 64-bit mixer +// disagree, so a key stored by one was not found by the other). +mix32(value: long) -> long { + x = value & (4294967295 as long) + x = ((x ^ lsr(x, 16)) * (73244475 as long)) & (4294967295 as long) + x = ((x ^ lsr(x, 16)) * (73244475 as long)) & (4294967295 as long) + return x ^ lsr(x, 16) +} + +// A 32-bit hash of a long, never zero (zero marks an empty slot): the +// low half mixed, the high half folded in and mixed again, so a pair of +// small ints packed in the two halves hashes by both and by their order. key_hash(key: long) -> int { - h = key - h = h ^ lsr(h, 33) - h = h * (0 - 48043033244054079 as long) // 0xff51afd7ed558ccd - h = h ^ lsr(h, 33) - h = h * (0 - 4265267296055464877 as long) // 0xc4ceb9fe1a85ec53 - h = h ^ lsr(h, 33) - hash = (h & 4294967295) as int + x = mix32(key) + x = mix32(x ^ (lsr(key, 32) & (4294967295 as long))) + hash = x as int if hash == 0 { hash = 1 } return hash } diff --git a/aephysics/test_broad_phase.ae b/aephysics/test_broad_phase.ae new file mode 100644 index 0000000..ebe5d2e --- /dev/null +++ b/aephysics/test_broad_phase.ae @@ -0,0 +1,384 @@ +// aephysics.broad_phase: the reference has no test of its own for the +// broad phase (its world tests cover it), so this one is ours: proxies +// keyed by type, the pairs found on a grid of dynamic boxes over a +// static ground against a brute force over the boxes, the pair set +// hiding pairs that have a contact and the update finding nothing when +// nothing moved, moves finding only the new pairs and a destroyed proxy +// finding none, the kinematic tree, the filter visitor, a static proxy +// forced into pairs, a compound's pairs one per child, the sort, and +// the keys' layout. + +import std.string +import aephysics.math +import aephysics.core +import aephysics.dynamic_tree +import aephysics.hull +import aephysics.distance +import aephysics.manifold +import aephysics.mesh +import aephysics.material +import aephysics.sphere +import aephysics.capsule +import aephysics.compound +import aephysics.broad_phase + +extern calloc(count: int, size: int) -> ptr +extern exit(code: int) +extern free(p: ptr) + +var failures = 0 +var checks = 0 + +ensure(name: string, ok: bool) { + checks = checks + 1 + if !ok { + println("broad_phase: FAIL ${name}") + failures = failures + 1 + } +} + +all_bits() -> long { return (0 as long) - (1 as long) } + +// The scene the visitors see: every shape's box and body, the odd ones +// filtered out when asked, and one compound. +struct Scene { + boxes: ptr // AABB[] + bodies: ptr // int[]: a body per shape (shapes on one body never pair) + keys: ptr // int[]: the proxy keys + count: int + filter_odd: bool + compound_shape: int // or -1 + compound_tree: ptr + compound_transform: Transform + should_calls: int +} + +should_pair(a: int, b: int, context: ptr) -> bool { + scene = context as *Scene + scene.should_calls = scene.should_calls + 1 + bodies = scene.bodies as int[] + if bodies[a] == bodies[b] { return false } + if scene.filter_odd && ((a & 1) == 1 || (b & 1) == 1) { return false } + return true +} + +compound_tree_of(shape: int, context: ptr) -> ptr { + scene = context as *Scene + if shape == scene.compound_shape { return scene.compound_tree } + return null +} + +compound_transform_of(shape: int, context: ptr) -> Transform { + scene = context as *Scene + return scene.compound_transform +} + +visitors(scene: *Scene) -> PairVisitors { + return PairVisitors { should_pair: should_pair, compound_tree_of: compound_tree_of, compound_transform: compound_transform_of, context: scene as ptr } +} + +// The pairs a brute force finds among the boxes whose visitor passes, +// leaving out the ones the set already holds. +brute_force_count(scene: *Scene, bp: *BroadPhase) -> int { + boxes = scene.boxes as AABB[] + bodies = scene.bodies as int[] + keys = scene.keys as int[] + count = 0 + i = 0 + while i < scene.count { + j = i + 1 + while j < scene.count { + kind_i = broad_phase.proxy_type(keys[i]) + kind_j = broad_phase.proxy_type(keys[j]) + // Two static or two kinematic proxies never pair; nor static with kinematic. + movable = kind_i == broad_phase.BODY_DYNAMIC || kind_j == broad_phase.BODY_DYNAMIC + if movable && math.aabb_overlaps(boxes[i], boxes[j]) && bodies[i] != bodies[j] { + odd = scene.filter_odd && ((i & 1) == 1 || (j & 1) == 1) + if odd == false && broad_phase.has_pair(bp, core.shape_pair_key(i, j, 0)) == false { count = count + 1 } + } + j = j + 1 + } + i = i + 1 + } + return count +} + +// Every key the update found is a real overlap of two passing shapes. +keys_are_overlaps(scene: *Scene, bp: *BroadPhase) -> bool { + boxes = scene.boxes as AABB[] + bodies = scene.bodies as int[] + n = broad_phase.pair_key_count(bp) + i = 0 + while i < n { + key = broad_phase.pair_key(bp, i) + a = broad_phase.pair_key_shape_a(key) + b = broad_phase.pair_key_shape_b(key) + if a >= b { return false } + if math.aabb_overlaps(boxes[a], boxes[b]) == false { return false } + if bodies[a] == bodies[b] { return false } + if i > 0 && key <= broad_phase.pair_key(bp, i - 1) { return false } + i = i + 1 + } + return true +} + +// The keys become contacts: into the pair set. +adopt_keys(bp: *BroadPhase) { + n = broad_phase.pair_key_count(bp) + i = 0 + while i < n { + broad_phase.add_pair(bp, broad_phase.pair_key(bp, i)) + i = i + 1 + } +} + +box_at(center: Vec3, half: float) -> AABB { + h = math.vec3(half, half, half) + return AABB { lower: math.sub(center, h), upper: math.add(center, h) } +} + +test_keys() { + ensure("proxy key layout", broad_phase.proxy_type(broad_phase.proxy_key(37, 2)) == 2 && broad_phase.proxy_id(broad_phase.proxy_key(37, 2)) == 37) + key = core.shape_pair_key(5, 3, 7) + ensure("pair key orders the shapes", broad_phase.pair_key_shape_a(key) == 3 && broad_phase.pair_key_shape_b(key) == 5 && broad_phase.pair_key_child(key) == 7) + ensure("pair key is symmetric", core.shape_pair_key(3, 5, 7) == key) + ensure("child distinguishes keys", core.shape_pair_key(3, 5, 8) != key) + // The sort: a scrambled array of longs, with duplicates. + block = calloc(1000, 8) + values = block as long[] + i = 0 + while i < 1000 { + values[i] = (((i * 7919) % 613) as long) * (1000000007 as long) + ((i % 3) as long) + i = i + 1 + } + broad_phase.sort_longs(values, 1000) + sorted = true + i = 1 + while i < 1000 { + if values[i] < values[i - 1] { sorted = false } + i = i + 1 + } + ensure("sort_longs sorts", sorted) + broad_phase.sort_longs(values, 1) + broad_phase.sort_longs(values, 0) + free(block) +} + +test_grid() { + bp_block = calloc(1, sizeof(BroadPhase)) + bp = bp_block as *BroadPhase + broad_phase.create_broad_phase(bp, Capacity { static_shape_count: 4, dynamic_shape_count: 64, contact_count: 64 }) + // A 6 x 6 grid of dynamic unit boxes at 1.5 spacing (no overlaps yet), + // a static ground under them, and a kinematic platform through the + // first row. + count = 38 + scene_block = calloc(1, sizeof(Scene)) + scene = scene_block as *Scene + scene.boxes = calloc(count, sizeof(AABB)) + scene.bodies = calloc(count, 4) + scene.keys = calloc(count, 4) + scene.count = count + scene.compound_shape = 0 - 1 + boxes = scene.boxes as AABB[] + bodies = scene.bodies as int[] + keys = scene.keys as int[] + i = 0 + while i < 36 { + boxes[i] = box_at(math.vec3(1.5 * ((i % 6) as float), 1.0, 1.5 * ((i / 6) as float)), 0.5) + bodies[i] = i + keys[i] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_DYNAMIC, boxes[i], all_bits(), i, false) + i = i + 1 + } + boxes[36] = AABB { lower: math.vec3(0.0 - 2.0, 0.0 - 1.0, 0.0 - 2.0), upper: math.vec3(10.0, 0.6, 10.0) } + bodies[36] = 36 + keys[36] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_STATIC, boxes[36], all_bits(), 36, false) + boxes[37] = AABB { lower: math.vec3(0.0 - 2.0, 0.8, 0.0 - 0.6), upper: math.vec3(10.0, 1.2, 0.0 - 0.4) } + bodies[37] = 37 + keys[37] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_KINEMATIC, boxes[37], all_bits(), 37, false) + ensure("proxy kinds", broad_phase.proxy_type(keys[0]) == broad_phase.BODY_DYNAMIC && broad_phase.proxy_type(keys[36]) == broad_phase.BODY_STATIC && broad_phase.proxy_type(keys[37]) == broad_phase.BODY_KINEMATIC) + ensure("shape index round trip", broad_phase.broad_phase_get_shape_index(bp, keys[36]) == 36 && broad_phase.broad_phase_get_shape_index(bp, keys[17]) == 17) + ensure("test overlap", broad_phase.broad_phase_test_overlap(bp, keys[0], keys[36]) && broad_phase.broad_phase_test_overlap(bp, keys[0], keys[1]) == false) + ensure("trees valid", broad_phase.validate_broad_phase(bp)) + + // The first update: every box on the ground (36), the first row on the platform (6). + expected = brute_force_count(scene, bp) + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), false) + ensure("first update count (${n} of ${expected})", n == expected && n == 42) + ensure("first update keys are overlaps", keys_are_overlaps(scene, bp)) + ensure("the visitor was asked for each (${scene.should_calls})", scene.should_calls == 42) + ensure("no moved flags after the update", broad_phase.validate_no_moved(bp)) + ensure("trees valid after the update", broad_phase.validate_broad_phase(bp)) + // The keys become contacts (an update clears the last keys, so they + // are taken before the next one). + adopt_keys(bp) + ensure("pair set holds them", broad_phase.pair_count(bp) == 42 && broad_phase.has_pair(bp, core.shape_pair_key(0, 36, 0))) + // Nothing moved: nothing to find. + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), false) + ensure("quiet update", n == 0) + // A proxy moved onto its neighbour reports only the new pair, not the ones it has. + boxes[8] = box_at(math.vec3(1.5 * 2.0 - 0.7, 1.0, 1.5), 0.5) + broad_phase.broad_phase_move_proxy(bp, keys[8], boxes[8]) + expected = brute_force_count(scene, bp) + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), false) + ensure("move finds the new pair (${n} of ${expected})", n == expected && n == 1) + ensure("the new pair", broad_phase.pair_key_shape_a(broad_phase.pair_key(bp, 0)) == 7 && broad_phase.pair_key_shape_b(broad_phase.pair_key(bp, 0)) == 8) + adopt_keys(bp) + // Marking a proxy moved without moving it finds nothing new. + broad_phase.broad_phase_mark_proxy_moved(bp, keys[8], boxes[8]) + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), false) + ensure("a mark alone finds nothing", n == 0) + // A contact ends: its pair leaves the set, and the next move finds it again. + broad_phase.remove_pair(bp, core.shape_pair_key(7, 8, 0)) + broad_phase.broad_phase_move_proxy(bp, keys[7], boxes[7]) + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), false) + ensure("a removed pair is found again", n == 1 && broad_phase.pair_key_shape_a(broad_phase.pair_key(bp, 0)) == 7) + adopt_keys(bp) + // Several move at once, some onto each other, some onto the platform. + i = 12 + while i < 18 { + boxes[i] = box_at(math.vec3(0.7 * ((i - 12) as float), 1.0, 0.0 - 0.3), 0.5) + broad_phase.broad_phase_move_proxy(bp, keys[i], boxes[i]) + i = i + 1 + } + expected = brute_force_count(scene, bp) + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), false) + ensure("a row moved (${n} of ${expected})", n == expected && n > 5) + ensure("a row moved: keys are overlaps", keys_are_overlaps(scene, bp)) + adopt_keys(bp) + // The filter: with the odd shapes refused, a move of an even one onto an odd finds nothing. + scene.filter_odd = true + boxes[20] = box_at(math.vec3(1.5 * 3.0 - 0.7, 1.0, 1.5 * 3.0), 0.5) + broad_phase.broad_phase_move_proxy(bp, keys[20], boxes[20]) + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), false) + ensure("the filter refuses", n == 0) + scene.filter_odd = false + broad_phase.broad_phase_move_proxy(bp, keys[20], boxes[20]) + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), false) + ensure("the same move without the filter", n == 1 && broad_phase.pair_key_shape_a(broad_phase.pair_key(bp, 0)) == 20 && broad_phase.pair_key_shape_b(broad_phase.pair_key(bp, 0)) == 21) + adopt_keys(bp) + // Shapes on one body never pair. + bodies[21] = 22 + bodies[22] = 22 + boxes[21] = box_at(math.vec3(1.5 * 4.0 - 0.7, 1.0, 1.5 * 3.0), 0.5) + broad_phase.broad_phase_move_proxy(bp, keys[21], boxes[21]) + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), false) + ensure("one body's shapes do not pair", n == 0) + // A static proxy forced into pairs finds the dynamic boxes over it. + boxes[36] = AABB { lower: math.vec3(0.0 - 2.0, 0.0 - 1.0, 0.0 - 2.0), upper: math.vec3(10.0, 0.6, 10.0) } + broad_phase.remove_pair(bp, core.shape_pair_key(0, 36, 0)) + broad_phase.remove_pair(bp, core.shape_pair_key(1, 36, 0)) + broad_phase.broad_phase_destroy_proxy(bp, keys[36]) + keys[36] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_STATIC, boxes[36], all_bits(), 36, true) + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), false) + ensure("a forced static proxy pairs (${n})", n == 2) + adopt_keys(bp) + // A static proxy created without the force finds nothing until a dynamic one moves onto it. + broad_phase.remove_pair(bp, core.shape_pair_key(0, 36, 0)) + broad_phase.broad_phase_destroy_proxy(bp, keys[36]) + keys[36] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_STATIC, boxes[36], all_bits(), 36, false) + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), false) + ensure("a quiet static proxy", n == 0) + broad_phase.broad_phase_move_proxy(bp, keys[0], boxes[0]) + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), false) + ensure("then the mover finds it", n == 1 && broad_phase.pair_key_shape_b(broad_phase.pair_key(bp, 0)) == 36) + adopt_keys(bp) + // A destroyed proxy is found by no one. + broad_phase.broad_phase_destroy_proxy(bp, keys[7]) + keys[7] = 0 - 1 + boxes[7] = AABB { lower: math.vec3(100.0, 100.0, 100.0), upper: math.vec3(100.0, 100.0, 100.0) } + boxes[6] = box_at(math.vec3(1.5, 1.0, 1.5), 0.5) + broad_phase.broad_phase_move_proxy(bp, keys[6], boxes[6]) + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), false) + ensure("a destroyed proxy is not found", keys_are_overlaps(scene, bp)) + ensure("trees valid at the end", broad_phase.validate_broad_phase(bp)) + + broad_phase.destroy_broad_phase(bp) + free(scene.keys) + free(scene.bodies) + free(scene.boxes) + free(scene_block) + free(bp_block) +} + +test_compound_pairs() { + // A static compound of three spheres along x under one dynamic box: + // the pair comes out once per child the box's bounds overlap. + mat = material.default_surface_material() + spheres_block = calloc(3, sizeof(CompoundSphereDef)) + spheres = spheres_block as CompoundSphereDef[] + spheres[0] = compound.compound_sphere_def(manifold.sphere(math.vec3_zero(), 0.5), mat) + spheres[1] = compound.compound_sphere_def(manifold.sphere(math.vec3(3.0, 0.0, 0.0), 0.5), mat) + spheres[2] = compound.compound_sphere_def(manifold.sphere(math.vec3(6.0, 0.0, 0.0), 0.5), mat) + def = compound.compound_def() + def.spheres = spheres_block + def.sphere_count = 3 + c = compound.create_compound(&def) + + bp_block = calloc(1, sizeof(BroadPhase)) + bp = bp_block as *BroadPhase + broad_phase.create_broad_phase(bp, Capacity { static_shape_count: 4, dynamic_shape_count: 4, contact_count: 4 }) + scene_block = calloc(1, sizeof(Scene)) + scene = scene_block as *Scene + scene.boxes = calloc(2, sizeof(AABB)) + scene.bodies = calloc(2, 4) + scene.keys = calloc(2, 4) + scene.count = 2 + scene.compound_shape = 1 + scene.compound_tree = (&c.tree) as ptr + // The compound's body is moved up by 10 and turned a quarter about z: its local x is world y. + scene.compound_transform = Transform { p: math.vec3(0.0, 10.0, 0.0), q: math.make_quat_from_axis_angle(math.vec3_axis_z(), 0.5 * math.PI) } + boxes = scene.boxes as AABB[] + bodies = scene.bodies as int[] + keys = scene.keys as int[] + bodies[0] = 0 + bodies[1] = 1 + // The box straddles the children at local x 0 and 3 (world y 10 and 13), not the one at 6. + boxes[0] = AABB { lower: math.vec3(0.0 - 0.6, 9.5, 0.0 - 0.6), upper: math.vec3(0.6, 13.2, 0.6) } + boxes[1] = compound.compute_compound_aabb(c, scene.compound_transform) + keys[0] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_DYNAMIC, boxes[0], all_bits(), 0, false) + keys[1] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_STATIC, boxes[1], all_bits(), 1, false) + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), true) + ensure("compound pairs (${n})", n == 2) + if n == 2 { + k0 = broad_phase.pair_key(bp, 0) + k1 = broad_phase.pair_key(bp, 1) + ensure("compound pair shapes", broad_phase.pair_key_shape_a(k0) == 0 && broad_phase.pair_key_shape_b(k0) == 1) + ensure("compound pair children", broad_phase.pair_key_child(k0) == 0 && broad_phase.pair_key_child(k1) == 1) + } + adopt_keys(bp) + // Without the compound flag the pair is plain; with it and the pair set holding one child, only the other comes. + broad_phase.remove_pair(bp, core.shape_pair_key(0, 1, 1)) + broad_phase.broad_phase_move_proxy(bp, keys[0], boxes[0]) + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), true) + ensure("compound child already paired is skipped", n == 1 && broad_phase.pair_key_child(broad_phase.pair_key(bp, 0)) == 1) + // The gauntlet refusing the pair drops every child. + bodies[1] = 0 + broad_phase.remove_pair(bp, core.shape_pair_key(0, 1, 0)) + broad_phase.broad_phase_move_proxy(bp, keys[0], boxes[0]) + n = broad_phase.update_broad_phase_pairs(bp, visitors(scene), true) + ensure("compound pair refused drops every child", n == 0) + broad_phase.destroy_broad_phase(bp) + free(scene.keys) + free(scene.bodies) + free(scene.boxes) + free(scene_block) + free(bp_block) + compound.destroy_compound(c) + free(spheres_block) +} + +main() { + before = core.alloc_count() + test_keys() + test_grid() + test_compound_pairs() + ensure("every counted allocation was freed (${core.alloc_count() - before})", core.alloc_count() == before) + + println("broad_phase: ${checks} checks") + if failures == 0 { + println("broad_phase: all checks passed") + } else { + println("broad_phase: ${failures} failure(s)") + exit(1) + } +} diff --git a/bench/RESULTS.md b/bench/RESULTS.md index 81cbc35..235d9c7 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -201,8 +201,8 @@ binned SAH, edges identified each time; 100,000 rays cast down onto it; | phase | aephysics | Box3D | |---|---|---| -| 10 builds, median split (80,000 triangles) | 199 ms | **129** | -| 10 builds, binned SAH | 316 | **256** | +| 10 builds, median split (80,000 triangles) | 221 ms | **129** | +| 10 builds, binned SAH | 335 | **256** | | 100,000 ray casts | 20.8 | **10.2** | | 100,000 box queries | 70.4 | **39.5** | | 10,000 shape casts | 24.7 | **14.3** | @@ -322,3 +322,32 @@ velocities, 1,662,623 iterations here against 1,662,624 there (one convergence test on the float's side of the slop). The solver runs at 0.9x: the reference reads its planes through a pointer per pass where the loop here indexes the array. + +## broad_phase + +`bench/broad_phase.ae`: 10,000 dynamic unit boxes on a jittered 25 x 16 +x 25 grid at 1.4 spacing over a static ground, the first update finding +every pair, then 20 steps in which every box drifts a little and only +the new pairs come out, each update's keys adopted into the pair set. +The reference finds its pairs inside its world (broad_phase.c filters +through the shapes and creates the contacts itself), so it has no +free-standing counterpart; its pair update is measured against ours +through the world benchmarks once the world steps. + +| phase | aephysics | +|---|---| +| 10,000 proxies created | 3.5 ms | +| first update (2,900 pairs) | 2.2 | +| 20 steps of 10,000 moves and an update (757 new pairs) | 91 | + +A step is 4.6 ms, most of it the 10,000 proxy moves (a leaf removed and +re-inserted each); the update itself walks only the sibling pairs a +moved node touched. The test checks every update against a brute force +over the boxes. + +The hash under the pair set and every map (core's `key_hash`) was +rewritten during this layer: the previous 64-bit mixer multiplied +signed longs, which is undefined in the C underneath, and gcc at -O2 +made two inlined copies of it disagree, so a key stored by one copy was +not found by the other. The new mixer stays in 32-bit products; the +mesh builds above moved from 199 to 221 ms and 316 to 335 ms with it. diff --git a/bench/broad_phase.ae b/bench/broad_phase.ae new file mode 100644 index 0000000..5c138f2 --- /dev/null +++ b/bench/broad_phase.ae @@ -0,0 +1,96 @@ +// The broad phase on its own: 10,000 dynamic unit boxes on a jittered +// 25 x 16 x 25 grid at 1.4 spacing over a static ground (so a quarter +// of the neighbours touch), the first update finding every pair, then +// 20 steps in which every box drifts a little and only the new pairs +// come out, each update's keys adopted into the pair set. The reference +// finds its pairs inside its world (broad_phase.c calls into shapes and +// contacts), so it has no free-standing counterpart here; its pair +// update is measured against ours through the world benchmarks. Single +// thread, wall time per phase, with the pair counts as the checksum. +import std.string +import std.os +import aephysics.math +import aephysics.core +import aephysics.dynamic_tree +import aephysics.broad_phase + +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 COUNT = 10000 +const STEPS = 20 + +var g_bodies: ptr = null + +should_pair(a: int, b: int, context: ptr) -> bool { return a != b } +no_compound(shape: int, context: ptr) -> ptr { return null } +identity(shape: int, context: ptr) -> Transform { return math.transform_identity() } + +all_bits() -> long { return (0 as long) - (1 as long) } + +adopt(bp: *BroadPhase) { + n = broad_phase.pair_key_count(bp) + i = 0 + while i < n { + broad_phase.add_pair(bp, broad_phase.pair_key(bp, i)) + i = i + 1 + } +} + +main() { + bp_block = calloc(1, sizeof(BroadPhase)) + bp = bp_block as *BroadPhase + broad_phase.create_broad_phase(bp, Capacity { static_shape_count: 16, dynamic_shape_count: COUNT, contact_count: 4 * COUNT }) + boxes_block = calloc(COUNT + 1, sizeof(AABB)) + boxes = boxes_block as AABB[] + keys_block = calloc(COUNT + 1, 4) + keys = keys_block as int[] + visitors = PairVisitors { should_pair: should_pair, compound_tree_of: no_compound, compound_transform: identity, context: null } + + t0 = clock() + i = 0 + while i < COUNT { + x = (i % 25) as float + y = ((i / 25) % 16) as float + z = (i / 400) as float + jitter = 0.3 * sin(7.0 * (i as float)) + c = math.vec3(1.4 * x + jitter, 1.0 + 1.4 * y, 1.4 * z + 0.3 * cos(11.0 * (i as float))) + boxes[i] = AABB { lower: math.sub(c, math.vec3(0.5, 0.5, 0.5)), upper: math.add(c, math.vec3(0.5, 0.5, 0.5)) } + keys[i] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_DYNAMIC, boxes[i], all_bits(), i, false) + i = i + 1 + } + boxes[COUNT] = AABB { lower: math.vec3(0.0 - 5.0, 0.0 - 1.0, 0.0 - 5.0), upper: math.vec3(40.0, 0.6, 40.0) } + keys[COUNT] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_STATIC, boxes[COUNT], all_bits(), COUNT, false) + t1 = clock() + first = broad_phase.update_broad_phase_pairs(bp, visitors, false) + adopt(bp) + t2 = clock() + + found = 0 + step = 0 + while step < STEPS { + i = 0 + while i < COUNT { + d = math.vec3(0.05 * sin((i + step) as float), 0.05 * cos((3 * i + step) as float), 0.05 * sin((5 * i + 2 * step) as float)) + boxes[i] = AABB { lower: math.add(boxes[i].lower, d), upper: math.add(boxes[i].upper, d) } + broad_phase.broad_phase_move_proxy(bp, keys[i], boxes[i]) + i = i + 1 + } + n = broad_phase.update_broad_phase_pairs(bp, visitors, false) + found = found + n + adopt(bp) + step = step + 1 + } + t3 = clock() + + println("aephysics broad_phase: ${COUNT} proxies created ${ms(t1 - t0)} ms, first update ${ms(t2 - t1)} ms (${first} pairs), ${STEPS} steps of moves and updates ${ms(t3 - t2)} ms (${found} new pairs, ${broad_phase.pair_count(bp)} in the set)") + broad_phase.destroy_broad_phase(bp) + free(keys_block) + free(boxes_block) + free(bp_block) +} diff --git a/design.md b/design.md index 9148008..6f4141f 100644 --- a/design.md +++ b/design.md @@ -140,13 +140,16 @@ started until its tests pass. (the point culling and the per-cluster reduction are pure; the triangle cache it refreshes is the contact's, so the entry point takes the cache as a struct). - - `aephysics.broad_phase`: broad_phase.c's trees per body type, - proxies keyed by type in the low bits, the moved-sibling gathering, - the self and cross pair walks and the pair set; the pair filter and - the pair emission are visitors, since the reference does its shape - filtering and contact creation inside. Own test: pairs found and - not found across moves, a compound's children, the pair set's - persistence. + - `aephysics.broad_phase` (done): broad_phase.c's trees per body + type, proxies keyed by type in the low bits, the moved-sibling + gathering, the self and cross pair walks and the pair set; the pair + filter and the compound lookups are visitors, and the update leaves + sorted keys for the client to turn into contacts. 36 checks against + a brute force: pairs found and not found across moves, the filter, + a forced static proxy, a destroyed proxy, a compound's children. + Found and fixed on the way: core's key_hash multiplied signed + longs (undefined in C; gcc at -O2 made two inlined copies disagree) + -- it now mixes in 32-bit products. - `aephysics.dynamics`: one module for the world's state and its bookkeeping -- the World with its arrays (bodies, shapes, contacts, joints, islands, solver sets), the ids with generations, the diff --git a/scripts/bench.sh b/scripts/bench.sh index f436502..db71a72 100755 --- a/scripts/bench.sh +++ b/scripts/bench.sh @@ -10,10 +10,13 @@ cd "$root" mkdir -p target layers="${1:-tree}" for layer in $layers; do - gcc -O3 -Ireference/box3d/include "bench/${layer}_box3d.c" -o "target/${layer}_box3d" -Lreference/box3d/out/src -lbox3d -lm || exit 1 + # A layer the reference only has inside its world (the broad phase) runs ours alone. + if [ -f "bench/${layer}_box3d.c" ]; then + gcc -O3 -Ireference/box3d/include "bench/${layer}_box3d.c" -o "target/${layer}_box3d" -Lreference/box3d/out/src -lbox3d -lm || exit 1 + ref="target/${layer}_box3d"; [ -x "$ref" ] || ref="$ref.exe" + "$ref" + fi AETHER_LIB_DIR="$root" ae build "bench/$layer.ae" -o "target/$layer" >"target/$layer.log" 2>&1 || { cat "target/$layer.log"; exit 1; } - ref="target/${layer}_box3d"; [ -x "$ref" ] || ref="$ref.exe" ours="target/$layer"; [ -x "$ours" ] || ours="$ours.exe" - "$ref" "$ours" done