Mover queries hand their visitors planes and segments of their own

aephysics/physics_world/module.ae

@@ -475,24 +475,17 @@ struct WorldMoverContext {
     mover: Capsule
     origin: Vec3
     user_context: ptr
+    planes: ptr              // PlaneResult[MOVER_PLANE_CAPACITY], this query's own
 }
 
 const MOVER_PLANE_CAPACITY = 64
-var g_mover_planes: ptr = null   // PlaneResult[MOVER_PLANE_CAPACITY]
-mover_plane_block() -> ptr { return native.scratch(g_mover_planes, MOVER_PLANE_CAPACITY * sizeof(PlaneResult)) }
-
-mover_planes() -> PlaneResult[] {
-    if g_mover_planes == null { g_mover_planes = basics.alloc(native.MAX_WORKERS * (MOVER_PLANE_CAPACITY * sizeof(PlaneResult))) }
-    return mover_plane_block() as PlaneResult[]
-}
-
 mover_collide_visitor(proxy_id: int, user_data: long, context: ptr) -> bool {
     mc = context as *WorldMoverContext
     w = mc.w
     s = dynamics.world_shape_at(w, user_data as int)
     if shape.should_query_collide(s.filter, mc.filter) == false { return true }
     t = relative_transform(w, s.body_id, mc.origin)
-    planes = mover_planes()
+    planes = mc.planes as PlaneResult[]
     count = shape.collide_mover(planes, MOVER_PLANE_CAPACITY, &s.geometry, t, mc.mover)
     if count > 0 {
         visitor = mc.visitor
@@ -509,12 +502,17 @@ collide_mover(id: WorldId, origin: Vec3, mover: Capsule, filter: QueryFilter, vi
     if w == null { return }
     r = math.vec3(mover.radius, mover.radius, mover.radius)
     box = math.offset_aabb(AABB { lower: math.sub(math.min_vec3(mover.center1, mover.center2), r), upper: math.add(math.max_vec3(mover.center1, mover.center2), r) }, origin)
-    mc = WorldMoverContext { w: w, visitor: visitor, filter: filter, mover: mover, origin: origin, user_context: context }
+    // The planes a visitor reads are this query's own block, as the
+    // reference's are on its stack: a visitor that queries the mover again
+    // (a character checking a platform) does not overwrite them.
+    planes_block = basics.alloc(MOVER_PLANE_CAPACITY * sizeof(PlaneResult))
+    mc = WorldMoverContext { w: w, visitor: visitor, filter: filter, mover: mover, origin: origin, user_context: context, planes: planes_block }
     i = 0
     while i < BODY_KIND_COUNT {
         dynamic_tree.tree_query(broad_phase.broad_phase_tree(&w.broad_phase, i), box, filter.mask_bits, false, mover_collide_visitor, (&mc) as ptr)
         i = i + 1
     }
+    basics.free_bytes(planes_block, MOVER_PLANE_CAPACITY * sizeof(PlaneResult))
 }
 
 struct WorldCastContext {
@@ -657,6 +655,8 @@ struct WorldMoverCastContext {
     translation: Vec3
     can_encroach: bool
     user_context: ptr
+    point1: Vec3             // the mover's segment, which proxy points at: this cast's own
+    point2: Vec3
 }
 
 mover_cast_visitor(input: *BoxCastInput, proxy_id: int, user_data: long, context: ptr) -> float {
@@ -684,14 +684,14 @@ mover_points() -> ptr { return native.scratch(g_mover_points, 2 * sizeof(Vec3))
 cast_mover(id: WorldId, origin: Vec3, mover: Capsule, translation: Vec3, filter: QueryFilter, has_filter: bool, filter_fn: fn(ShapeId, ptr) -> bool, context: ptr) -> float {
     w = world_of(id)
     if w == null { return 1.0 }
-    if g_mover_points == null { g_mover_points = basics.alloc(native.MAX_WORKERS * (2 * sizeof(Vec3))) }
-    points = mover_points() as Vec3[]
-    points[0] = mover.center1
-    points[1] = mover.center2
+    // The segment lives in this cast's context, as the reference's on its
+    // stack: a filter that casts again (or asks a body's time of impact) does
+    // not move it under this cast.
     mc = WorldMoverCastContext { w: w, filter_fn: filter_fn, has_filter: has_filter, filter: filter, fraction: 1.0, origin: origin,
-                            proxy: distance.shape_proxy(mover_points(), 2, mover.radius), translation: translation,
-                            can_encroach: mover.radius > 0.0, user_context: context }
-    input = BoxCastInput { box: math.offset_aabb(math.make_aabb(mover_points() as float[], 2, mover.radius), origin), translation: translation, max_fraction: 1.0 }
+                            proxy: distance.shape_proxy(null, 2, mover.radius), translation: translation,
+                            can_encroach: mover.radius > 0.0, user_context: context, point1: mover.center1, point2: mover.center2 }
+    mc.proxy.points = (&mc.point1) as ptr
+    input = BoxCastInput { box: math.offset_aabb(math.make_aabb(mc.proxy.points as float[], 2, mover.radius), origin), translation: translation, max_fraction: 1.0 }
     i = 0
     while i < BODY_KIND_COUNT {
         dynamic_tree.tree_box_cast(broad_phase.broad_phase_tree(&w.broad_phase, i), &input, filter.mask_bits, false, mover_cast_visitor, (&mc) as ptr)
@@ -797,15 +797,15 @@ body_collide_mover(id: BodyId, planes: BodyPlaneResult[], capacity: int, origin:
     body = dynamics.get_body_full_id(w, id)
     t = Transform { p: math.sub(body_transform.p, origin), q: body_transform.q }
     count = 0
-    one = mover_planes()
     shape_id = body.head_shape_id
     while shape_id != NULL_INDEX {
         s = dynamics.world_shape_at(w, shape_id)
         shape_id = s.next_shape_id
         if shape.should_query_collide(s.filter, filter) == false { continue }
         if shape.is_convex(s.geometry.kind) == false { continue }
-        if shape.collide_mover(one, 1, &s.geometry, t, mover) > 0 {
-            planes[count] = BodyPlaneResult { shape_id: dynamics.make_shape_id(w, s), result: one[0] }
+        // Straight into the caller's result, no shared scratch to overwrite.
+        if shape.collide_mover((&planes[count].result) as ptr as PlaneResult[], 1, &s.geometry, t, mover) > 0 {
+            planes[count].shape_id = dynamics.make_shape_id(w, s)
             count = count + 1
             if count == capacity { return count }
         }

tests/test_mover_world.ae

@@ -96,6 +96,60 @@ capture_planes(shape_id: ShapeId, planes: PlaneResult[], count: int, context: pt
     return true
 }
 
+// A visitor that asks a platform body for its plane mid-query, as a
+// character controller does, and checks its own planes did not move.
+struct NestedQuery {
+    platform: BodyId
+    before_normal: Vec3
+    before_separation: float
+    after_normal: Vec3
+    after_separation: float
+    platform_planes: int
+    visits: int
+}
+
+nested_visitor(shape_id: ShapeId, planes: PlaneResult[], count: int, context: ptr) -> bool {
+    nq = context as *NestedQuery
+    nq.visits = nq.visits + 1
+    nq.before_normal = planes[0].plane.normal
+    nq.before_separation = planes[0].plane.offset
+    found_block = calloc(4, sizeof(BodyPlaneResult))
+    found = found_block as BodyPlaneResult[]
+    nq.platform_planes = physics_world.body_collide_mover(nq.platform, found, 4, math.vec3_zero(), capsule_at(5.0, 1.15, 1.35, 0.2),
+                                                          shape.default_query_filter(), dynamics.body_get_transform(nq.platform))
+    free(found_block)
+    nq.after_normal = planes[0].plane.normal
+    nq.after_separation = planes[0].plane.offset
+    return false
+}
+
+test_nested_mover_queries() {
+    wdef = dynamics.default_world_def()
+    world_id = dynamics.create_world(&wdef)
+    gdef = dynamics.default_body_def()
+    gdef.position = math.vec3(0.0, 0.0 - 0.5, 0.0)
+    ground = dynamics.create_body(world_id, &gdef)
+    slab = hull.make_box_hull(4.0, 0.5, 4.0)
+    sdef = dynamics.default_shape_def()
+    dynamics.create_hull_shape(ground, &sdef, slab)
+    pdef = dynamics.default_body_def()
+    pdef.position = math.vec3(5.0, 1.0, 0.0)
+    pdef.rotation = math.make_quat_from_axis_angle(math.vec3(0.0, 0.0, 1.0), 0.4)
+    platform = dynamics.create_body(world_id, &pdef)
+    plank = hull.make_box_hull(1.0, 0.1, 1.0)
+    dynamics.create_hull_shape(platform, &sdef, plank)
+    physics_world.step(world_id, 1.0 / 60.0, 1)
+    nq = NestedQuery { platform: platform, before_normal: math.vec3_zero(), before_separation: 0.0, after_normal: math.vec3_zero(),
+                       after_separation: 0.0, platform_planes: 0, visits: 0 }
+    physics_world.collide_mover(world_id, math.vec3_zero(), capsule_at(0.0, 0.15, 0.35, 0.2), shape.default_query_filter(), nested_visitor, (&nq) as ptr)
+    ensure("nested: the ground visited, the platform found (${nq.visits}, ${nq.platform_planes})", nq.visits == 1 && nq.platform_planes == 1)
+    ensure("nested: the ground's plane unchanged by the platform query",
+           nq.after_normal.x == nq.before_normal.x && nq.after_normal.y == nq.before_normal.y && nq.after_separation == nq.before_separation)
+    dynamics.destroy_world(world_id)
+    hull.destroy_hull(slab)
+    hull.destroy_hull(plank)
+}
+
 capsule_at(x: float, y0: float, y1: float, radius: float) -> Capsule {
     return Capsule { center1: math.vec3(x, y0, 0.0), center2: math.vec3(x, y1, 0.0), radius: radius }
 }
@@ -315,6 +369,7 @@ main() {
     test_body_material_indices()
     test_body_skips_mesh_and_compound()
     test_world_mesh_material_clamp()
+    test_nested_mover_queries()
     println("mover_world: ${checks} checks")
     if failures == 0 {
         println("mover_world: all checks passed")

tests/test_physics_world.ae

@@ -904,8 +904,8 @@ main() {
     test_body_queries()
     test_world_queries()
     test_determinism()
-    // The scratch stays allocated: the worlds, the masses, the sweep and query buffers, the solvers' dummies (the modules' scratch is reserved before the count).
-    ensure("every other counted allocation was freed (${basics.alloc_count() - before})", basics.alloc_count() == before + 9)
+    // The scratch stays allocated: the worlds, the masses, the sweep and query buffers, the solvers' dummies (the modules' scratch is reserved before the count). The mover's planes are each query's own now, freed with it.
+    ensure("every other counted allocation was freed (${basics.alloc_count() - before})", basics.alloc_count() == before + 8)
     println("physics_world: ${checks} checks")
     if failures == 0 {
         println("physics_world: all checks passed")