From 5ce4e84360092c589a81be61eeed000b570ca877 Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Fri, 9 Oct 2026 01:24:33 -0300 Subject: [PATCH 1/4] Broad phase: float tree boxes rounded outward, candidates batched with prefetch The dynamic tree's nodes and the world's fat boxes are single precision, narrowed outward from double (math.aabb_round_out, b3RoundDownFloat and b3RoundUpFloat): 32-byte nodes, the reference's. The insertion costs and the rays stay in double on the widened boxes; storage, refit, overlaps and queries are float. The golden trace is unchanged. The pairs phase's cost was the pair set's cache misses, not the box bytes: candidates are batched 32 at a time with every hash slot prefetched before any probe (b3FlushCandidatePairs), and the walk reads a leaf's shape index from the node instead of the proxy array. scripts/profile.sh passes ae cflags whole (0.795's -idirafter pairs). --- aephysics/basics/module.ae | 8 +- aephysics/broad_phase/module.ae | 90 ++++++++++++++++------ aephysics/compound/module.ae | 2 +- aephysics/dynamic_tree/module.ae | 123 ++++++++++++++++++------------- aephysics/dynamics/module.ae | 26 ++++--- aephysics/math/module.ae | 76 +++++++++++++++++++ aephysics/solver/module.ae | 12 +-- scripts/profile.sh | 5 +- tests/test_broad_phase.ae | 40 +++++----- tests/test_dynamic_tree.ae | 61 ++++++++++++++- tests/test_math.ae | 55 ++++++++++++++ tests/test_physics_world.ae | 2 +- 12 files changed, 384 insertions(+), 116 deletions(-) diff --git a/aephysics/basics/module.ae b/aephysics/basics/module.ae index 95fa70b..dd37fbe 100644 --- a/aephysics/basics/module.ae +++ b/aephysics/basics/module.ae @@ -31,7 +31,7 @@ exports ( idpool_create, idpool_destroy, idpool_alloc, idpool_free, idpool_count, idpool_capacity, key_hash, shape_pair_key, SHAPE_POWER, CHILD_POWER, MAX_SHAPES, MAX_CHILD_SHAPES, set_create, set_destroy, set_clear, set_add, set_remove, set_contains, set_contains_hashed, - set_count, set_bytes, + set_count, set_bytes, set_prefetch, LongMap, map_create, map_destroy, map_clear, map_set, map_get, map_has, map_remove, map_count, map_bytes, hash_bytes, section_fits, ints_create, ints_destroy, ints_reserve, ints_push, ints_pop, ints_get, ints_set, ints_clear, @@ -840,6 +840,12 @@ set_contains(s: *HashSet, key: long) -> bool { return items[index].key == key } +// The cache line of a hash's home slot, fetched ahead: a batch prefetches +// every key's slot before it probes any (the reference's b3PrefetchHash). +set_prefetch(s: *HashSet, hash: int) { + native.prefetch(s.items + (hash & (s.capacity - 1)) * sizeof(SetItem)) +} + // With the hash already in hand, for a batch that prefetches slots. set_contains_hashed(s: *HashSet, key: long, hash: int) -> bool { index = find_slot(s, key, hash) diff --git a/aephysics/broad_phase/module.ae b/aephysics/broad_phase/module.ae index 4d2305b..0749c6d 100644 --- a/aephysics/broad_phase/module.ae +++ b/aephysics/broad_phase/module.ae @@ -48,6 +48,9 @@ const BODY_DYNAMIC = 2 const BODY_TYPE_COUNT = 3 const CROSS_SEED_COUNT = 64 const STACK_SIZE = 512 +// Candidates gathered before the pair set is asked about any of them, so +// every slot is prefetched first (the reference's B3_CANDIDATE_BATCH). +const CANDIDATE_BATCH = 32 // What a world expects to hold, for the initial capacities. struct Capacity { @@ -68,6 +71,9 @@ struct BroadPhase { 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 + candidates: ptr // int[2 * CANDIDATE_BATCH]: the batch's shape pairs, the smaller first + candidate_keys: ptr // long[CANDIDATE_BATCH]: their keys + candidate_hashes: ptr // int[CANDIDATE_BATCH]: and the keys' hashes } // What the client answers about its shapes during an update: whether a @@ -107,6 +113,9 @@ create_broad_phase(bp: *BroadPhase, capacity: Capacity) { bp.pair_stack = basics.alloc(2 * STACK_SIZE * 4) bp.seeds = basics.alloc(4 * CROSS_SEED_COUNT * 4) bp.queue = basics.alloc(4 * CROSS_SEED_COUNT * 4) + bp.candidates = basics.alloc(2 * CANDIDATE_BATCH * 4) + bp.candidate_keys = basics.alloc(CANDIDATE_BATCH * 8) + bp.candidate_hashes = basics.alloc(CANDIDATE_BATCH * 4) } copy_tree(dst: *DynamicTree, src: DynamicTree) { @@ -144,12 +153,16 @@ destroy_broad_phase(bp: *BroadPhase) { basics.free_bytes(bp.pair_stack, 2 * STACK_SIZE * 4) basics.free_bytes(bp.seeds, 4 * CROSS_SEED_COUNT * 4) basics.free_bytes(bp.queue, 4 * CROSS_SEED_COUNT * 4) + basics.free_bytes(bp.candidates, 2 * CANDIDATE_BATCH * 4) + basics.free_bytes(bp.candidate_keys, CANDIDATE_BATCH * 8) + basics.free_bytes(bp.candidate_hashes, CANDIDATE_BATCH * 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 { +// A shape's proxy in the tree of its body's type, at its fat box (single +// precision, rounded outward, #151). 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: AABBf, 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) @@ -160,7 +173,7 @@ broad_phase_destroy_proxy(bp: *BroadPhase, key: int) { } // The proxy re-inserted at a new box and marked moved. -broad_phase_move_proxy(bp: *BroadPhase, key: int, box: AABB) { +broad_phase_move_proxy(bp: *BroadPhase, key: int, box: AABBf) { dynamic_tree.tree_move_proxy_internal(broad_phase_tree(bp, proxy_type(key)), proxy_id(key), box, true) } @@ -168,7 +181,7 @@ 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) { +broad_phase_mark_proxy_moved(bp: *BroadPhase, key: int, box: AABBf) { dynamic_tree.tree_mark_proxy_moved(broad_phase_tree(bp, proxy_type(key)), proxy_id(key), box) } @@ -176,12 +189,12 @@ 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_get_aabb(bp: *BroadPhase, key: int) -> AABBf { + return dynamic_tree.tree_get_aabbf(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)) + return math.aabbf_overlaps(broad_phase_get_aabb(bp, key_a), broad_phase_get_aabb(bp, key_b)) } // --- the pair set and the keys ------------------------------------------------------------------- @@ -226,6 +239,7 @@ struct PairContext { visitors: PairVisitors check_compounds: bool // only the static cross pass can meet a compound stack_count: int + batch_count: int // candidates waiting in bp.candidates } // One of the two is a compound: the pair becomes one per child whose box @@ -249,7 +263,7 @@ emit_compound_pairs(ctx: *PairContext, compound_shape: int, other_shape: int, ot 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) + other_box = math.aabb_widen(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 @@ -276,27 +290,59 @@ add_candidate_pair(ctx: *PairContext, shape_a: int, shape_b: int, key_a: int, ke return } } - key = basics.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) + batch = bp.candidates as int[] + n = ctx.batch_count + batch[2 * n] = math.min_int(shape_a, shape_b) + batch[2 * n + 1] = math.max_int(shape_a, shape_b) + ctx.batch_count = n + 1 + if n + 1 == CANDIDATE_BATCH { flush_candidates(ctx) } +} + +// The batch against the pair set: every key's slot prefetched, then the +// pairs already held culled, then the visitor's gauntlet, the survivors +// kept as keys (b3FlushCandidatePairs). The pair set is a table larger +// than the caches in a big pile, so asking key by key waited on a miss +// for each; the keys end sorted, so the batch changes no result. +flush_candidates(ctx: *PairContext) { + bp = ctx.bp + visitors = ctx.visitors + n = ctx.batch_count + ctx.batch_count = 0 + batch = bp.candidates as int[] + keys = bp.candidate_keys as long[] + hashes = bp.candidate_hashes as int[] + i = 0 + while i < n { + keys[i] = basics.shape_pair_key(batch[2 * i], batch[2 * i + 1], 0) + hashes[i] = basics.key_hash(keys[i]) + basics.set_prefetch(&bp.pair_set, hashes[i]) + i = i + 1 + } + i = 0 + while i < n { + if basics.set_contains_hashed(&bp.pair_set, keys[i], hashes[i]) == false { + if visitors.should_pair(batch[2 * i], batch[2 * i + 1], visitors.context) { push_key(bp, keys[i]) } + } + i = i + 1 } } // Did either move, and if so do they overlap? test_pair(a: DynamicTreeNode, b: DynamicTreeNode) -> bool { if ((a.flag_index | b.flag_index) & dynamic_tree.MOVED_NODE) == 0 { return false } - return math.aabb_overlaps(a.aabb, b.aabb) + return math.aabbf_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: DynamicTreeNode, leaf_key: int, tree: *DynamicTree, tree_kind: int, pair: int) { nodes = tree.nodes as DynamicTreeNode[] - 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) + // A leaf keeps its proxy's user data, the shape index, in its height: + // read from the node in hand, not the proxy array (one miss fewer a + // candidate; the reference's nodes hold the shape index the same way). + shape_id = leaf.height stack = ctx.bp.stack as int[] stack_count = 1 stack[0] = pair @@ -309,10 +355,10 @@ collide_proxy_and_subtree(ctx: *PairContext, leaf: DynamicTreeNode, leaf_key: in 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 math.aabbf_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)) + add_candidate_pair(ctx, shape_id, nodes[index].height, 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 @@ -332,12 +378,9 @@ visit_pair(ctx: *PairContext, tree_a: *DynamicTree, kind_a: int, tree_b: *Dynami 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)) + add_candidate_pair(ctx, node_a.height, node_b.height, 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 { @@ -510,7 +553,7 @@ update_broad_phase_pairs(bp: *BroadPhase, visitors: PairVisitors, has_compounds: 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 } + ctx = PairContext { bp: bp, visitors: visitors, check_compounds: has_compounds, stack_count: 0, batch_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]) @@ -529,6 +572,7 @@ update_broad_phase_pairs(bp: *BroadPhase, visitors: PairVisitors, has_compounds: collide_cross_pairs(&ctx, dynamic_tree_, BODY_DYNAMIC, dynamic_tree_, BODY_DYNAMIC, siblings[i], siblings[i] + 1) i = i + 1 } + flush_candidates(&ctx) dynamic_tree.tree_clear_moved(static_tree) // The stale trees, which the reference rebuilds beside the narrow phase. diff --git a/aephysics/compound/module.ae b/aephysics/compound/module.ae index 11a6b6d..2e15dc7 100644 --- a/aephysics/compound/module.ae +++ b/aephysics/compound/module.ae @@ -48,7 +48,7 @@ extern memcpy(dst: ptr, src: ptr, size: int) -> ptr extern memcmp(a: ptr, b: ptr, size: int) -> int const NULL_INDEX = 0 - 1 -const COMPOUND_VERSION = 0x5C0E7B9B // a mesh child's materials became a table (Box3D #172) +const COMPOUND_VERSION = 0x5C0E7B9C // the baked tree's nodes in single precision (#151); before, a mesh child's materials became a table (Box3D #172) // Kept for the API; a mesh child is no longer limited to it. const MAX_COMPOUND_MESH_MATERIALS = 4 // The child index shares a 64-bit key with two shape ids in the reference's tables. diff --git a/aephysics/dynamic_tree/module.ae b/aephysics/dynamic_tree/module.ae index 56463e5..85748ed 100644 --- a/aephysics/dynamic_tree/module.ae +++ b/aephysics/dynamic_tree/module.ae @@ -34,7 +34,7 @@ exports ( tree_query, tree_query_closest, tree_ray_cast, tree_box_cast, tree_get_height, tree_get_area_ratio, tree_get_root_bounds, tree_get_proxy_count, tree_rebuild, tree_get_byte_count, tree_validate, tree_validate_no_moved, - tree_get_user_data, tree_get_aabb, + tree_get_user_data, tree_get_aabb, tree_get_aabbf, tree_enlarge_proxy_internal, tree_create_proxy_internal, tree_move_proxy_internal, tree_mark_proxy_moved_serial, tree_mark_proxy_moved, tree_clear_moved, tree_gather_moved_proxies, tree_refit, tree_has_moved, tree_needs_rebuild, test_category, test_bounds_ray_overlap, @@ -68,7 +68,7 @@ const COPY_ITEM_INTS = 2 // A node. Siblings sit together at an even index. For a leaf, `height` // holds the shape index, truncated from the proxy's user data. struct DynamicTreeNode { - aabb: AABB + aabb: AABBf // in single precision, rounded outward (#151): 32 bytes a node, the reference's flag_index: int height: int } @@ -140,11 +140,13 @@ node_height(n: DynamicTreeNode) -> int { // float limits were not enough: a query box out to them overlapped, and the // empty node's proxy index read far past the proxies. make_empty_node() -> DynamicTreeNode { - inf = math.infinity() - return DynamicTreeNode { aabb: AABB { lower: Vec3 { x: inf, y: inf, z: inf }, upper: Vec3 { x: 0.0 - inf, y: 0.0 - inf, z: 0.0 - inf } }, - flag_index: EMPTY_NODE, height: 0 } + return DynamicTreeNode { aabb: math.aabbf_empty(), flag_index: EMPTY_NODE, height: 0 } } +// A node's box in double, exactly: the insertion's costs and the rays are +// measured on it, so only the storage is single precision. +wide(n: DynamicTreeNode) -> AABB { return math.aabb_widen(n.aabb) } + // The pair below the root, or the root itself when it is a leaf. root_pair(nodes: DynamicTreeNode[]) -> int { root = nodes[ROOT_NODE] @@ -158,13 +160,13 @@ make_internal_node(nodes: DynamicTreeNode[], pair: int) -> DynamicTreeNode { c1 = nodes[pair] c2 = nodes[pair + 1] return DynamicTreeNode { - aabb: math.aabb_union(c1.aabb, c2.aabb), + aabb: math.aabbf_union(c1.aabb, c2.aabb), flag_index: pair | ((c1.flag_index | c2.flag_index) & MOVED_NODE), height: 1 + math.max_int(node_height(c1), node_height(c2)) } } -make_leaf_node(box: AABB, proxy_id: int, user_data: long, moved: bool) -> DynamicTreeNode { +make_leaf_node(box: AABBf, proxy_id: int, user_data: long, moved: bool) -> DynamicTreeNode { flags = proxy_id | LEAF_NODE if moved { flags = flags | MOVED_NODE } return DynamicTreeNode { aabb: box, flag_index: flags, height: user_data as int } @@ -361,14 +363,15 @@ free_proxy(t: *DynamicTree, proxy_id: int) { // or D beside or below B, or beside or below C. The cost of a sibling is // the area of its union with D plus the growth of its ancestors; for an // internal child the lower bound of going below it prunes the descent. -find_best_sibling(t: *DynamicTree, box_d: AABB) -> int { +find_best_sibling(t: *DynamicTree, leaf_box: AABBf) -> int { + box_d = math.aabb_widen(leaf_box) center_d = math.aabb_center(box_d) area_d = math.aabb_area(box_d) nodes = t.nodes as DynamicTreeNode[] node_index = ROOT_NODE if is_leaf(nodes[node_index].flag_index) { return node_index } - root_box = nodes[node_index].aabb + root_box = wide(nodes[node_index]) area_base = math.aabb_area(root_box) direct_cost = math.aabb_area(math.aabb_union(root_box, box_d)) inherited_cost = 0.0 @@ -391,7 +394,7 @@ find_best_sibling(t: *DynamicTree, box_d: AABB) -> int { leaf2 = is_leaf(nodes[child2].flag_index) lower_cost1 = math.MAX_FLOAT - box1 = nodes[child1].aabb + box1 = wide(nodes[child1]) direct_cost1 = math.aabb_area(math.aabb_union(box1, box_d)) area1 = 0.0 if leaf1 { @@ -408,7 +411,7 @@ find_best_sibling(t: *DynamicTree, box_d: AABB) -> int { } lower_cost2 = math.MAX_FLOAT - box2 = nodes[child2].aabb + box2 = wide(nodes[child2]) direct_cost2 = math.aabb_area(math.aabb_union(box2, box_d)) area2 = 0.0 if leaf2 { @@ -483,14 +486,14 @@ rotate_nodes(t: *DynamicTree, i_a: int) { // Swap B with F or G. i_f = left_child(c.flag_index) i_g = i_f + 1 - area_c = math.aabb_area(c.aabb) - delta_bf = math.aabb_area(math.aabb_union(b.aabb, nodes[i_g].aabb)) - area_c + area_c = math.aabb_area(wide(c)) + delta_bf = math.aabb_area(math.aabb_union(wide(b), wide(nodes[i_g]))) - area_c if delta_bf < best_delta { best_down = i_b best_up = i_f best_delta = delta_bf } - delta_bg = math.aabb_area(math.aabb_union(b.aabb, nodes[i_f].aabb)) - area_c + delta_bg = math.aabb_area(math.aabb_union(wide(b), wide(nodes[i_f]))) - area_c if delta_bg < best_delta { best_down = i_b best_up = i_g @@ -502,14 +505,14 @@ rotate_nodes(t: *DynamicTree, i_a: int) { // Swap C with D or E. i_d = left_child(b.flag_index) i_e = i_d + 1 - area_b = math.aabb_area(b.aabb) - delta_cd = math.aabb_area(math.aabb_union(c.aabb, nodes[i_e].aabb)) - area_b + area_b = math.aabb_area(wide(b)) + delta_cd = math.aabb_area(math.aabb_union(wide(c), wide(nodes[i_e]))) - area_b if delta_cd < best_delta { best_down = i_c best_up = i_d best_delta = delta_cd } - delta_ce = math.aabb_area(math.aabb_union(c.aabb, nodes[i_d].aabb)) - area_b + delta_ce = math.aabb_area(math.aabb_union(wide(c), wide(nodes[i_d]))) - area_b if delta_ce < best_delta { best_down = i_c best_up = i_e @@ -520,7 +523,7 @@ rotate_nodes(t: *DynamicTree, i_a: int) { if best_down != NULL_INDEX { swap_nodes(t, best_down, best_up) } } -insert_leaf(t: *DynamicTree, box: AABB, proxy_id: int, moved: bool, should_rotate: bool) { +insert_leaf(t: *DynamicTree, box: AABBf, proxy_id: int, moved: bool, should_rotate: bool) { proxies = t.proxies as TreeProxy[] leaf = make_leaf_node(box, proxy_id, proxies[proxy_id].user_data, moved) nodes = t.nodes as DynamicTreeNode[] @@ -585,7 +588,7 @@ remove_leaf(t: *DynamicTree, proxy_id: int) { // --- proxies ------------------------------------------------------------------- -tree_create_proxy_internal(t: *DynamicTree, box: AABB, category_bits: long, user_data: long, mark_moved: bool) -> int { +tree_create_proxy_internal(t: *DynamicTree, box: AABBf, category_bits: long, user_data: long, mark_moved: bool) -> int { proxy_id = allocate_proxy(t) proxies = t.proxies as TreeProxy[] proxies[proxy_id].category_bits = category_bits @@ -594,8 +597,10 @@ tree_create_proxy_internal(t: *DynamicTree, box: AABB, category_bits: long, user return proxy_id } +// The box is narrowed outward to the node's single precision: the leaf +// holds it whatever its distance from the origin. tree_create_proxy(t: *DynamicTree, box: AABB, category_bits: long, user_data: long) -> int { - return tree_create_proxy_internal(t, box, category_bits, user_data, false) + return tree_create_proxy_internal(t, math.aabb_round_out(box), category_bits, user_data, false) } tree_destroy_proxy(t: *DynamicTree, proxy_id: int) { @@ -606,18 +611,22 @@ tree_destroy_proxy(t: *DynamicTree, proxy_id: int) { tree_get_proxy_count(t: *DynamicTree) -> int { return t.proxy_count } // Remove and reinsert, without rotations. -tree_move_proxy_internal(t: *DynamicTree, proxy_id: int, box: AABB, mark_moved: bool) { +tree_move_proxy_internal(t: *DynamicTree, proxy_id: int, box: AABBf, mark_moved: bool) { remove_leaf(t, proxy_id) insert_leaf(t, box, proxy_id, mark_moved, false) } tree_move_proxy(t: *DynamicTree, proxy_id: int, box: AABB) { - tree_move_proxy_internal(t, proxy_id, box, false) + tree_move_proxy_internal(t, proxy_id, math.aabb_round_out(box), false) } // Grow a leaf in place and every ancestor that has to grow with it; mark // the whole path moved. The caller ensures the box is not already contained. tree_enlarge_proxy(t: *DynamicTree, proxy_id: int, box: AABB) { + tree_enlarge_proxy_internal(t, proxy_id, math.aabb_round_out(box)) +} + +tree_enlarge_proxy_internal(t: *DynamicTree, proxy_id: int, box: AABBf) { nodes = t.nodes as DynamicTreeNode[] parents = t.parents as int[] proxies = t.proxies as TreeProxy[] @@ -642,7 +651,7 @@ tree_enlarge_proxy(t: *DynamicTree, proxy_id: int, box: AABB) { } // Enlarge a node's box to contain b; true if it grew. -enlarge_node(nodes: DynamicTreeNode[], index: int, b: AABB) -> bool { +enlarge_node(nodes: DynamicTreeNode[], index: int, b: AABBf) -> bool { a = nodes[index].aabb changed = false if b.lower.x < a.lower.x { a.lower.x = b.lower.x @@ -676,7 +685,15 @@ tree_get_user_data(t: *DynamicTree, proxy_id: int) -> long { return proxies[proxy_id].user_data } +// The leaf's box, in double (exactly the float box it stores). tree_get_aabb(t: *DynamicTree, proxy_id: int) -> AABB { + proxies = t.proxies as TreeProxy[] + nodes = t.nodes as DynamicTreeNode[] + return math.aabb_widen(nodes[proxies[proxy_id].node].aabb) +} + +// The leaf's box as it is stored. +tree_get_aabbf(t: *DynamicTree, proxy_id: int) -> AABBf { proxies = t.proxies as TreeProxy[] nodes = t.nodes as DynamicTreeNode[] return nodes[proxies[proxy_id].node].aabb @@ -692,13 +709,13 @@ tree_get_height(t: *DynamicTree) -> int { tree_get_area_ratio(t: *DynamicTree) -> float { if t.proxy_count == 0 { return 0.0 } nodes = t.nodes as DynamicTreeNode[] - root_area = math.aabb_area(nodes[ROOT_NODE].aabb) + root_area = math.aabb_area(wide(nodes[ROOT_NODE])) if root_area <= 0.0 { return 0.0 } // Free nodes and the empty node are leaf tagged. internal_area = 0.0 i = 2 while i < t.node_end { - if is_leaf(nodes[i].flag_index) == false { internal_area = internal_area + math.aabb_area(nodes[i].aabb) } + if is_leaf(nodes[i].flag_index) == false { internal_area = internal_area + math.aabb_area(wide(nodes[i])) } i = i + 1 } return internal_area / root_area @@ -707,7 +724,7 @@ tree_get_area_ratio(t: *DynamicTree) -> float { tree_get_root_bounds(t: *DynamicTree) -> AABB { if t.proxy_count == 0 { return AABB { lower: math.vec3_zero(), upper: math.vec3_zero() } } nodes = t.nodes as DynamicTreeNode[] - return nodes[ROOT_NODE].aabb + return wide(nodes[ROOT_NODE]) } tree_get_byte_count(t: *DynamicTree) -> int { @@ -732,7 +749,7 @@ validate_subtree(t: *DynamicTree, node_index: int, leaf_count: int[]) -> int { proxies = t.proxies as TreeProxy[] n = nodes[node_index] if is_empty_node(n.flag_index) { return 0 - 1 } - if math.is_valid_aabb(n.aabb) == false { return 0 - 1 } + if math.is_valid_aabbf(n.aabb) == false { return 0 - 1 } if is_leaf(n.flag_index) { proxy_id = proxy_id_of(n.flag_index) @@ -750,7 +767,7 @@ validate_subtree(t: *DynamicTree, node_index: int, leaf_count: int[]) -> int { if t.dfs_ordered && node_index >= pair { return 0 - 1 } c1 = nodes[pair] c2 = nodes[pair + 1] - if math.aabb_contains(n.aabb, c1.aabb) == false || math.aabb_contains(n.aabb, c2.aabb) == false { return 0 - 1 } + if math.aabbf_contains(n.aabb, c1.aabb) == false || math.aabbf_contains(n.aabb, c2.aabb) == false { return 0 - 1 } if is_node_moved(n.flag_index) != (is_node_moved(c1.flag_index) || is_node_moved(c2.flag_index)) { return 0 - 1 } height1 = validate_subtree(t, pair, leaf_count) @@ -836,9 +853,10 @@ struct TreeSaveHeader { padding: int } -// The reference's B3_DYNAMIC_TREE_VERSION with this port's boxes (in -// double) folded in, so neither engine takes the other's bytes. -tree_save_version() -> long { return (0x1D6F4C2A73B80E91 as long) ^ (0x00000000D0B1E64 as long) } +// The reference's B3_DYNAMIC_TREE_VERSION with this port's layout folded +// in (single-precision boxes, its own node and header), so neither engine +// takes the other's bytes. +tree_save_version() -> long { return (0x1D6F4C2A73B80E91 as long) ^ (0x00000000F10A7151 as long) } // The tree as one block of bytes and its length, the block the caller's // to free (basics.free_bytes). A tree without parents (a compound's baked @@ -924,13 +942,14 @@ tree_load_bytes(block: ptr, byte_count: int, scale: float) -> DynamicTree { tree_destroy(&t) return empty_tree() } - // A positive scale keeps every containment: rounding is monotonic. + // Scaled in double and narrowed outward again: a positive scale and the + // outward rounding are both monotonic, so every containment holds. nodes = t.nodes as DynamicTreeNode[] i = 0 while i < t.node_end { if is_empty_node(nodes[i].flag_index) == false { - nodes[i].aabb.lower = math.mul_sv(scale, nodes[i].aabb.lower) - nodes[i].aabb.upper = math.mul_sv(scale, nodes[i].aabb.upper) + box = wide(nodes[i]) + nodes[i].aabb = math.aabb_round_out(AABB { lower: math.mul_sv(scale, box.lower), upper: math.mul_sv(scale, box.upper) }) } i = i + 1 } @@ -982,6 +1001,9 @@ tree_query_on(t: *DynamicTree, stack_block: ptr, box: AABB, mask_bits: long, req nodes = t.nodes as DynamicTreeNode[] proxies = t.proxies as TreeProxy[] stack = stack_block as int[] + // The query box narrowed outward once, so a node it touches in double + // is never missed in single precision. + q = math.aabb_round_out(box) stack_count = 0 stack[stack_count] = root_pair(nodes) stack_count = stack_count + 1 @@ -996,9 +1018,9 @@ tree_query_on(t: *DynamicTree, stack_block: ptr, box: AABB, mask_bits: long, req i = i + 1 lo = nodes[index].aabb.lower up = nodes[index].aabb.upper - if up.x < box.lower.x || lo.x > box.upper.x { continue } - if up.y < box.lower.y || lo.y > box.upper.y { continue } - if up.z < box.lower.z || lo.z > box.upper.z { continue } + if up.x < q.lower.x || lo.x > q.upper.x { continue } + if up.y < q.lower.y || lo.y > q.upper.y { continue } + if up.z < q.lower.z || lo.z > q.upper.z { continue } flags = nodes[index].flag_index if is_leaf(flags) { proxy_id = proxy_id_of(flags) @@ -1018,7 +1040,8 @@ tree_query_on(t: *DynamicTree, stack_block: ptr, box: AABB, mask_bits: long, req return result } -distance_to_node_sqr(point: Vec3, box: AABB) -> float { +distance_to_node_sqr(point: Vec3, node_box: AABBf) -> float { + box = math.aabb_widen(node_box) r = math.sub(point, math.clamp_vec3(point, box.lower, box.upper)) return math.dot(r, r) } @@ -1170,14 +1193,14 @@ tree_ray_cast_on(t: *DynamicTree, stack_block: ptr, input: *RayCastInput, mask_b while i < 2 { index = pair + i i = i + 1 - if math.slab_overlap(nodes[index].aabb.lower, nodes[index].aabb.upper, p1, inv, max_fraction) == false { continue } + if math.slab_overlap(math.widen3(nodes[index].aabb.lower), math.widen3(nodes[index].aabb.upper), p1, inv, max_fraction) == false { continue } if hit_count == 0 { hit0 = index } else { hit1 = index } hit_count = hit_count + 1 } if hit_count == 2 && is_leaf(nodes[hit0].flag_index) == false && is_leaf(nodes[hit1].flag_index) == false { - d1 = math.distance_squared(math.aabb_center(nodes[hit0].aabb), p1) - d2 = math.distance_squared(math.aabb_center(nodes[hit1].aabb), p1) + d1 = math.distance_squared(math.aabb_center(wide(nodes[hit0])), p1) + d2 = math.distance_squared(math.aabb_center(wide(nodes[hit1])), p1) // The closest is pushed last. if d1 < d2 { swap = hit0 @@ -1253,14 +1276,14 @@ tree_box_cast_on(t: *DynamicTree, stack_block: ptr, input: *BoxCastInput, mask_b while i < 2 { index = pair + i i = i + 1 - if math.slab_overlap(math.sub(nodes[index].aabb.lower, extension), math.add(nodes[index].aabb.upper, extension), p1, inv, max_fraction) == false { continue } + if math.slab_overlap(math.sub(math.widen3(nodes[index].aabb.lower), extension), math.add(math.widen3(nodes[index].aabb.upper), extension), p1, inv, max_fraction) == false { continue } if hit_count == 0 { hit0 = index } else { hit1 = index } hit_count = hit_count + 1 } if hit_count == 2 && is_leaf(nodes[hit0].flag_index) == false && is_leaf(nodes[hit1].flag_index) == false { - d1 = math.distance_squared(math.aabb_center(nodes[hit0].aabb), p1) - d2 = math.distance_squared(math.aabb_center(nodes[hit1].aabb), p1) + d1 = math.distance_squared(math.aabb_center(wide(nodes[hit0])), p1) + d2 = math.distance_squared(math.aabb_center(wide(nodes[hit1])), p1) if d1 < d2 { swap = hit0 hit0 = hit1 @@ -1543,7 +1566,7 @@ tree_rebuild(t: *DynamicTree, full_build: bool) -> int { if full_build == false { node.flag_index = node.flag_index & (0 - 1 - MOVED_NODE) } leaf_indices[0] = 0 leaves[0] = node - leaf_centers[0] = math.aabb_center(node.aabb) + leaf_centers[0] = math.aabb_center(wide(node)) leaf_count = 1 } @@ -1564,7 +1587,7 @@ tree_rebuild(t: *DynamicTree, full_build: bool) -> int { if full_build == false { node.flag_index = node.flag_index & (0 - 1 - MOVED_NODE) } leaf_indices[leaf_count] = leaf_count leaves[leaf_count] = node - leaf_centers[leaf_count] = math.aabb_center(node.aabb) + leaf_centers[leaf_count] = math.aabb_center(wide(node)) leaf_count = leaf_count + 1 } } @@ -1601,7 +1624,7 @@ tree_mark_proxy_moved_serial(t: *DynamicTree, proxy_id: int) { // body's own, unraced): a plain read first to skip the fetch-or where // the flag is already set, then the fetch-or, and the previous flags // say whether another worker got there first. -tree_mark_proxy_moved(t: *DynamicTree, proxy_id: int, box: AABB) { +tree_mark_proxy_moved(t: *DynamicTree, proxy_id: int, box: AABBf) { nodes = t.nodes as DynamicTreeNode[] parents = t.parents as int[] proxies = t.proxies as TreeProxy[] @@ -1693,7 +1716,7 @@ refit_subtree(t: *DynamicTree, root_index: int) { if item < 0 { index = 0 - 1 - item pair = left_child(nodes[index].flag_index) - nodes[index].aabb = math.aabb_union(nodes[pair].aabb, nodes[pair + 1].aabb) + nodes[index].aabb = math.aabbf_union(nodes[pair].aabb, nodes[pair + 1].aabb) continue } n = nodes[item] @@ -1724,11 +1747,11 @@ tree_refit(t: *DynamicTree) { if ((flags1 | flags2) & MOVED_NODE) != 0 { if (flags1 & (LEAF_NODE | MOVED_NODE)) == MOVED_NODE { children = flags1 & NODE_INDEX_MASK - nodes[pair].aabb = math.aabb_union(nodes[children].aabb, nodes[children + 1].aabb) + nodes[pair].aabb = math.aabbf_union(nodes[children].aabb, nodes[children + 1].aabb) } if (flags2 & (LEAF_NODE | MOVED_NODE)) == MOVED_NODE { children = flags2 & NODE_INDEX_MASK - nodes[pair + 1].aabb = math.aabb_union(nodes[children].aabb, nodes[children + 1].aabb) + nodes[pair + 1].aabb = math.aabbf_union(nodes[children].aabb, nodes[children + 1].aabb) } } pair = pair - 2 diff --git a/aephysics/dynamics/module.ae b/aephysics/dynamics/module.ae index ed5b119..fa1c512 100644 --- a/aephysics/dynamics/module.ae +++ b/aephysics/dynamics/module.ae @@ -1240,7 +1240,7 @@ struct World { islands: Buffer // Island shape_id_pool: IdPool shapes: Buffer // WorldShape - fat_aabbs: Buffer // AABB by shape id: the fat boxes, dense for the collide pass (the reference's fatAABBs) + fat_aabbs: Buffer // AABBf by shape id: the fat boxes, dense for the collide pass (the reference's fatAABBs), in single precision rounded outward (#151) hull_entries: Buffer // HullEntry: the hull database (the reference's hullDatabase) hull_heads: LongMap // a hull's hash to the first entry of that hash hull_free: int // the first free entry, chained through next @@ -1323,12 +1323,14 @@ world_at(index: int) -> *World { return (g_worlds + index * sizeof(World)) as *W body_at(w: *World, id: int) -> *Body { return (w.bodies.data + id * sizeof(Body)) as *Body } world_shape_at(w: *World, id: int) -> *WorldShape { return (w.shapes.data + id * sizeof(WorldShape)) as *WorldShape } -shape_fat_aabb(w: *World, id: int) -> AABB { - boxes = w.fat_aabbs.data as AABB[] +// A shape's fat box: the one its proxy holds, in single precision rounded +// outward, so the collide pass's overlap test is the broad phase's own. +shape_fat_aabb(w: *World, id: int) -> AABBf { + boxes = w.fat_aabbs.data as AABBf[] return boxes[id] } -set_shape_fat_aabb(w: *World, id: int, box: AABB) { - boxes = w.fat_aabbs.data as AABB[] +set_shape_fat_aabb(w: *World, id: int, box: AABBf) { + boxes = w.fat_aabbs.data as AABBf[] boxes[id] = box } island_at(w: *World, id: int) -> *Island { return (w.islands.data + id * sizeof(Island)) as *Island } @@ -1450,7 +1452,7 @@ create_world(def: *WorldDef) -> WorldId { basics.buffer_reserve(&solver_set_at(w, awake_set).body_states, math.max_int(16, def.dynamic_body_count)) w.shape_id_pool = basics.idpool_create() w.shapes = basics.buffer_create(sizeof(WorldShape), math.max_int(16, def.static_shape_count + def.dynamic_shape_count)) - w.fat_aabbs = basics.buffer_create(sizeof(AABB), math.max_int(16, def.static_shape_count + def.dynamic_shape_count)) + w.fat_aabbs = basics.buffer_create(sizeof(AABBf), math.max_int(16, def.static_shape_count + def.dynamic_shape_count)) w.hull_entries = basics.buffer_create(sizeof(HullEntry), 0) w.hull_heads = basics.map_create(16) w.hull_free = NULL_INDEX @@ -1984,7 +1986,7 @@ destroy_body(id: BodyId) { native.name_free(s.name) s.name = null basics.idpool_free(&w.shape_id_pool, shape_id) - set_shape_fat_aabb(w, shape_id, AABB { lower: math.vec3_zero(), upper: math.vec3_zero() }) + set_shape_fat_aabb(w, shape_id, AABBf { lower: math.vec3f_zero(), upper: math.vec3f_zero() }) s.id = NULL_INDEX shape_id = s.next_shape_id } @@ -2042,7 +2044,7 @@ update_shape_aabbs(w: *World, s: *WorldShape, t: Transform, proxy_kind: int) { s.aabb = box margin = s.aabb_margin if proxy_kind == BODY_STATIC { margin = SPECULATIVE_DISTANCE } - set_shape_fat_aabb(w, s.id, AABB { lower: math.sub(box.lower, math.vec3(margin, margin, margin)), upper: math.add(box.upper, math.vec3(margin, margin, margin)) }) + set_shape_fat_aabb(w, s.id, math.aabb_round_out(AABB { lower: math.sub(box.lower, math.vec3(margin, margin, margin)), upper: math.add(box.upper, math.vec3(margin, margin, margin)) })) } create_shape_proxy(w: *World, s: *WorldShape, kind: int, t: Transform, force_pair_creation: bool) { @@ -2106,7 +2108,7 @@ create_shape(body_id: BodyId, def: *ShapeDef, geometry: Shape) -> ShapeId { s.local_centroid = shape.get_shape_centroid(&s.geometry) s.aabb_margin = compute_shape_margin(&s.geometry) s.aabb = AABB { lower: math.vec3_zero(), upper: math.vec3_zero() } - set_shape_fat_aabb(w, shape_id, s.aabb) + set_shape_fat_aabb(w, shape_id, math.aabb_round_out(s.aabb)) s.generation = s.generation + 1 s.sensor_index = NULL_INDEX s.name = native.name_copy(def.name) @@ -2373,7 +2375,7 @@ destroy_shape(id: ShapeId, update_body_mass: bool) { native.name_free(s.name) s.name = null basics.idpool_free(&w.shape_id_pool, s.id) - set_shape_fat_aabb(w, s.id, AABB { lower: math.vec3_zero(), upper: math.vec3_zero() }) + set_shape_fat_aabb(w, s.id, AABBf { lower: math.vec3f_zero(), upper: math.vec3f_zero() }) s.id = NULL_INDEX if update_body_mass { update_body_mass_data(w, body) } w.locked = false @@ -2870,7 +2872,7 @@ body_set_transform(id: BodyId, position: Vec3, rotation: Quat) { // frame is not taken out of the tree and put back each time. old_fat = shape_fat_aabb(w, s.id) update_shape_aabbs(w, s, sim.transform, body.kind) - if math.aabb_contains(old_fat, s.aabb) { + if math.aabbf_holds(old_fat, s.aabb) { set_shape_fat_aabb(w, s.id, old_fat) } else if s.proxy_key != NULL_INDEX { broad_phase.broad_phase_move_proxy(&w.broad_phase, s.proxy_key, shape_fat_aabb(w, s.id)) @@ -4857,7 +4859,7 @@ collide_contact(w: *World, contact_id: int) { c = contact_at(w, contact_id) shape_a = world_shape_at(w, c.shape_id_a) shape_b = world_shape_at(w, c.shape_id_b) - if math.aabb_overlaps(shape_fat_aabb(w, c.shape_id_a), shape_fat_aabb(w, c.shape_id_b)) == false { + if math.aabbf_overlaps(shape_fat_aabb(w, c.shape_id_a), shape_fat_aabb(w, c.shape_id_b)) == false { c.flags = (c.flags | SIM_DISJOINT) & (0 - 1 - SIM_TOUCHING) basics.bitset_set(&task_context(w).contact_state, contact_id) return diff --git a/aephysics/math/module.ae b/aephysics/math/module.ae index a1aa080..9a8ff38 100644 --- a/aephysics/math/module.ae +++ b/aephysics/math/module.ae @@ -25,6 +25,8 @@ import std.string exports ( Vec2, Vec3, CosSin, Quat, Transform, Matrix2, Matrix3, AABB, Plane, SegmentDistanceResult, Vec3f, Quatf, Matrix3f, EPSILON_F32, TINY_F32, + AABBf, round_down_f32, round_up_f32, infinity_f32, aabb_round_out, aabb_widen, aabbf_empty, + aabbf_union, aabbf_overlaps, aabbf_contains, aabbf_holds, is_valid_aabbf, vec3f, quatf, matrix3f, vec3f_zero, vec3f_one, vec3f_axis_x, vec3f_axis_y, vec3f_axis_z, quatf_identity, mat3f_zero, mat3f_identity, narrow3, widen3, narrow_quat, widen_quat, narrow_mat3, widen_mat3, @@ -71,6 +73,9 @@ extern sqrt(x: float) -> float // libm's single-precision square root: the one the f32 family uses, so a // length is computed at the width it is stored at. extern sqrtf(x: f32) -> f32 +// The next float toward a direction: exact IEEE, so the outward rounding +// of a box is the same on every platform. +extern nextafterf(x: f32, y: f32) -> f32 extern remainder(x: float, y: float) -> float const PI = 3.14159265359 @@ -1247,6 +1252,77 @@ clamp_f32(a: f32, lower: f32, upper: f32) -> f32 { } sqrt_f32(a: f32) -> f32 { return sqrtf(a) } +// --- boxes in single precision (#151) ------------------------------------------- + +// A box in single precision: the dynamic tree's and the broad phase's, as +// the reference keeps them. Built in double and narrowed outward, so it +// always holds the double box: far from the origin a float is coarse (a +// metre at 1e7) and the box only grows, never clips its shape. +struct AABBf { + lower: Vec3f + upper: Vec3f +} + +infinity_f32() -> f32 { return infinity() as f32 } + +// A coordinate narrowed toward negative infinity (b3RoundDownFloat): the +// nearest float at or below it. +round_down_f32(x: float) -> f32 { + f = x as f32 + if (f as float) > x { return nextafterf(f, 0.0 - infinity_f32()) } + return f +} + +// A coordinate narrowed toward positive infinity (b3RoundUpFloat). +round_up_f32(x: float) -> f32 { + f = x as f32 + if (f as float) < x { return nextafterf(f, infinity_f32()) } + return f +} + +// The box narrowed outward (the rounding of b3OffsetAABB): it holds `a`. +aabb_round_out(a: AABB) -> AABBf { + return AABBf { lower: Vec3f { x: round_down_f32(a.lower.x), y: round_down_f32(a.lower.y), z: round_down_f32(a.lower.z) }, + upper: Vec3f { x: round_up_f32(a.upper.x), y: round_up_f32(a.upper.y), z: round_up_f32(a.upper.z) } } +} + +// The box in double: every float is a double, so this is exact. +aabb_widen(a: AABBf) -> AABB { return AABB { lower: widen3(a.lower), upper: widen3(a.upper) } } + +// Inverted infinities: no box overlaps it (an empty tree node's bounds). +aabbf_empty() -> AABBf { + inf = infinity_f32() + return AABBf { lower: Vec3f { x: inf, y: inf, z: inf }, upper: Vec3f { x: 0.0 - inf, y: 0.0 - inf, z: 0.0 - inf } } +} + +// A union is a min and a max, exact at any precision. +aabbf_union(a: AABBf, b: AABBf) -> AABBf { return AABBf { lower: min_vec3f(a.lower, b.lower), upper: max_vec3f(a.upper, b.upper) } } + +aabbf_overlaps(a: AABBf, b: AABBf) -> bool { + if a.upper.x < b.lower.x || a.lower.x > b.upper.x { return false } + if a.upper.y < b.lower.y || a.lower.y > b.upper.y { return false } + if a.upper.z < b.lower.z || a.lower.z > b.upper.z { return false } + return true +} + +// Does a fully contain b? +aabbf_contains(a: AABBf, b: AABBf) -> bool { + if a.lower.x > b.lower.x || b.upper.x > a.upper.x { return false } + if a.lower.y > b.lower.y || b.upper.y > a.upper.y { return false } + if a.lower.z > b.lower.z || b.upper.z > a.upper.z { return false } + return true +} + +// Does the float box hold the double one? Compared widened, so exactly. +aabbf_holds(a: AABBf, b: AABB) -> bool { + if (a.lower.x as float) > b.lower.x || b.upper.x > (a.upper.x as float) { return false } + if (a.lower.y as float) > b.lower.y || b.upper.y > (a.upper.y as float) { return false } + if (a.lower.z as float) > b.lower.z || b.upper.z > (a.upper.z as float) { return false } + return true +} + +is_valid_aabbf(a: AABBf) -> bool { return is_valid_aabb(aabb_widen(a)) } + // m * a, a as a column. mul_mvf(m: Matrix3f, a: Vec3f) -> Vec3f { return Vec3f { x: m.cx.x * a.x + m.cy.x * a.y + m.cz.x * a.z, diff --git a/aephysics/solver/module.ae b/aephysics/solver/module.ae index d636e5b..2642065 100644 --- a/aephysics/solver/module.ae +++ b/aephysics/solver/module.ae @@ -314,9 +314,9 @@ continuous_query_visitor(proxy_id: int, user_data: long, context: ptr) -> bool { // A shape's bounds after the sweep, and the fat bounds and proxy grown when they no longer hold it. grow_bounds(w: *World, s: *WorldShape, sim: *BodySim, is_bullet: bool) { - if math.aabb_contains(dynamics.shape_fat_aabb(w, s.id), s.aabb) { return } + if math.aabbf_holds(dynamics.shape_fat_aabb(w, s.id), s.aabb) { return } margin = math.vec3(s.aabb_margin, s.aabb_margin, s.aabb_margin) - fat = AABB { lower: math.sub(s.aabb.lower, margin), upper: math.add(s.aabb.upper, margin) } + fat = math.aabb_round_out(AABB { lower: math.sub(s.aabb.lower, margin), upper: math.add(s.aabb.upper, margin) }) dynamics.set_shape_fat_aabb(w, s.id, fat) native.atomic_or((&sim.flags) as ptr, dynamics.ENLARGE_BOUNDS) // A bullet's proxies are enlarged later, serially. @@ -520,9 +520,9 @@ finalize_range(start: int, end: int, worker: int, context: ptr) { while shape_id != NULL_INDEX { s = dynamics.world_shape_at(w, shape_id) s.aabb = shape.compute_fat_shape_aabb(&s.geometry, t, dynamics.SPECULATIVE_DISTANCE) - if math.aabb_contains(dynamics.shape_fat_aabb(w, shape_id), s.aabb) == false { + if math.aabbf_holds(dynamics.shape_fat_aabb(w, shape_id), s.aabb) == false { margin = math.vec3(s.aabb_margin, s.aabb_margin, s.aabb_margin) - fat = AABB { lower: math.sub(s.aabb.lower, margin), upper: math.add(s.aabb.upper, margin) } + fat = math.aabb_round_out(AABB { lower: math.sub(s.aabb.lower, margin), upper: math.add(s.aabb.upper, margin) }) dynamics.set_shape_fat_aabb(w, shape_id, fat) broad_phase.broad_phase_mark_proxy_moved(&w.broad_phase, s.proxy_key, fat) } @@ -1318,8 +1318,8 @@ solve_step(w: *World, ctx: *StepContext) { s = dynamics.world_shape_at(w, shape_id) proxy_id = broad_phase.proxy_id(s.proxy_key) fat = dynamics.shape_fat_aabb(w, shape_id) - if math.aabb_contains(dynamic_tree.tree_get_aabb(tree, proxy_id), fat) == false { - dynamic_tree.tree_enlarge_proxy(tree, proxy_id, fat) + if math.aabbf_contains(dynamic_tree.tree_get_aabbf(tree, proxy_id), fat) == false { + dynamic_tree.tree_enlarge_proxy_internal(tree, proxy_id, fat) } shape_id = s.next_shape_id } diff --git a/scripts/profile.sh b/scripts/profile.sh index 9306cfa..1d89c65 100755 --- a/scripts/profile.sh +++ b/scripts/profile.sh @@ -22,7 +22,10 @@ out="target/profile" cflags="$(sed -n 's/^cflags = "\(.*\)"/\1/p' aether.toml)" # aether.toml's ${AETHER_AEPHYSICS_CFLAGS}, expanded as ae expands it (empty when unset). cflags="${cflags//'${AETHER_AEPHYSICS_CFLAGS}'/${AETHER_AEPHYSICS_CFLAGS:-}}" -includes="$(ae cflags | tr ' ' '\n' | grep '^-[Ifw]' | tr '\n' ' ')" +# The compile flags ae gives a C build, whole: since Aether 0.795 the std and +# runtime directories come as `-idirafter ` pairs (aether#2552), which +# a filter on single tokens would split. +includes="$(ae cflags | tr ' ' '\n' | grep -v '^-[Ll]' | tr '\n' ' ')" libs="$(ae cflags | tr ' ' '\n' | grep '^-[Ll]' | tr '\n' ' ')" AETHER_LIB_DIR="$root" aetherc "$source" "$out/$name.c" >"$out/$name.log" 2>&1 || { cat "$out/$name.log"; exit 1; } # aephysics.native's @c_include header, which ae build finds beside the module. diff --git a/tests/test_broad_phase.ae b/tests/test_broad_phase.ae index eee6b47..7e60184 100644 --- a/tests/test_broad_phase.ae +++ b/tests/test_broad_phase.ae @@ -92,7 +92,7 @@ brute_force_count(scene: *Scene, bp: *BroadPhase) -> int { 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] { + if movable && math.aabbf_overlaps(math.aabb_round_out(boxes[i]), math.aabb_round_out(boxes[j])) && bodies[i] != bodies[j] { odd = scene.filter_odd && ((i & 1) == 1 || (j & 1) == 1) if odd == false && broad_phase.has_pair(bp, basics.shape_pair_key(i, j, 0)) == false { count = count + 1 } } @@ -114,7 +114,7 @@ keys_are_overlaps(scene: *Scene, bp: *BroadPhase) -> bool { 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 math.aabbf_overlaps(math.aabb_round_out(boxes[a]), math.aabb_round_out(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 @@ -186,15 +186,15 @@ test_grid() { 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) + keys[i] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_DYNAMIC, math.aabb_round_out(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) + keys[36] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_STATIC, math.aabb_round_out(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) + keys[37] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_KINEMATIC, math.aabb_round_out(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) @@ -217,19 +217,19 @@ test_grid() { 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]) + broad_phase.broad_phase_move_proxy(bp, keys[8], math.aabb_round_out(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]) + broad_phase.broad_phase_mark_proxy_moved(bp, keys[8], math.aabb_round_out(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, basics.shape_pair_key(7, 8, 0)) - broad_phase.broad_phase_move_proxy(bp, keys[7], boxes[7]) + broad_phase.broad_phase_move_proxy(bp, keys[7], math.aabb_round_out(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) @@ -237,7 +237,7 @@ test_grid() { 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]) + broad_phase.broad_phase_move_proxy(bp, keys[i], math.aabb_round_out(boxes[i])) i = i + 1 } expected = brute_force_count(scene, bp) @@ -248,11 +248,11 @@ test_grid() { // 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]) + broad_phase.broad_phase_move_proxy(bp, keys[20], math.aabb_round_out(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]) + broad_phase.broad_phase_move_proxy(bp, keys[20], math.aabb_round_out(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) @@ -260,7 +260,7 @@ test_grid() { 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]) + broad_phase.broad_phase_move_proxy(bp, keys[21], math.aabb_round_out(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. @@ -268,17 +268,17 @@ test_grid() { broad_phase.remove_pair(bp, basics.shape_pair_key(0, 36, 0)) broad_phase.remove_pair(bp, basics.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) + keys[36] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_STATIC, math.aabb_round_out(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, basics.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) + keys[36] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_STATIC, math.aabb_round_out(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]) + broad_phase.broad_phase_move_proxy(bp, keys[0], math.aabb_round_out(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) @@ -287,7 +287,7 @@ test_grid() { 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]) + broad_phase.broad_phase_move_proxy(bp, keys[6], math.aabb_round_out(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)) @@ -335,8 +335,8 @@ test_compound_pairs() { // 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) + keys[0] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_DYNAMIC, math.aabb_round_out(boxes[0]), all_bits(), 0, false) + keys[1] = broad_phase.broad_phase_create_proxy(bp, broad_phase.BODY_STATIC, math.aabb_round_out(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 { @@ -348,13 +348,13 @@ test_compound_pairs() { 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, basics.shape_pair_key(0, 1, 1)) - broad_phase.broad_phase_move_proxy(bp, keys[0], boxes[0]) + broad_phase.broad_phase_move_proxy(bp, keys[0], math.aabb_round_out(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, basics.shape_pair_key(0, 1, 0)) - broad_phase.broad_phase_move_proxy(bp, keys[0], boxes[0]) + broad_phase.broad_phase_move_proxy(bp, keys[0], math.aabb_round_out(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) diff --git a/tests/test_dynamic_tree.ae b/tests/test_dynamic_tree.ae index 5aa50e8..f8c0def 100644 --- a/tests/test_dynamic_tree.ae +++ b/tests/test_dynamic_tree.ae @@ -48,10 +48,12 @@ random_float(lo: float, hi: float) -> float { return lo + t * (hi - lo) } +// Boxes a float holds exactly, so the tree's single-precision nodes keep +// them as they are and the scans agree with the tree by construction. random_box(extent: float, largest: float) -> AABB { center = math.vec3(random_float(0.0 - extent, extent), random_float(0.0 - extent, extent), random_float(0.0 - extent, extent)) half = math.vec3(random_float(0.1, largest), random_float(0.1, largest), random_float(0.1, largest)) - return math.aabb(math.sub(center, half), math.add(center, half)) + return math.aabb_widen(math.aabb_round_out(math.aabb(math.sub(center, half), math.add(center, half)))) } // A set of hits keyed on user data (the box index): proxy ids recycle, @@ -814,6 +816,62 @@ corrupted(block: ptr, byte_count: int, what: int) -> bool { return result } +// Single-precision nodes far out (#151): 300 centimetre boxes at 1e7, +// each held by its leaf, the root holding them all; every proxy a double +// query box touches is found (the tree may find more, a float being a +// metre there), and exactly the ones whose rounded-out boxes touch it. +test_far_boxes() { + seed = 777 + tree = dynamic_tree.tree_create(16) + boxes_block = calloc(PROXY_COUNT, sizeof(AABB)) + boxes = boxes_block as AABB[] + origin = math.vec3(10000000.0, 0.0 - 2000.0, 3000000.0) + held = true + i = 0 + while i < PROXY_COUNT { + center = math.add(origin, math.vec3(random_float(0.0 - 20.0, 20.0), random_float(0.0 - 20.0, 20.0), random_float(0.0 - 20.0, 20.0))) + half = random_float(0.005, 0.05) + boxes[i] = math.aabb(math.sub(center, math.vec3(half, half, half)), math.add(center, math.vec3(half, half, half))) + id = dynamic_tree.tree_create_proxy(&tree, boxes[i], dynamic_tree.default_category_bits(), i as long) + if math.aabb_contains(dynamic_tree.tree_get_aabb(&tree, id), boxes[i]) == false { held = false } + i = i + 1 + } + ensure("far: every leaf holds its box", held) + ensure("far: valid", dynamic_tree.tree_validate(&tree)) + root = dynamic_tree.tree_get_root_bounds(&tree) + all_held = true + i = 0 + while i < PROXY_COUNT { + if math.aabb_contains(root, boxes[i]) == false { all_held = false } + i = i + 1 + } + ensure("far: the root holds them all", all_held) + superset = true + exact = true + round = 0 + while round < 200 { + center = math.add(origin, math.vec3(random_float(0.0 - 20.0, 20.0), random_float(0.0 - 20.0, 20.0), random_float(0.0 - 20.0, 20.0))) + r = random_float(0.01, 3.0) + query = math.aabb(math.sub(center, math.vec3(r, r, r)), math.add(center, math.vec3(r, r, r))) + tree_hits = hitset_create() + dynamic_tree.tree_query(&tree, query, dynamic_tree.default_mask_bits(), false, collect, (&tree_hits) as ptr) + q = math.aabb_round_out(query) + i = 0 + while i < PROXY_COUNT { + found = hitset_has(&tree_hits, i) + if math.aabb_overlaps(boxes[i], query) && found == false { superset = false } + if math.aabbf_overlaps(math.aabb_round_out(boxes[i]), q) != found { exact = false } + i = i + 1 + } + hitset_destroy(&tree_hits) + round = round + 1 + } + ensure("far: every box a query touches in double is found", superset) + ensure("far: and exactly the rounded-out boxes it touches", exact) + free(boxes_block) + dynamic_tree.tree_destroy(&tree) +} + main() { // The compound module's query stacks live as long as the program; the // save test makes a compound, so they are taken before the count. @@ -829,6 +887,7 @@ main() { test_query_closest() test_ray_cast_aabb() test_save_load() + test_far_boxes() ensure("every counted allocation was freed (${basics.alloc_count() - before} left, ${basics.bytes_in_use() - bytes_before} bytes)", basics.alloc_count() == before && basics.bytes_in_use() == bytes_before) println("dynamic_tree: ${checks} checks") diff --git a/tests/test_math.ae b/tests/test_math.ae index dae24d8..bca1c6d 100644 --- a/tests/test_math.ae +++ b/tests/test_math.ae @@ -13,6 +13,7 @@ extern sin(x: float) -> float extern calloc(count: int, size: int) -> ptr extern exit(code: int) extern free(p: ptr) +extern nextafterf(x: f32, y: f32) -> f32 var failures = 0 var checks = 0 @@ -33,6 +34,59 @@ ensure_small(name: string, value: float, tolerance: float) { // the double's, written once at a float's width, so a result converted // from one is the other to what a float can hold (#42). The tolerance is // a float's epsilon, scaled by the size of what is being compared. +// The boxes the tree keeps in single precision (#151): a coordinate rounded +// down is the nearest float at or below it and rounded up the nearest at +// or above, the two the same float when it is one and adjacent floats when +// not, at the origin, at 1e7, past a float's range and at the infinities; +// a box rounded outward holds the double box; the float box operations. +var g_zero_m = 0.0 + +outward_rounding() { + values_block = calloc(12, 8) + values = values_block as float[] + values[0] = 0.0 + values[1] = 0.1 + values[2] = 0.0 - 0.1 + values[3] = 10000000.1 + values[4] = 0.0 - 10000000.3 + values[5] = 1.0 / 3.0 + values[6] = 0.5 + values[7] = 1.0e-42 + values[8] = 3.5e38 + values[9] = 0.0 - 3.5e38 + values[10] = 16777217.0 + values[11] = 123456.789 + held = true + tight = true + i = 0 + while i < 12 { + x = values[i] + down = math.round_down_f32(x) + up = math.round_up_f32(x) + if (down as float) > x || (up as float) < x { held = false } + exact = ((x as f32) as float) == x + if exact && (down != up || (down as float) != x) { tight = false } + if exact == false && nextafterf(down, math.infinity_f32()) != up { tight = false } + i = i + 1 + } + ensure("rounding: down at or below, up at or above, every value", held) + ensure("rounding: one float when exact, adjacent floats when not", tight) + inf = 1.0 / g_zero_m + ensure("rounding: the infinities stay infinite", (math.round_down_f32(inf) as float) == inf && (math.round_up_f32(0.0 - inf) as float) == 0.0 - inf) + far = math.aabb(math.vec3(10000000.01, 0.0 - 0.3, 5.7), math.vec3(10000000.02, 0.4, 5.71)) + out = math.aabb_round_out(far) + ensure("rounding: a centimetre box at 1e7 is held by its float box", math.aabb_contains(math.aabb_widen(out), far) && math.aabbf_holds(out, far)) + ensure("rounding: and the float box is a float step wider at most", (out.upper.x as float) - (out.lower.x as float) <= 0.01 + 2.0) + a = math.aabb_round_out(math.aabb(math.vec3(0.0, 0.0, 0.0), math.vec3(1.0, 1.0, 1.0))) + b = math.aabb_round_out(math.aabb(math.vec3(0.5, 0.5, 0.5), math.vec3(2.0, 2.0, 2.0))) + c = math.aabb_round_out(math.aabb(math.vec3(1.5, 0.0, 0.0), math.vec3(2.0, 1.0, 1.0))) + ensure("float boxes: overlap and not", math.aabbf_overlaps(a, b) && math.aabbf_overlaps(a, c) == false) + u = math.aabbf_union(a, c) + ensure("float boxes: the union contains both", math.aabbf_contains(u, a) && math.aabbf_contains(u, c) && math.aabbf_contains(a, u) == false) + ensure("float boxes: the empty box overlaps nothing", math.aabbf_overlaps(math.aabbf_empty(), u) == false && math.is_valid_aabbf(u) && math.is_valid_aabbf(math.aabbf_empty()) == false) + free(values_block) +} + single_precision() { a = math.vec3(0.35, 0.0 - 1.25, 2.5) b = math.vec3(0.0 - 0.75, 0.5, 1.5) @@ -355,6 +409,7 @@ main() { ensure("safe scale keeps a scale off zero", math.safe_scale(math.vec3(0.0, 0.0 - 0.001, 2.0)).y == 0.0 - math.MIN_SCALE) single_precision() + outward_rounding() println("math: ${checks} checks, ${pairs} direction pairs; cos/sin off libm by at most ${worst_cs}, atan2 by ${worst_atan}") if failures == 0 { println("math: all checks passed") diff --git a/tests/test_physics_world.ae b/tests/test_physics_world.ae index f7516af..cae2531 100644 --- a/tests/test_physics_world.ae +++ b/tests/test_physics_world.ae @@ -1002,7 +1002,7 @@ stress_pairs_hold(world_id: WorldId, sleeping: bool) -> bool { if b.id != dynamics.NULL_INDEX && b.proxy_key != dynamics.NULL_INDEX && b.sensor_index == dynamics.NULL_INDEX && a.body_id != b.body_id { body_b = dynamics.body_at(w, b.body_id) expected = shape.should_shapes_collide(a.filter, b.filter) && dynamics.should_bodies_collide(w, body_a, body_b) && - math.aabb_overlaps(dynamics.shape_fat_aabb(w, i), dynamics.shape_fat_aabb(w, j)) + math.aabbf_overlaps(dynamics.shape_fat_aabb(w, i), dynamics.shape_fat_aabb(w, j)) actual = broad_phase.has_pair(&w.broad_phase, basics.shape_pair_key(i, j, 0)) if (sleeping && actual && expected == false) == false && expected != actual { return false } } From e01e943f9b57d8bfb3e2c784a06e693cf471ebcd Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Fri, 9 Oct 2026 01:44:52 -0300 Subject: [PATCH 2/4] Wide separating axis test, the hulls' single-precision rows, #158 Every hull carries f32 rows of its vertices and normals, padded to eight lanes with vertex 0 and zeros, and its edges' cosines, filled where its hash is stamped (HULL_VERSION 0x4A4C9588; hull_bytes_valid checks them). The separating axis test reads them as the reference's wide test does: support searches eight vertices a pass (the lowest index wins a tie; the reference's embedded-index trick waits on aether#2603), edge candidates culled by the inscribed spheres with the stored cosines and again by a probe point (b3FilterEdgeCandidates), edge pairs eight a pass with the axis and separation in lanes. Every winner is measured again in double, and the golden trace is unchanged. hull_bytes_valid bounded half-edges by MAX_HULL_EDGES, half what the builder allows: a compound of big hulls failed its byte round trip (#158). Fixed, with the round trip tested on a 240-half-edge cylinder. --- README.md | 8 +- aephysics/hull/module.ae | 75 ++++++- aephysics/manifold/module.ae | 367 ++++++++++++++++++++++++----------- tests/test_collision.ae | 7 +- tests/test_compound.ae | 43 ++++ tests/test_manifold.ae | 7 +- 6 files changed, 383 insertions(+), 124 deletions(-) diff --git a/README.md b/README.md index 749672c..e4e24ad 100644 --- a/README.md +++ b/README.md @@ -29,10 +29,10 @@ so a test written against the reference reads the same here. | `aephysics.basics` | bit set, id pool, hash set, a long-to-int map, arrays, the stack and arena allocators, the content hash (the reference's core.c, named so a host's own `core` module and this one can share a program) | done, `test_basics.ae` (100k checks, test_container.c and test_allocator.c among them); `test_hash.ae` (36 checks: test_hash.c's properties on our hash, every bit and the length reaching it, families of real geometry never colliding) | | `aephysics.native` | the Aether face of the one C file built with every program: the threads' helpers Aether has not (a thread-local worker index, yield, pause, the processor count, atomics on an int in place, a semaphore) and the per-worker scratch every module keeps a block of | done | | `aephysics.parallel` | the reference's scheduler and `parallel_for`: worker threads made once and waiting on a semaphore, tasks in slots claimed by compare-and-swap, the main thread helping while it waits; a range in blocks the tasks claim, the caller working as worker 0 | done, `test_parallel.ae` (18 checks); [the step by worker count against the reference's](bench/RESULTS.md#parallel) | -| `aephysics.dynamic_tree` | the bounding volume hierarchy under the broad phase: SAH insertion, rotations, enlarge, sweep refit, partial rebuild in depth-first order, box / closest / ray / swept-box queries, the tree saved and loaded as bytes (b3DynamicTree_Save and _Load; every corrupt input refused) | done, `test_dynamic_tree.ae` (15k checks: TreeSaveLoadRoundtrip among them, the loaded tree's queries against a scan, a scale, and ten corruptions refused); [same tree as the reference; inserts 0.4x its time, queries 1.1x, rays 0.9x](bench/RESULTS.md#dynamic_tree) | -| `aephysics.hull` | quickhull with face merging, the half-edge hull with its mass properties, box / cylinder / cone / rock hulls and PEEL's convex (the reference's complex hull, the same 32 points), clone-and-transform with mirroring, support functions, ray cast, the 2D hull | done, `test_hull.ae` (445 checks); [same hulls as the reference; sphere hulls at parity, merge-heavy cubes 2x, box hulls 3.6x (a heap block against its stack value)](bench/RESULTS.md#hull) | +| `aephysics.dynamic_tree` | the bounding volume hierarchy under the broad phase: SAH insertion, rotations, enlarge, sweep refit, partial rebuild in depth-first order, box / closest / ray / swept-box queries, the tree saved and loaded as bytes (b3DynamicTree_Save and _Load; every corrupt input refused); its boxes in single precision rounded outward from double, as the reference's large-world build keeps them (32-byte nodes; the costs and rays measured on the widened boxes) | done, `test_dynamic_tree.ae` (15k checks: TreeSaveLoadRoundtrip among them, the loaded tree's queries against a scan, a scale, ten corruptions refused, and centimetre boxes at 1e7 each held by its leaf and found by every query that touches it); [same tree as the reference; inserts 0.4x its time, queries 1.1x, rays 0.9x](bench/RESULTS.md#dynamic_tree) | +| `aephysics.hull` | quickhull with face merging, the half-edge hull with its mass properties, box / cylinder / cone / rock hulls and PEEL's convex (the reference's complex hull, the same 32 points), clone-and-transform with mirroring, support functions, ray cast, the 2D hull; every hull carries single-precision rows of its vertices and normals and its edges' cosines, padded to eight lanes, for the wide separating axis test | done, `test_hull.ae` (445 checks); [same hulls as the reference; sphere hulls at parity, merge-heavy cubes 2x, box hulls 3.6x (a heap block against its stack value)](bench/RESULTS.md#hull) | | `aephysics.distance` | GJK with the warm-started simplex cache, the shape cast by conservative advancement, the time of impact by separating-axis root finding | done, `test_distance.ae` (1.1k checks); [same results as the reference; 1.1-1.2x its time, the time of impact faster](bench/RESULTS.md#distance) | -| `aephysics.manifold` | contact manifolds for sphere, capsule and hull in every pairing: the separating axis test with its cache, reference-face clipping, the feature pairs, reduction to four points | done, `test_manifold.ae` (43k checks, 7,000 pairs against a brute-force oracle); [same manifolds as the reference, warm cache at parity](bench/RESULTS.md#manifold) | +| `aephysics.manifold` | contact manifolds for sphere, capsule and hull in every pairing: the separating axis test with its cache (support searches eight vertices a pass over the hulls' rows, edge candidates culled by the inscribed spheres and a probe point, edge pairs eight a pass, every winner measured in double), reference-face clipping, the feature pairs, reduction to four points | done, `test_manifold.ae` (43k checks, 7,000 pairs against a brute-force oracle); [same manifolds as the reference, warm cache at parity](bench/RESULTS.md#manifold) | | `aephysics.triangle_manifold` | one mesh triangle against a sphere, capsule or hull: back-side cull with hysteresis, GJK shallow, the separating axis test deep with the triangle's edges as zero-area faces, the feature recorded for the mesh contact's ghost-collision reduction | done, `test_triangle_manifold.ae` (1.5k checks); [same manifolds as the reference, within 10% on hulls](bench/RESULTS.md#triangle_manifold) | | `aephysics.mesh` | the triangle mesh: a BVH by binned SAH or median split with the triangles in depth-first order, vertex welding, edge flags, any scale including mirrored; overlap, ray cast, shape cast, the mover's planes, a box query | done, `test_mesh.ae` (1.6k checks); [same trees as the reference; the SAH build at parity, the median build 1.2-1.4x, rays and casts faster, the box query 1.5x](bench/RESULTS.md#mesh) | | `aephysics.height_field` | the height field: quantised heights on a fixed diagonal, materials and holes per cell, edge flags per triangle, either winding; overlap, ray and shape casts by a walk along the grid, the mover's planes, a box query | done, `test_height_field.ae` (158 checks, casts against a brute force over a wave, the file roundtrip through `aephysics.files`: the definition back byte for byte, the reference's own text with CRLF lines, and every bad file refused); [same results and the same 1.3 MB as the reference; rays at parity, queries and casts faster, builds 1.1x](bench/RESULTS.md#height_field) | @@ -43,7 +43,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; the compound as bytes and back (b3ConvertCompoundToBytes, every section, shared hull and mesh, material and child checked against the bytes) | done, `test_compound.ae` (200 checks: the three serialize tests, the same bytes for the same compound, and ten corruptions refused); [same results as the reference; builds 0.9x, rays 1.1x, box queries 1.3x](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` (443 checks); [same results as the reference; rays, shape casts and overlaps at or under its time](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.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, candidates checked against the pair set 32 at a time with every slot prefetched first (b3FlushCandidatePairs), 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.mesh_contact` | a convex shape against a mesh or height field: the triangle cache with per-triangle warm starts, the manifolds per triangle, the seam rules against ghost collisions, clusters by normal, the four-point cull | done, `test_mesh_contact.ae` (64 checks); [a box on a wave at 3.2 us a step](bench/RESULTS.md#mesh_contact) | | `aephysics.dynamics` | the world's state and bookkeeping: worlds, bodies (creation, mass from shapes or by hand, transforms, velocities, extents, locks, sleep and enable flags, the type changed, disabled and enabled) in solver sets, shapes of every kind on bodies with their broad-phase proxies, the hull database (a hull shape holds the world's copy, equal hulls share one, transformed hulls baked into their own), islands linked by contacts and joints and split by union-find, sleeping sets, waking and merging, the constraint graph's colouring, contacts from the broad phase's pairs with the narrow phase (convex, mesh, height field, compound children) and the state changes with their events, contact recycling, joints of every kind (definitions, creation with the edges, sets and islands, collide-connected in the pair filter, destruction, separations, reactions), sensors with their begin and end events | `test_dynamics.ae` (641 checks: test_body.c, the step-free test_world.c and test_joint.c, test_shape.c's names and flags, contacts, islands, sleep, joints, sensors); [creation 0.9x, the first collide at parity, recycling 1.1-1.2x, joints 0.6x, sensors 0.75-1.0x](bench/RESULTS.md#dynamics-bookkeeping-contacts-joints-sensors) | | `aephysics.contact_solver` | the contact constraints of a step, scalar: prepared from the manifolds (anchors, base separations, normal masses, the friction centre and its tangent mass, twist and rolling masses), warm started, solved per sub-step (soft normal constraints with speculative bias, then central friction, twist friction and rolling resistance in the relax pass), the restitution pass, the impulses stored with the hit events; the step context | `test_contact_solver.ae` (50 checks: the passes by hand on a cube on a slab) | [ours alone](bench/RESULTS.md#contact_solver) | diff --git a/aephysics/hull/module.ae b/aephysics/hull/module.ae index c44cc98..34810c1 100644 --- a/aephysics/hull/module.ae +++ b/aephysics/hull/module.ae @@ -28,6 +28,7 @@ exports ( HullData, HalfEdge, MassData, ShapeExtent, Point2D, MAX_HULL_VERTICES, MAX_HULL_FACES, MAX_HULL_EDGES, HULL_VERSION, NULL_INDEX, hull_vertices, hull_points, hull_edges, hull_planes, hull_faces, + HULL_SOA_PADDING, hull_soa_stride, hull_soa_vertices, hull_soa_normals, hull_edge_cosines, create_hull, destroy_hull, clone_hull, clone_and_transform_hull, is_valid_hull, hull_bytes_valid, hash_hull_data, compare_hull_data, create_cylinder, create_cone, create_rock, create_complex_hull, @@ -52,7 +53,12 @@ const MAX_HULL_FACES = 128 const MAX_HULL_EDGES = 128 // The final hull's counts are capped at 256 as in the reference, whose indices are bytes. const HULL_MAX_COUNT = 256 -const HULL_VERSION = 0x4A4C9587 +// The layout's tag: the single-precision rows the wide separating axis +// test reads (#150) came with 0x4A4C9588. +const HULL_VERSION = 0x4A4C9588 +// The rows' lanes: a row of vertices or normals is padded to a multiple of +// this, so eight lanes read whole blocks (the reference's B3_HULL_SOA_PADDING). +const HULL_SOA_PADDING = 8 const MARK_VISIBLE = 0 const MARK_DELETE = 1 @@ -87,6 +93,9 @@ struct HullData { face_count: int plane_offset: int face_offset: int + soa_vertex_offset: int // f32 rows x, y, z of the vertices, each hull_soa_stride(vertex_count) long, padded with vertex 0 + soa_normal_offset: int // f32 rows x, y, z of the face normals, each hull_soa_stride(face_count) long, padded with zeros + edge_cosine_offset: int // f32 per full edge: the cosine between its two faces' normals byte_count: int } @@ -117,6 +126,14 @@ hull_edges(h: *HullData) -> HalfEdge[] { return ((h as ptr) + h.edge_offset) as hull_planes(h: *HullData) -> Plane[] { return ((h as ptr) + h.plane_offset) as Plane[] } hull_faces(h: *HullData) -> int[] { return ((h as ptr) + h.face_offset) as int[] } +// The single-precision rows (#150, the reference's b3GetHullSoaVertices, +// b3GetHullSoaNormals and b3GetHullEdgeCosines): what the wide separating +// axis test reads eight lanes at a time, made with the hull. +hull_soa_stride(count: int) -> int { return (count + HULL_SOA_PADDING - 1) & (0 - HULL_SOA_PADDING) } +hull_soa_vertices(h: *HullData) -> ptr { return (h as ptr) + h.soa_vertex_offset } +hull_soa_normals(h: *HullData) -> ptr { return (h as ptr) + h.soa_normal_offset } +hull_edge_cosines(h: *HullData) -> f32[] { return ((h as ptr) + h.edge_cosine_offset) as f32[] } + align8(x: int) -> int { return (x + 7) & (0 - 8) } // --- the builder --------------------------------------------------------------- @@ -1370,11 +1387,52 @@ update_hull_bulk_properties(h: *HullData) -> bool { return mass > 0.0 && volume > 0.0 && area > 0.0 && radius > 0.0 } +// The hull finished: its single-precision rows from its points and planes, +// then its hash over every byte. Every builder ends here. stamp_hash(h: *HullData) { + refresh_hull_soa(h) h.hash = 0 as long h.hash = basics.hash_bytes(h as ptr, h.byte_count) } +// The rows of the vertices and normals and the edges' cosines, from the +// hull's points and planes (narrowed from double at the store). A row's +// padding repeats vertex 0, so it never wins a support search that keeps +// the lowest index on a tie, and the normals' padding is zero. +refresh_hull_soa(h: *HullData) { + points = hull_points(h) + planes = hull_planes(h) + edges = hull_edges(h) + stride = hull_soa_stride(h.vertex_count) + v = hull_soa_vertices(h) as f32[] + i = 0 + while i < stride { + p = points[0] + if i < h.vertex_count { p = points[i] } + v[i] = p.x as f32 + v[stride + i] = p.y as f32 + v[2 * stride + i] = p.z as f32 + i = i + 1 + } + face_stride = hull_soa_stride(h.face_count) + n = hull_soa_normals(h) as f32[] + i = 0 + while i < face_stride { + normal = math.vec3_zero() + if i < h.face_count { normal = planes[i].normal } + n[i] = normal.x as f32 + n[face_stride + i] = normal.y as f32 + n[2 * face_stride + i] = normal.z as f32 + i = i + 1 + } + cosines = hull_edge_cosines(h) + i = 0 + while i < h.edge_count / 2 { + cosines[i] = math.dot(planes[edges[2 * i].face].normal, planes[edges[2 * i + 1].face].normal) as f32 + i = i + 1 + } +} + hash_hull_data(h: *HullData) -> long { return h.hash } compare_hull_data(a: *HullData, b: *HullData) -> bool { @@ -1395,12 +1453,16 @@ hull_bytes_valid(h: *HullData, available: int) -> bool { if n < sizeof(HullData) || n > available { return false } if h.vertex_count < 4 || h.vertex_count > MAX_HULL_VERTICES { return false } if h.face_count < 4 || h.face_count > MAX_HULL_FACES { return false } - if h.edge_count < 12 || h.edge_count > MAX_HULL_EDGES || (h.edge_count & 1) != 0 { return false } + // edge_count counts half-edges; the builder allows MAX_HULL_EDGES full ones (#158). + if h.edge_count < 12 || h.edge_count > 2 * MAX_HULL_EDGES || (h.edge_count & 1) != 0 { return false } if basics.section_fits(h.vertex_offset, h.vertex_count, 4, n) == false { return false } if basics.section_fits(h.point_offset, h.vertex_count, sizeof(Vec3), n) == false { return false } if basics.section_fits(h.edge_offset, h.edge_count, sizeof(HalfEdge), n) == false { return false } if basics.section_fits(h.plane_offset, h.face_count, sizeof(Plane), n) == false { return false } if basics.section_fits(h.face_offset, h.face_count, 4, n) == false { return false } + if basics.section_fits(h.soa_vertex_offset, 3 * hull_soa_stride(h.vertex_count), 4, n) == false { return false } + if basics.section_fits(h.soa_normal_offset, 3 * hull_soa_stride(h.face_count), 4, n) == false { return false } + if basics.section_fits(h.edge_cosine_offset, h.edge_count / 2, 4, n) == false { return false } vertices = hull_vertices(h) edges = hull_edges(h) faces = hull_faces(h) @@ -1437,6 +1499,12 @@ allocate_hull(vertex_count: int, edge_count: int, face_count: int) -> *HullData byte_count = byte_count + align8(face_count * sizeof(Plane)) face_offset = byte_count byte_count = byte_count + align8(face_count * 4) + soa_vertex_offset = byte_count + byte_count = byte_count + align8(3 * hull_soa_stride(vertex_count) * 4) + soa_normal_offset = byte_count + byte_count = byte_count + align8(3 * hull_soa_stride(face_count) * 4) + edge_cosine_offset = byte_count + byte_count = byte_count + align8((edge_count / 2) * 4) h = basics.alloc(byte_count) as *HullData h.version = HULL_VERSION h.vertex_offset = vertex_offset @@ -1444,6 +1512,9 @@ allocate_hull(vertex_count: int, edge_count: int, face_count: int) -> *HullData h.edge_offset = edge_offset h.plane_offset = plane_offset h.face_offset = face_offset + h.soa_vertex_offset = soa_vertex_offset + h.soa_normal_offset = soa_normal_offset + h.edge_cosine_offset = edge_cosine_offset h.vertex_count = vertex_count h.edge_count = edge_count h.face_count = face_count diff --git a/aephysics/manifold/module.ae b/aephysics/manifold/module.ae index bb10a6d..d30d938 100644 --- a/aephysics/manifold/module.ae +++ b/aephysics/manifold/module.ae @@ -228,6 +228,12 @@ clip_points() -> ptr { return native.scratch(g_clip_points, MAX_CLIP_POINTS * si // hundred times (see edge_axes). var g_edge_axes: ptr = null edge_axes() -> ptr { return native.scratch(g_edge_axes, EDGE_SCRATCH_BYTES) } +// The two hulls' candidate half-edges: A's first, B's from EDGE_LIST_B. +var g_edge_lists: ptr = null +edge_lists() -> ptr { return native.scratch(g_edge_lists, EDGE_LIST_BYTES) } +// Sixteen lanes stored for a reduction: a support search's best dots and their indices. +var g_lane_scratch: ptr = null +lane_scratch() -> ptr { return native.scratch(g_lane_scratch, 64) } b_frame_scratch() -> ptr { return edge_axes() + AXIS_FIELDS * AXIS_SLOTS * 4 } scratch_ready() { @@ -236,6 +242,8 @@ scratch_ready() { g_clip2 = basics.alloc(native.MAX_WORKERS * (CLIP_CAPACITY * sizeof(ClipVertex))) g_clip_points = basics.alloc(native.MAX_WORKERS * (MAX_CLIP_POINTS * sizeof(LocalManifoldPoint))) g_edge_axes = basics.alloc(native.MAX_WORKERS * EDGE_SCRATCH_BYTES) + g_edge_lists = basics.alloc(native.MAX_WORKERS * EDGE_LIST_BYTES) + g_lane_scratch = basics.alloc(native.MAX_WORKERS * 64) } } @@ -264,6 +272,16 @@ reserve_scratch() { scratch_ready() } // A hull has up to MAX_HULL_EDGES full edges (2 * MAX_HULL_EDGES half-edges, // the builder's limit), so the table holds that many, a multiple of four. const AXIS_SLOTS = 128 +// A hull has at most MAX_HULL_EDGES full edges: each list holds them all. +const EDGE_LIST_B = 128 +const EDGE_LIST_BYTES = 2 * 128 * 4 +// The pairs left that pay for the probe's second cull (B3_EDGE_PROBE_MIN_TESTS). +const EDGE_PROBE_MIN_TESTS = 10 +// How far under the best edge separation a lane's single-precision +// separation may fall and still be measured in double: well past a float's +// rounding at the hulls' scale (the test runs in A's frame, the hulls +// touching), so the double alone decides which edge wins. +const EDGE_LANE_SLACK = 0.0001 const AXIS_FIELDS = 14 // Where each array starts, in elements: x, y, z of the direction, of // both face normals and of the origin, then the tolerance. @@ -937,7 +955,7 @@ compute_separating_axis(hull_a: *HullData, hull_b: *HullData, xf_b: Transform, e if early_return { plane = planes_a[seed_a] direction = math.neg(math.mul_mv(inv_r, plane.normal)) - seed_vertex = support_index(points_b, hull_b.vertex_count, direction) + seed_vertex = support_index_wide(hull_b, direction) seed_separation = math.dot(plane.normal, xf_b.p) - plane.offset - math.dot(direction, points_b[seed_vertex]) floor_a = math.min_float(seed_separation, SPECULATIVE_DISTANCE) } @@ -950,7 +968,7 @@ compute_separating_axis(hull_a: *HullData, hull_b: *HullData, xf_b: Transform, e if early_return == false || i != seed_a { direction = math.neg(math.mul_mv(inv_r, plane.normal)) plane_separation = math.dot(plane.normal, xf_b.p) - plane.offset - vertex_index = support_index(points_b, hull_b.vertex_count, direction) + vertex_index = support_index_wide(hull_b, direction) separation = plane_separation - math.dot(direction, points_b[vertex_index]) } if separation > res.face_a.separation { @@ -987,7 +1005,7 @@ compute_separating_axis(hull_a: *HullData, hull_b: *HullData, xf_b: Transform, e if early_return { plane = planes_b[seed_b] direction = math.neg(math.mul_mv(r, plane.normal)) - seed_vertex = support_index(points_a, hull_a.vertex_count, direction) + seed_vertex = support_index_wide(hull_a, direction) seed_separation = math.dot(direction, xf_b.p) - plane.offset - math.dot(direction, points_a[seed_vertex]) // A's faces have set a floor of their own. floor_b = math.min_float(math.max_float(seed_separation, res.face_a.separation), SPECULATIVE_DISTANCE) @@ -1001,7 +1019,7 @@ compute_separating_axis(hull_a: *HullData, hull_b: *HullData, xf_b: Transform, e separation = seed_separation if early_return == false || i != seed_b { plane_separation = math.dot(direction, xf_b.p) - plane.offset - vertex_index = support_index(points_a, hull_a.vertex_count, direction) + vertex_index = support_index_wide(hull_a, direction) separation = plane_separation - math.dot(direction, points_a[vertex_index]) } if separation > res.face_b.separation { @@ -1018,22 +1036,15 @@ compute_separating_axis(hull_a: *HullData, hull_b: *HullData, xf_b: Transform, e i = i + 1 } - // The edge pairs. B's data is taken into A's frame and negated once: - // C and D are its two face normals, DC its edge, bv0 its first vertex. + // The edge pairs. Only the edges that can still beat the best face take + // part (Box3D's inscribed-sphere test, b3TestEdgeCandidateSorted): an + // edge pair's axis lies on both edges' arcs, and no axis on an edge's + // arc separates by more than the other hull's centre reaches along it + // less that hull's inner radius. The cosine between an edge's faces is + // the hull's own (hull_edge_cosines). Without an early return every + // edge is kept. squared_tol = PARALLEL_EDGE_TOL * PARALLEL_EDGE_TOL eps = 0.0 - 0.0001 - // A's edges, gathered once: what the inner loop reads for every edge - // of B. The axis test is reachable without a clip (a query that only - // asks whether two hulls are apart), so it makes sure of its own - // scratch rather than relying on the caller's path. - scratch_ready() - axes = edge_axes() - slots = axes as f32[] - // Only the edges that can still beat the best face take part (Box3D - // adec25b): an edge pair's axis passes through both edges' arcs, and - // no axis on an edge's arc separates by more than the other hull's - // centre reaches along it less that hull's inner radius. Without an - // early return every edge is kept, as before. max_face_separation = math.max_float(res.face_a.separation, res.face_b.separation) bound_slack = radius - radius_bound threshold_a = 0.0 - math.MAX_FLOAT @@ -1044,56 +1055,92 @@ compute_separating_axis(hull_a: *HullData, hull_b: *HullData, xf_b: Transform, e } center_b_in_a = math.add(math.mul_mv(r, hull_b.center), xf_b.p) center_a_in_b = math.mul_mv(inv_r, math.sub(hull_a.center, xf_b.p)) - axis_count = 0 + scratch_ready() + lists = edge_lists() as int[] + cosines_a = hull.hull_edge_cosines(hull_a) + cosines_b = hull.hull_edge_cosines(hull_b) + na = 0 i = 0 while i < hull_a.edge_count { - twin_a = i + 1 - plane0 = planes_a[edges_a[i].face] - plane1 = planes_a[edges_a[twin_a].face] - if edge_can_reach(math.plane_separation(plane0, center_b_in_a), math.plane_separation(plane1, center_b_in_a), - math.dot(plane0.normal, plane1.normal), threshold_a) == false { - i = i + 2 - continue + if edge_can_reach(math.plane_separation(planes_a[edges_a[i].face], center_b_in_a), math.plane_separation(planes_a[edges_a[i + 1].face], center_b_in_a), + cosines_a[i >> 1] as float, threshold_a) { + lists[na] = i + na = na + 1 + } + i = i + 2 + } + nb = 0 + j = 0 + while j < hull_b.edge_count { + if edge_can_reach(math.plane_separation(planes_b[edges_b[j].face], center_a_in_b), math.plane_separation(planes_b[edges_b[j + 1].face], center_a_in_b), + cosines_b[j >> 1] as float, threshold_b) { + lists[EDGE_LIST_B + nb] = j + nb = nb + 1 } + j = j + 2 + } + // The best faces' support vertices are points of the other hull, so no + // edge pair's axis separates by more than such a point's plane + // separations over the edge's arc: with enough pairs left to pay for + // it, each list is culled again against them (b3FilterEdgeCandidates). + if early_return && nb * ((na + 3) >> 2) >= EDGE_PROBE_MIN_TESTS { + probe_bound = max_face_separation - (0.1 * math.LINEAR_SLOP + 0.001 * math.abs_float(center_distance + radius)) + vertex_b = res.face_a.index_b + if vertex_b == NULL_INDEX { vertex_b = 0 } + probe_b = math.add(math.mul_mv(r, points_b[vertex_b]), xf_b.p) + na = filter_edge_candidates(planes_a, edges_a, cosines_a, probe_b, probe_bound, lists, 0, na) + vertex_a = res.face_b.index_a + if vertex_a == NULL_INDEX { vertex_a = 0 } + probe_a = math.mul_mv(inv_r, math.sub(points_a[vertex_a], xf_b.p)) + nb = filter_edge_candidates(planes_b, edges_b, cosines_b, probe_a, probe_bound, lists, EDGE_LIST_B, nb) + } + if na == 0 || nb == 0 { return res } + + // A's candidate edges, gathered once into flat single-precision rows: + // what the inner loop reads for every edge of B. A separating axis is a + // direction, and the reference finds it in floats. + axes = edge_axes() + slots = axes as f32[] + k = 0 + while k < na { + i = lists[k] + twin_a = i + 1 av0 = points_a[edges_a[i].origin] ad = math.sub(points_a[edges_a[twin_a].origin], av0) dir32 = math.narrow3(ad) n0_32 = math.narrow3(planes_a[edges_a[i].face].normal) n1_32 = math.narrow3(planes_a[edges_a[twin_a].face].normal) v0_32 = math.narrow3(av0) - slots[AXIS_DIR_X + axis_count] = dir32.x - slots[AXIS_DIR_Y + axis_count] = dir32.y - slots[AXIS_DIR_Z + axis_count] = dir32.z - slots[AXIS_N0_X + axis_count] = n0_32.x - slots[AXIS_N0_Y + axis_count] = n0_32.y - slots[AXIS_N0_Z + axis_count] = n0_32.z - slots[AXIS_N1_X + axis_count] = n1_32.x - slots[AXIS_N1_Y + axis_count] = n1_32.y - slots[AXIS_N1_Z + axis_count] = n1_32.z - slots[AXIS_V0_X + axis_count] = v0_32.x - slots[AXIS_V0_Y + axis_count] = v0_32.y - slots[AXIS_V0_Z + axis_count] = v0_32.z - slots[AXIS_TOL + axis_count] = (squared_tol * math.length_squared(ad)) as f32 - slots[AXIS_EDGE + axis_count] = i as f32 - axis_count = axis_count + 1 - i = i + 2 + slots[AXIS_DIR_X + k] = dir32.x + slots[AXIS_DIR_Y + k] = dir32.y + slots[AXIS_DIR_Z + k] = dir32.z + slots[AXIS_N0_X + k] = n0_32.x + slots[AXIS_N0_Y + k] = n0_32.y + slots[AXIS_N0_Z + k] = n0_32.z + slots[AXIS_N1_X + k] = n1_32.x + slots[AXIS_N1_Y + k] = n1_32.y + slots[AXIS_N1_Z + k] = n1_32.z + slots[AXIS_V0_X + k] = v0_32.x + slots[AXIS_V0_Y + k] = v0_32.y + slots[AXIS_V0_Z + k] = v0_32.z + slots[AXIS_TOL + k] = (squared_tol * math.length_squared(ad)) as f32 + slots[AXIS_EDGE + k] = i as f32 + k = k + 1 } - // The tail of the last four. A zero direction answers zero to both of - // the first dot products, and zero is not under eps, so a lane past - // the count decides nothing. - padded = axis_count - while padded % 4 != 0 { + // The tail of the last eight. A zero direction answers zero to both of + // the first dot products, and zero is not under eps, so a lane past the + // count decides nothing. + padded = na + while padded % 8 != 0 { slots[AXIS_DIR_X + padded] = 0.0 slots[AXIS_DIR_Y + padded] = 0.0 slots[AXIS_DIR_Z + padded] = 0.0 padded = padded + 1 } f32 eps32 = eps - if axis_count == 0 { return res } // B's face normals and vertices into A's frame, negated, once (Box3D // adec25b) rather than four products for every B edge, into the room // after the edge table. - scratch_ready() b_in_a = b_frame_scratch() as Vec3[] k = 0 while k < hull_b.face_count { @@ -1105,16 +1152,17 @@ compute_separating_axis(hull_a: *HullData, hull_b: *HullData, xf_b: Transform, e b_in_a[hull_b.face_count + k] = math.neg(math.add(math.mul_mv(r, points_b[k]), xf_b.p)) k = k + 1 } - j = 0 - while j < hull_b.edge_count { + // Every candidate of B against A's, eight of A's a pass (the reference's + // f32x8 loop); the lanes only choose, the axis and its separation are + // computed again in double for the pairs that pass. + eps_lane = lanes.splat8(eps32) + zero_lane = lanes.splat8(0.0) + one_lane = lanes.splat8(1.0) + m = 0 + while m < nb { + j = lists[EDGE_LIST_B + m] + m = m + 1 twin_b = j + 1 - plane0 = planes_b[edges_b[j].face] - plane1 = planes_b[edges_b[twin_b].face] - if edge_can_reach(math.plane_separation(plane0, center_a_in_b), math.plane_separation(plane1, center_a_in_b), - math.dot(plane0.normal, plane1.normal), threshold_b) == false { - j = j + 2 - continue - } c = b_in_a[edges_b[j].face] d = b_in_a[edges_b[twin_b].face] bv0 = b_in_a[hull_b.face_count + edges_b[j].origin] @@ -1123,73 +1171,97 @@ compute_separating_axis(hull_a: *HullData, hull_b: *HullData, xf_b: Transform, e c32 = math.narrow3(c) d32 = math.narrow3(d) dc32 = math.narrow3(dc) - cx = lanes.splat4(c32.x) - cy = lanes.splat4(c32.y) - cz = lanes.splat4(c32.z) - dx = lanes.splat4(d32.x) - dy = lanes.splat4(d32.y) - dz = lanes.splat4(d32.z) - dcx = lanes.splat4(dc32.x) - dcy = lanes.splat4(dc32.y) - dcz = lanes.splat4(dc32.z) - eps_lane = lanes.splat4(eps32) + cx = lanes.splat8(c32.x) + cy = lanes.splat8(c32.y) + cz = lanes.splat8(c32.z) + dx = lanes.splat8(d32.x) + dy = lanes.splat8(d32.y) + dz = lanes.splat8(d32.z) + dcx = lanes.splat8(dc32.x) + dcy = lanes.splat8(dc32.y) + dcz = lanes.splat8(dc32.z) + bv0_32 = math.narrow3(bv0) + bvx = lanes.splat8(bv0_32.x) + bvy = lanes.splat8(bv0_32.y) + bvz = lanes.splat8(bv0_32.z) k = 0 while k < padded { - dir_x = lanes.load4(axes, (AXIS_DIR_X + k) as long) - dir_y = lanes.load4(axes, (AXIS_DIR_Y + k) as long) - dir_z = lanes.load4(axes, (AXIS_DIR_Z + k) as long) + dir_x = lanes.load8(axes, (AXIS_DIR_X + k) as long) + dir_y = lanes.load8(axes, (AXIS_DIR_Y + k) as long) + dir_z = lanes.load8(axes, (AXIS_DIR_Z + k) as long) cba = cx * dir_x + cy * dir_y + cz * dir_z dba = dx * dir_x + dy * dir_y + dz * dir_z - live = lanes.lt4(cba * dba, eps_lane) - if lanes.any4(live) { - n0_x = lanes.load4(axes, (AXIS_N0_X + k) as long) - n0_y = lanes.load4(axes, (AXIS_N0_Y + k) as long) - n0_z = lanes.load4(axes, (AXIS_N0_Z + k) as long) - n1_x = lanes.load4(axes, (AXIS_N1_X + k) as long) - n1_y = lanes.load4(axes, (AXIS_N1_Y + k) as long) - n1_z = lanes.load4(axes, (AXIS_N1_Z + k) as long) + live = lanes.lt8(cba * dba, eps_lane) + if lanes.any8(live) { + n0_x = lanes.load8(axes, (AXIS_N0_X + k) as long) + n0_y = lanes.load8(axes, (AXIS_N0_Y + k) as long) + n0_z = lanes.load8(axes, (AXIS_N0_Z + k) as long) + n1_x = lanes.load8(axes, (AXIS_N1_X + k) as long) + n1_y = lanes.load8(axes, (AXIS_N1_Y + k) as long) + n1_z = lanes.load8(axes, (AXIS_N1_Z + k) as long) adc = n0_x * dcx + n0_y * dcy + n0_z * dcz bdc = n1_x * dcx + n1_y * dcy + n1_z * dcz - live = lanes.mask_and(live, lanes.lt4(adc * bdc, eps_lane)) - live = lanes.mask_and(live, lanes.lt4(cba * bdc, eps_lane)) + live = lanes.mask8_and(live, lanes.lt8(adc * bdc, eps_lane)) + live = lanes.mask8_and(live, lanes.lt8(cba * bdc, eps_lane)) // Near-parallel edges are not an axis. - max_cd = lanes.max4(cba * cba, dba * dba) - live = lanes.mask_and(live, lanes.gt4(max_cd, lanes.load4(axes, (AXIS_TOL + k) as long))) - lane = 0 - while lane < 4 { - if lanes.mask_lane(live, lane as long) != 0 { - // The axis itself is worth a double: it is the - // normal a contact is built on, and the - // separation is compared against the others'. - at = k + lane - edge_dir = math.vec3(slots[AXIS_DIR_X + at] as float, - slots[AXIS_DIR_Y + at] as float, - slots[AXIS_DIR_Z + at] as float) - edge_v0 = math.vec3(slots[AXIS_V0_X + at] as float, - slots[AXIS_V0_Y + at] as float, - slots[AXIS_V0_Z + at] as float) - cbad = math.dot(c, edge_dir) - dbad = math.dot(d, edge_dir) - t = (0.0 - cbad) / (dbad - cbad) - n = math.normalize(math.add(c, math.mul_sv(t, math.sub(d, c)))) - separation = 0.0 - math.dot(math.add(edge_v0, bv0), n) - if separation > res.edge.separation { - res.edge.normal = n - res.edge.separation = separation - res.edge.index_a = slots[AXIS_EDGE + at] as int - res.edge.index_b = j - if separation > SPECULATIVE_DISTANCE && early_return { - res.separated_feature = AXIS_EDGE_PAIR - return res + max_cd = lanes.max8(cba * cba, dba * dba) + live = lanes.mask8_and(live, lanes.gt8(max_cd, lanes.load8(axes, (AXIS_TOL + k) as long))) + // The axis and its separation for all eight in lanes, as the + // reference finds them: t = -CBA / (DBA - CBA), the normal + // C + t (D - C) normalised, the separation -dot(av0 + bv0, n). + // Only a lane that comes within EDGE_LANE_SLACK of the best so + // far is measured again in double, which decides: no lane the + // double would have kept is lost to the float's rounding. + lane_t = (zero_lane - cba) / (dba - cba) + lane_nx = cx + lane_t * (dx - cx) + lane_ny = cy + lane_t * (dy - cy) + lane_nz = cz + lane_t * (dz - cz) + lane_inv = one_lane / lanes.sqrt8(lane_nx * lane_nx + lane_ny * lane_ny + lane_nz * lane_nz) + lane_nx = lane_nx * lane_inv + lane_ny = lane_ny * lane_inv + lane_nz = lane_nz * lane_inv + v0x = lanes.load8(axes, (AXIS_V0_X + k) as long) + bvx + v0y = lanes.load8(axes, (AXIS_V0_Y + k) as long) + bvy + v0z = lanes.load8(axes, (AXIS_V0_Z + k) as long) + bvz + sep = zero_lane - (v0x * lane_nx + v0y * lane_ny + v0z * lane_nz) + f32 floor32 = res.edge.separation - EDGE_LANE_SLACK + live = lanes.mask8_and(live, lanes.gt8(sep, lanes.splat8(floor32))) + if lanes.any8(live) { + lane = 0 + while lane < 8 { + if lanes.mask8_lane(live, lane as long) != 0 { + // The axis itself is worth a double: it is the + // normal a contact is built on, and the + // separation is compared against the others'. + at = k + lane + edge_dir = math.vec3(slots[AXIS_DIR_X + at] as float, + slots[AXIS_DIR_Y + at] as float, + slots[AXIS_DIR_Z + at] as float) + edge_v0 = math.vec3(slots[AXIS_V0_X + at] as float, + slots[AXIS_V0_Y + at] as float, + slots[AXIS_V0_Z + at] as float) + cbad = math.dot(c, edge_dir) + dbad = math.dot(d, edge_dir) + t = (0.0 - cbad) / (dbad - cbad) + n = math.normalize(math.add(c, math.mul_sv(t, math.sub(d, c)))) + separation = 0.0 - math.dot(math.add(edge_v0, bv0), n) + if separation > res.edge.separation { + res.edge.normal = n + res.edge.separation = separation + res.edge.index_a = slots[AXIS_EDGE + at] as int + res.edge.index_b = j + if separation > SPECULATIVE_DISTANCE && early_return { + res.separated_feature = AXIS_EDGE_PAIR + return res + } } } + lane = lane + 1 } - lane = lane + 1 } } - k = k + 4 + k = k + 8 } - j = j + 2 } return res } @@ -1211,6 +1283,77 @@ edge_can_reach(a1: float, a2: float, c: float, bound: float) -> bool { return u * u >= (bound - hi) * (bound + hi) * s } +// The candidates of one hull's list (from `offset`, `count` of them) that +// can still reach `bound` from the probe point's plane separations, kept +// in order at the front; how many (b3FilterEdgeCandidates). +filter_edge_candidates(planes: Plane[], edges: HalfEdge[], cosines: f32[], probe: Vec3, bound: float, lists: int[], offset: int, count: int) -> int { + kept = 0 + k = 0 + while k < count { + i = lists[offset + k] + if edge_can_reach(math.plane_separation(planes[edges[i].face], probe), math.plane_separation(planes[edges[i + 1].face], probe), + cosines[i >> 1] as float, bound) { + lists[offset + kept] = i + kept = kept + 1 + } + k = k + 1 + } + return kept +} + +// The vertex farthest along the direction, eight vertices a pass over the +// hull's single-precision rows (the reference's b3GetSupportWide). Each +// lane keeps its best dot and that vertex's index; the lanes are then +// reduced in order, the larger dot winning and the lower index on a tie, +// so the padding (vertex 0 again, past the count) never wins and the +// answer is the scalar search's first maximum. The reference embeds the +// index in each value's low mantissa bits instead (b3EmbedIndexW), which +// std.lanes cannot express yet (aether#2603); the compare here is exact +// where that one ties values within 2^-16 of each other. The caller +// measures the separation of the chosen vertex in double. +support_index_wide(h: *HullData, direction: Vec3) -> int { + scratch_ready() + stride = hull.hull_soa_stride(h.vertex_count) + rows = hull.hull_soa_vertices(h) + d = math.narrow3(direction) + dx = lanes.splat8(d.x) + dy = lanes.splat8(d.y) + dz = lanes.splat8(d.z) + best = lanes.splat8(0.0 - math.infinity_f32()) + best_index = lanes.splat8(0.0 - 1.0) + index = lanes.f32x8(0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0) + eight = lanes.splat8(8.0) + i = 0 + while i < stride { + x = lanes.load8(rows, i as long) + y = lanes.load8(rows, (stride + i) as long) + z = lanes.load8(rows, (2 * stride + i) as long) + dot = dx * x + dy * y + dz * z + better = lanes.gt8(dot, best) + best = lanes.select8(better, dot, best) + best_index = lanes.select8(better, index, best_index) + index = index + eight + i = i + 8 + } + // The lanes stored once and reduced in order. + out = lane_scratch() + lanes.store8(out, 0 as long, best) + lanes.store8(out, 8 as long, best_index) + values = out as f32[] + winner = values[8] + winner_dot = values[0] + lane = 1 + while lane < 8 { + v = values[lane] + if v > winner_dot || (v == winner_dot && values[8 + lane] < winner) { + winner_dot = v + winner = values[8 + lane] + } + lane = lane + 1 + } + return winner as int +} + // The vertex farthest along the direction. support_index(points: Vec3[], count: int, direction: Vec3) -> int { best = 0 diff --git a/tests/test_collision.ae b/tests/test_collision.ae index a1b98bc..0dfec39 100644 --- a/tests/test_collision.ae +++ b/tests/test_collision.ae @@ -172,9 +172,10 @@ main() { test_ray_aabb() test_large_world_manifold() test_large_world_aabb() - // The manifold module's scratch, four blocks made on its first use and - // kept, is all that stays. - ensure("every other counted allocation was freed (${basics.alloc_count() - before})", basics.alloc_count() == before + 4) + // The manifold module's scratch, six blocks made on its first use and + // kept (the clip polygons, the points, the edge rows and lists, the + // support search's lanes), is all that stays. + ensure("every other counted allocation was freed (${basics.alloc_count() - before})", basics.alloc_count() == before + 6) println("collision: ${checks} checks") if failures == 0 { diff --git a/tests/test_compound.ae b/tests/test_compound.ae index 3f56795..82fa6f0 100644 --- a/tests/test_compound.ae +++ b/tests/test_compound.ae @@ -1119,6 +1119,48 @@ test_serialize() { mesh.destroy_mesh(md) } +// #158: a compound of big hulls (a 40-sided cylinder has 120 edges, 240 +// half-edges, near the builder's 128-edge limit) +// goes through its bytes and back, its hulls checked whole: the half-edge +// bound is twice the edge limit, as the builder's is. +test_serialize_big_hulls() { + cylinder = hull.create_cylinder(2.0, 1.0, 0.0, 40) + complex = hull.create_complex_hull(1.0) + ensure("big hulls: made", cylinder != null && complex != null) + if cylinder == null || complex == null { + if cylinder != null { hull.destroy_hull(cylinder) } + if complex != null { hull.destroy_hull(complex) } + return + } + ensure("big hulls: past 128 half-edges (${cylinder.edge_count}, ${complex.edge_count})", cylinder.edge_count > 128 && complex.edge_count > 128) + ensure("big hulls: each valid as bytes", hull.hull_bytes_valid(cylinder, cylinder.byte_count) && hull.hull_bytes_valid(complex, complex.byte_count)) + mat = material.default_surface_material() + hulls_block = calloc(2, sizeof(CompoundHullDef)) + defs = hulls_block as CompoundHullDef[] + defs[0] = compound.compound_hull_def(cylinder, Transform { p: math.vec3(0.0 - 3.0, 0.0, 0.0), q: math.quat_identity() }, mat) + defs[1] = compound.compound_hull_def(complex, Transform { p: math.vec3(3.0, 0.0, 0.0), q: math.quat_identity() }, mat) + def = compound.compound_def() + def.hulls = hulls_block + def.hull_count = 2 + c = compound.create_compound(&def) + ensure("big hulls: the compound is made", c != null) + if c != null { + byte_count = c.byte_count + buffer = bytes_of(c) + compound.destroy_compound(c) + back = compound.convert_bytes_to_compound(buffer, byte_count) + ensure("big hulls: their compound's bytes turn back into it", back != null && back.hull_count == 2) + if back != null { + ray = compound.ray_cast_compound(back, math.vec3(0.0 - 10.0, 0.0, 0.0), math.vec3(20.0, 0.0, 0.0), 1.0) + ensure("big hulls: a ray finds the cylinder first", ray.hit && ray.child_index == 0) + } + free(buffer) + } + free(hulls_block) + hull.destroy_hull(cylinder) + hull.destroy_hull(complex) +} + // A copy of the good bytes with one thing broken, refused. corrupt_refused(good: ptr, byte_count: int, what: int) -> bool { copy = calloc(1, byte_count) @@ -1180,6 +1222,7 @@ main() { test_field() test_brute_force_rays() test_serialize() + test_serialize_big_hulls() // The scratch stays allocated: compound's five blocks (its baked trees' per-worker stacks among them), mesh's three, sphere's two, shape's one. ensure("every other counted allocation was freed (${basics.alloc_count() - before})", basics.alloc_count() == before + 11) diff --git a/tests/test_manifold.ae b/tests/test_manifold.ae index 55b76f0..c4eb88c 100644 --- a/tests/test_manifold.ae +++ b/tests/test_manifold.ae @@ -996,9 +996,10 @@ main() { test_sphere_hull_seam() test_rounded_pairs() // The module's scratch stays allocated: the two clip polygons, the - // unreduced points, and the edge axes the separating-axis test - // gathers A's edges into. - ensure("every other counted allocation was freed", basics.alloc_count() == before + 4) + // unreduced points, the edge axes the separating-axis test gathers A's + // edges into, its two hulls' candidate edge lists, and the support + // search's lanes. + ensure("every other counted allocation was freed (${basics.alloc_count() - before})", basics.alloc_count() == before + 6) println("manifold: ${checks} checks") if failures == 0 { From 35ece7ca23bfee0450558bfb2fb9baa9fc216c4c Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Fri, 9 Oct 2026 02:10:22 -0300 Subject: [PATCH 3/4] Outward rounding inline and without branches; queries compare widened The narrowing of a coordinate to the float at or below (or above) it is the bit step nextafterf takes, inline in the native header and without branches, checked against libm's at zero, the denormals and the largest float. A tree query compares the double box against each node's float box widened, which is exact (a node holds every double box under it), so a query pays no rounding at all. Small hulls (16 vertices or fewer) are searched one vertex at a time in double, where the lanes' reduction cost more than it saved. --- aephysics/dynamic_tree/module.ae | 12 +++++----- aephysics/manifold/module.ae | 8 +++++++ aephysics/math/module.ae | 19 ++++----------- aephysics/native/aephysics_inline.h | 36 +++++++++++++++++++++++++++++ aephysics/native/module.ae | 9 +++++++- tests/test_dynamic_tree.ae | 8 +++---- tests/test_math.ae | 10 +++++++- 7 files changed, 76 insertions(+), 26 deletions(-) diff --git a/aephysics/dynamic_tree/module.ae b/aephysics/dynamic_tree/module.ae index 85748ed..73cd9a3 100644 --- a/aephysics/dynamic_tree/module.ae +++ b/aephysics/dynamic_tree/module.ae @@ -1001,9 +1001,9 @@ tree_query_on(t: *DynamicTree, stack_block: ptr, box: AABB, mask_bits: long, req nodes = t.nodes as DynamicTreeNode[] proxies = t.proxies as TreeProxy[] stack = stack_block as int[] - // The query box narrowed outward once, so a node it touches in double - // is never missed in single precision. - q = math.aabb_round_out(box) + // The double query box against each node's float box widened (exact): + // a node's box holds every double box stored under it, so nothing the + // query touches is missed, and no rounding is paid for per query. stack_count = 0 stack[stack_count] = root_pair(nodes) stack_count = stack_count + 1 @@ -1018,9 +1018,9 @@ tree_query_on(t: *DynamicTree, stack_block: ptr, box: AABB, mask_bits: long, req i = i + 1 lo = nodes[index].aabb.lower up = nodes[index].aabb.upper - if up.x < q.lower.x || lo.x > q.upper.x { continue } - if up.y < q.lower.y || lo.y > q.upper.y { continue } - if up.z < q.lower.z || lo.z > q.upper.z { continue } + if (up.x as float) < box.lower.x || (lo.x as float) > box.upper.x { continue } + if (up.y as float) < box.lower.y || (lo.y as float) > box.upper.y { continue } + if (up.z as float) < box.lower.z || (lo.z as float) > box.upper.z { continue } flags = nodes[index].flag_index if is_leaf(flags) { proxy_id = proxy_id_of(flags) diff --git a/aephysics/manifold/module.ae b/aephysics/manifold/module.ae index d30d938..84cf382 100644 --- a/aephysics/manifold/module.ae +++ b/aephysics/manifold/module.ae @@ -277,6 +277,9 @@ const EDGE_LIST_B = 128 const EDGE_LIST_BYTES = 2 * 128 * 4 // The pairs left that pay for the probe's second cull (B3_EDGE_PROBE_MIN_TESTS). const EDGE_PROBE_MIN_TESTS = 10 +// The hulls at or under this many vertices are searched one vertex at a time. +const WIDE_SUPPORT_MIN_VERTICES = 16 + // How far under the best edge separation a lane's single-precision // separation may fall and still be measured in double: well past a float's // rounding at the hulls' scale (the test runs in A's frame, the hulls @@ -1312,6 +1315,11 @@ filter_edge_candidates(planes: Plane[], edges: HalfEdge[], cosines: f32[], probe // where that one ties values within 2^-16 of each other. The caller // measures the separation of the chosen vertex in double. support_index_wide(h: *HullData, direction: Vec3) -> int { + // A small hull is searched in double, one vertex at a time: the lanes' + // reduction costs more than eight or sixteen dot products (measured on + // the reference's rocks, ten vertices, and boxes and six-sided + // cylinders), and the answer is the same first maximum. + if h.vertex_count <= WIDE_SUPPORT_MIN_VERTICES { return support_index(hull.hull_points(h), h.vertex_count, direction) } scratch_ready() stride = hull.hull_soa_stride(h.vertex_count) rows = hull.hull_soa_vertices(h) diff --git a/aephysics/math/module.ae b/aephysics/math/module.ae index 9a8ff38..47ae306 100644 --- a/aephysics/math/module.ae +++ b/aephysics/math/module.ae @@ -21,6 +21,7 @@ // is_normalized, ...) a caller checks where it matters, and the functions // return what the original returns on the degenerate case. import std.string +import aephysics.native exports ( Vec2, Vec3, CosSin, Quat, Transform, Matrix2, Matrix3, AABB, Plane, SegmentDistanceResult, @@ -73,9 +74,6 @@ extern sqrt(x: float) -> float // libm's single-precision square root: the one the f32 family uses, so a // length is computed at the width it is stored at. extern sqrtf(x: f32) -> f32 -// The next float toward a direction: exact IEEE, so the outward rounding -// of a box is the same on every platform. -extern nextafterf(x: f32, y: f32) -> f32 extern remainder(x: float, y: float) -> float const PI = 3.14159265359 @@ -1266,19 +1264,12 @@ struct AABBf { infinity_f32() -> f32 { return infinity() as f32 } // A coordinate narrowed toward negative infinity (b3RoundDownFloat): the -// nearest float at or below it. -round_down_f32(x: float) -> f32 { - f = x as f32 - if (f as float) > x { return nextafterf(f, 0.0 - infinity_f32()) } - return f -} +// nearest float at or below it. The step to the next float is exact IEEE +// (nextafterf's, done on the bits inline), so every platform gets the same. +round_down_f32(x: float) -> f32 { return native.round_down_f32(x) } // A coordinate narrowed toward positive infinity (b3RoundUpFloat). -round_up_f32(x: float) -> f32 { - f = x as f32 - if (f as float) < x { return nextafterf(f, infinity_f32()) } - return f -} +round_up_f32(x: float) -> f32 { return native.round_up_f32(x) } // The box narrowed outward (the rounding of b3OffsetAABB): it holds `a`. aabb_round_out(a: AABB) -> AABBf { diff --git a/aephysics/native/aephysics_inline.h b/aephysics/native/aephysics_inline.h index 2091b85..f812ec0 100644 --- a/aephysics/native/aephysics_inline.h +++ b/aephysics/native/aephysics_inline.h @@ -86,4 +86,40 @@ static inline long long aephysics_f64_bits(double value) return bits; } +// A double narrowed to the nearest float at or below it, and at or above +// it: the bounds of a single-precision box rounded outward (the +// reference's b3RoundDownFloat and b3RoundUpFloat). The step to the next +// float is the one on its bits that nextafterf takes, inline: a box is +// rounded on every tree query, and the libm call was most of the cost. +// Zero steps to the smallest denormal of the other sign, the largest +// float to infinity and infinity to the largest float, as nextafterf's do; +// a NaN is never stepped. Written without branches: whether a coordinate +// needs the step is a coin flip, and a branch on it mispredicted half the +// time (the continuous pass rounds a box for every fast shape). +static inline float aephysics_round_down_f32(double x) +{ + float f = (float)x; + unsigned int bits; + __builtin_memcpy(&bits, &f, 4); + unsigned int need = (double)f > x; + unsigned int stepped = bits + ((bits >> 31) ? 1u : 0xFFFFFFFFu); + stepped = f == 0.0f ? 0x80000001u : stepped; + bits = need ? stepped : bits; + __builtin_memcpy(&f, &bits, 4); + return f; +} + +static inline float aephysics_round_up_f32(double x) +{ + float f = (float)x; + unsigned int bits; + __builtin_memcpy(&bits, &f, 4); + unsigned int need = (double)f < x; + unsigned int stepped = bits + ((bits >> 31) ? 0xFFFFFFFFu : 1u); + stepped = f == 0.0f ? 0x00000001u : stepped; + bits = need ? stepped : bits; + __builtin_memcpy(&f, &bits, 4); + return f; +} + #endif diff --git a/aephysics/native/module.ae b/aephysics/native/module.ae index 09b3154..a20f5ae 100644 --- a/aephysics/native/module.ae +++ b/aephysics/native/module.ae @@ -11,7 +11,8 @@ exports ( MAX_WORKERS, worker_index, set_worker_index, claim_slots, release_slots, enter_traversal, leave_traversal, yield_thread, pause, processor_count, scratch, atomic_load, atomic_store, atomic_add, atomic_or, atomic_cas, semaphore_new, semaphore_free, semaphore_wait, semaphore_signal, - name_copy, name_text, name_free, prefetch, load_word, load_u8, load_u16, f32_bits, f64_bits + name_copy, name_text, name_free, prefetch, load_word, load_u8, load_u16, f32_bits, f64_bits, + round_down_f32, round_up_f32 ) // The most workers a world can have: every module's scratch is a block @@ -42,6 +43,8 @@ extern aephysics_load_u8(p: ptr, index: long) -> int @c_import extern aephysics_load_u16(p: ptr, index: long) -> int @c_import extern aephysics_f32_bits(value: f32) -> long @c_import extern aephysics_f64_bits(value: float) -> long @c_import +extern aephysics_round_down_f32(x: float) -> f32 @c_import +extern aephysics_round_up_f32(x: float) -> f32 @c_import extern aephysics_name_copy(s: string) -> ptr extern aephysics_name_text(name: ptr) -> string extern free(p: ptr) @@ -64,6 +67,10 @@ load_u16(p: ptr, index: long) -> int { return aephysics_load_u16(p, index) } f32_bits(value: f32) -> long { return aephysics_f32_bits(value) } f64_bits(value: float) -> long { return aephysics_f64_bits(value) } +// A double narrowed to the nearest float at or below it, or at or above. +round_down_f32(x: float) -> f32 { return aephysics_round_down_f32(x) } +round_up_f32(x: float) -> f32 { return aephysics_round_up_f32(x) } + // A name the world keeps: a plain copy of `s` (null for none), its text // (empty for none), and its release. name_copy(s: string) -> ptr { return aephysics_name_copy(s) } diff --git a/tests/test_dynamic_tree.ae b/tests/test_dynamic_tree.ae index f8c0def..887aadd 100644 --- a/tests/test_dynamic_tree.ae +++ b/tests/test_dynamic_tree.ae @@ -819,7 +819,8 @@ corrupted(block: ptr, byte_count: int, what: int) -> bool { // Single-precision nodes far out (#151): 300 centimetre boxes at 1e7, // each held by its leaf, the root holding them all; every proxy a double // query box touches is found (the tree may find more, a float being a -// metre there), and exactly the ones whose rounded-out boxes touch it. +// metre there), and exactly the ones whose stored (rounded-out) boxes the +// query box touches, compared in double. test_far_boxes() { seed = 777 tree = dynamic_tree.tree_create(16) @@ -855,19 +856,18 @@ test_far_boxes() { query = math.aabb(math.sub(center, math.vec3(r, r, r)), math.add(center, math.vec3(r, r, r))) tree_hits = hitset_create() dynamic_tree.tree_query(&tree, query, dynamic_tree.default_mask_bits(), false, collect, (&tree_hits) as ptr) - q = math.aabb_round_out(query) i = 0 while i < PROXY_COUNT { found = hitset_has(&tree_hits, i) if math.aabb_overlaps(boxes[i], query) && found == false { superset = false } - if math.aabbf_overlaps(math.aabb_round_out(boxes[i]), q) != found { exact = false } + if math.aabb_overlaps(math.aabb_widen(math.aabb_round_out(boxes[i])), query) != found { exact = false } i = i + 1 } hitset_destroy(&tree_hits) round = round + 1 } ensure("far: every box a query touches in double is found", superset) - ensure("far: and exactly the rounded-out boxes it touches", exact) + ensure("far: and exactly the stored boxes it touches", exact) free(boxes_block) dynamic_tree.tree_destroy(&tree) } diff --git a/tests/test_math.ae b/tests/test_math.ae index bca1c6d..4ce82ed 100644 --- a/tests/test_math.ae +++ b/tests/test_math.ae @@ -70,7 +70,15 @@ outward_rounding() { i = i + 1 } ensure("rounding: down at or below, up at or above, every value", held) - ensure("rounding: one float when exact, adjacent floats when not", tight) + ensure("rounding: one float when exact, adjacent floats when not (libm's nextafterf the judge)", tight) + // The inline step against libm's at zero, at the largest float and at + // the smallest denormals: the same float every time. + tiny = 1.0e-46 + ensure("rounding: just under zero is the smallest negative denormal", math.round_down_f32(0.0 - tiny) == nextafterf(0.0, 0.0 - math.infinity_f32())) + ensure("rounding: just over zero is the smallest positive denormal", math.round_up_f32(tiny) == nextafterf(0.0, math.infinity_f32())) + big = 3.40282347e38 * 1.0000001 + ensure("rounding: past the largest float, down is the largest float and up infinity", + math.round_down_f32(big) == nextafterf(math.infinity_f32(), 0.0) && (math.round_up_f32(big) as float) == 1.0 / g_zero_m) inf = 1.0 / g_zero_m ensure("rounding: the infinities stay infinite", (math.round_down_f32(inf) as float) == inf && (math.round_up_f32(0.0 - inf) as float) == 0.0 - inf) far = math.aabb(math.vec3(10000000.01, 0.0 - 0.3, 5.7), math.vec3(10000000.02, 0.4, 5.71)) From 1d9e6288508194c4b4c0275e6d57835edb2dacf2 Mon Sep 17 00:00:00 2001 From: Nicolas Maman Date: Fri, 9 Oct 2026 02:15:42 -0300 Subject: [PATCH 4/4] RESULTS: the pairs phase and the wide separating axis test (#151, #150) --- bench/RESULTS.md | 59 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 59 insertions(+) diff --git a/bench/RESULTS.md b/bench/RESULTS.md index 20ebeeb..ffd7410 100644 --- a/bench/RESULTS.md +++ b/bench/RESULTS.md @@ -1246,6 +1246,65 @@ reference. That is the convex pile's gap, and #150. Across the scenes the pairs phase is 1.4-1.6x the reference's (2.59 against 1.63 ms on the convex pile): the broad phase's boxes are double, where the reference keeps float boxes rounded outward even in its large-world build (#151). +Both are taken up below. + +### The pairs phase and the wide separating axis test (2026-10-09, #151, #150) + +The tree's boxes are single precision now, rounded outward from double +as the reference's large-world build rounds them: 32-byte nodes, the +world's fat boxes in float, the insertion costs and the rays measured on +the widened boxes. Measured alone, that did not move the pairs phase +(main 1.64-1.82 ms on the convex pile, the float boxes 1.68-1.77). The +profile said why: two thirds of the walk's samples were the pair set's +probe, a cache miss on every candidate in a table larger than the +caches. The reference batches its candidates and prefetches every slot +before it probes any (b3FlushCandidatePairs); so do we now, 32 at a +time, and the walk reads a leaf's shape index from the node instead of +the proxy array. The pairs phase, ms per step, the better of two pinned +runs, Box3D `e77352c` AVX2 and ours AVX2 alternating, one session: + +| scene | Box3D | before | after | +|---|---|---|---| +| convex_pile | 1.24 | 1.82 | 1.44 | +| junkyard | 2.00 | 2.88 | 2.21 | +| washer | 1.83 | 2.65 | 2.21 | +| spinner | 0.284 | 0.403 | 0.353 | + +The separating axis test reads the hulls' new single-precision rows: +support searches eight vertices a pass (a hull of 16 vertices or fewer +is searched one vertex at a time in double, where the lanes' reduction +cost more than it saved), edge candidates culled with the stored +cosines and again by the best faces' probe points, edge pairs eight a +pass with the axis and separation in lanes. Every winner is measured +again in double, so the golden trace did not change. The SAT runs, +microseconds a query, before / after / the reference: + +| pair | SAT | SAT, no inscribed-sphere bound | +|---|---|---| +| complex/complex | 1.76 / 1.60 / 0.95 | 16.2 / 12.9 / 6.01 | +| complex/rock | 1.00 / 1.04 / 0.63 | 5.57 / 5.11 / 2.73 | +| complex/cylinder | 2.77 / 2.02 / 1.06 | 14.8 / 12.8 / 5.98 | +| rock/rock | 0.545 / 0.600 / 0.375 | 1.91 / 1.93 / 1.17 | +| rock/cylinder | 1.51 / 1.34 / 0.74 | 4.88 / 4.80 / 3.01 | +| cylinder/cylinder | 4.73 / 2.58 / 1.41 | 13.0 / 10.2 / 7.42 | + +(The rock rows were measured before the small-hull search went back to +double; the rocks have ten vertices and take that path now.) The rest of +the gap is the work around the lanes that the reference does in lanes +too: the face dots, B taken into A's frame, A's edges gathered, and the +reduction of a support search's lanes, which std.lanes cannot do in one +step yet (aether#2603, aether#2550). #150 stays open for it. + +The whole step with all of it, ms per step, the better of two runs, ours +before and after (that session was noisier for both engines, so the +ratio to the reference is from the table above): convex_pile 11.84 to +11.26, junkyard 28.3 to 25.0, washer 17.6 to 16.7, spinner 4.31 to 4.08, +sleep 2.40 to 2.33, large_world 0.0114 to 0.0111, trees25 1.589 to 1.587, +trees50 0.573 to 0.590, trees100 0.334 to 0.334. The trees scenes pay for +the outward rounding: their fast shapes leave their fat boxes every +step, and each new box is rounded (the inline, branch-free bit step of +nextafterf); a tree query no longer rounds at all, the double box +compared against the widened nodes. ## Build flags (inlining)