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