feat(physics): add BSPQuery.FindWalkableSphere wrapper
Thin public wrapper over the existing retail-faithful FindWalkableInternal (BSPNODE::find_walkable + BSPLEAF::find_walkable port). Probes downward by probeDistance along up, returns the closest walkable polygon the sphere would rest on plus the adjusted center. Will replace Transition.TryFindIndoorWalkablePlane's linear first-match scan (next commit). The wrapper is callable from any "stand here, find my floor" use case; current intent is indoor walkable-plane synthesis. 4 unit tests covering: two-floors-foot-between (sphere overlapping lower floor), only-upper-floor-foot-above (sphere overlapping upper floor), no-walkable-in-probe-range (sphere out of overlap distance for all polygons), steep-poly-rejected-by-WalkableAllowance. Note: find_walkable requires sphere to overlap the polygon plane (|dist| <= radius); the tests use geometry that exercises this correctly, unlike the spec's illustrative values which assumed a "nearest below" scan. Spec: docs/superpowers/specs/2026-05-19-indoor-walkable-plane-bsp-port-design.md Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
ff548b962c
commit
7f55e14cd7
2 changed files with 290 additions and 0 deletions
|
|
@ -1130,6 +1130,89 @@ public static class BSPQuery
|
||||||
return TransitionState.OK;
|
return TransitionState.OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// -------------------------------------------------------------------------
|
||||||
|
// find_walkable_sphere — "stand here, find my contact plane"
|
||||||
|
// Indoor walkable-plane synthesis entry point (Phase 2 follow-up 2026-05-19).
|
||||||
|
// -------------------------------------------------------------------------
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// "Stand here, find my contact plane" entry point over the BSPNode/BSPLeaf
|
||||||
|
/// find_walkable BSP traversal. Probes downward by <paramref name="probeDistance"/>
|
||||||
|
/// along <paramref name="up"/> and returns the closest walkable polygon the
|
||||||
|
/// sphere would rest on, with the sphere's center adjusted to lie on that plane.
|
||||||
|
///
|
||||||
|
/// <para>
|
||||||
|
/// Wraps the existing private <see cref="FindWalkableInternal"/> — which already
|
||||||
|
/// implements the retail-faithful walkable-finder
|
||||||
|
/// (BSPNODE::find_walkable + BSPLEAF::find_walkable +
|
||||||
|
/// CPolygon::walkable_hits_sphere + CPolygon::adjust_sphere_to_plane,
|
||||||
|
/// acclient_2013_pseudo_c.txt:326211, :326793, :323006, :322032).
|
||||||
|
/// </para>
|
||||||
|
///
|
||||||
|
/// <para>
|
||||||
|
/// Intended call site: indoor walkable-plane synthesis in
|
||||||
|
/// <c>Transition.TryFindIndoorWalkablePlane</c> when the indoor cell-BSP
|
||||||
|
/// collision returns OK (no wall hit) and the resolver still needs a
|
||||||
|
/// ContactPlane to feed ValidateWalkable. Outdoor terrain has its own path
|
||||||
|
/// (<see cref="PhysicsEngine.SampleTerrainWalkable"/>) and does not use this.
|
||||||
|
/// </para>
|
||||||
|
///
|
||||||
|
/// <para>
|
||||||
|
/// The caller is responsible for setting <c>transition.SpherePath.WalkableAllowance</c>
|
||||||
|
/// to the desired walkability threshold (typically <see cref="PhysicsGlobals.FloorZ"/>)
|
||||||
|
/// before calling, and restoring it after.
|
||||||
|
/// </para>
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="root">Root of the cell's PhysicsBSP.</param>
|
||||||
|
/// <param name="resolved">Pre-resolved polygon dictionary from PhysicsDataCache.</param>
|
||||||
|
/// <param name="transition">Current transition (read for WalkableAllowance / walk_interp).</param>
|
||||||
|
/// <param name="sphere">Foot sphere in cell-local space.</param>
|
||||||
|
/// <param name="probeDistance">Downward probe distance in meters. Typical: 0.5f.</param>
|
||||||
|
/// <param name="up">Up vector in cell-local space (typically Vector3.UnitZ).</param>
|
||||||
|
/// <param name="hitPoly">Output: the walkable polygon found, or null on miss.</param>
|
||||||
|
/// <param name="hitPolyId">Output: polygon id (dictionary key) of the hit. Zero on miss.</param>
|
||||||
|
/// <param name="adjustedCenter">
|
||||||
|
/// Output: sphere center adjusted onto the polygon plane. Equal to input
|
||||||
|
/// <c>sphere.Origin</c> on miss.
|
||||||
|
/// </param>
|
||||||
|
/// <returns>True if a walkable polygon was found; false otherwise.</returns>
|
||||||
|
public static bool FindWalkableSphere(
|
||||||
|
PhysicsBSPNode? root,
|
||||||
|
Dictionary<ushort, ResolvedPolygon> resolved,
|
||||||
|
Transition transition,
|
||||||
|
Sphere sphere,
|
||||||
|
float probeDistance,
|
||||||
|
Vector3 up,
|
||||||
|
out ResolvedPolygon? hitPoly,
|
||||||
|
out ushort hitPolyId,
|
||||||
|
out Vector3 adjustedCenter)
|
||||||
|
{
|
||||||
|
adjustedCenter = sphere.Origin;
|
||||||
|
hitPoly = null;
|
||||||
|
hitPolyId = 0;
|
||||||
|
|
||||||
|
if (root is null) return false;
|
||||||
|
|
||||||
|
var validPos = new CollisionSphere(sphere.Origin, sphere.Radius);
|
||||||
|
var movement = -up * probeDistance;
|
||||||
|
bool changed = false;
|
||||||
|
ushort polyId = 0;
|
||||||
|
ResolvedPolygon? poly = null;
|
||||||
|
|
||||||
|
FindWalkableInternal(root, resolved, transition.SpherePath, validPos,
|
||||||
|
movement, up, ref poly, ref polyId, ref changed);
|
||||||
|
|
||||||
|
if (changed && poly is not null)
|
||||||
|
{
|
||||||
|
hitPoly = poly;
|
||||||
|
hitPolyId = polyId;
|
||||||
|
adjustedCenter = validPos.Center;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
// -------------------------------------------------------------------------
|
// -------------------------------------------------------------------------
|
||||||
// step_sphere_up — BSPTree level
|
// step_sphere_up — BSPTree level
|
||||||
// ACE: BSPTree.cs step_sphere_up
|
// ACE: BSPTree.cs step_sphere_up
|
||||||
|
|
|
||||||
|
|
@ -204,4 +204,211 @@ public class BSPQueryTests
|
||||||
Assert.Null(cache.GetGfxObj(0x01000001u));
|
Assert.Null(cache.GetGfxObj(0x01000001u));
|
||||||
Assert.Null(cache.GetSetup(0x02000001u));
|
Assert.Null(cache.GetSetup(0x02000001u));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// =========================================================================
|
||||||
|
// FindWalkableSphere — indoor walkable-plane finder (spec 2026-05-19)
|
||||||
|
// =========================================================================
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Build a single-leaf BSP rooted at one node containing two horizontal
|
||||||
|
/// walkable polygons (id 0 at Z=lowerZ, id 1 at Z=upperZ), both covering
|
||||||
|
/// the unit square X[0..1] × Y[0..1]. Bounding sphere is sized to enclose
|
||||||
|
/// both polys.
|
||||||
|
/// </summary>
|
||||||
|
private static (PhysicsBSPNode root, Dictionary<ushort, ResolvedPolygon> resolved)
|
||||||
|
BuildTwoFloorsBsp(float lowerZ, float upperZ)
|
||||||
|
{
|
||||||
|
var center = new Vector3(0.5f, 0.5f, (lowerZ + upperZ) * 0.5f);
|
||||||
|
float halfHeight = MathF.Abs(upperZ - lowerZ) * 0.5f + 1.0f;
|
||||||
|
float radius = MathF.Sqrt(0.5f * 0.5f + 0.5f * 0.5f + halfHeight * halfHeight);
|
||||||
|
|
||||||
|
var root = new PhysicsBSPNode
|
||||||
|
{
|
||||||
|
Type = BSPNodeType.Leaf,
|
||||||
|
BoundingSphere = new Sphere { Origin = center, Radius = radius },
|
||||||
|
};
|
||||||
|
root.Polygons.Add(0);
|
||||||
|
root.Polygons.Add(1);
|
||||||
|
|
||||||
|
Vector3[] lowerVerts =
|
||||||
|
{
|
||||||
|
new Vector3(0f, 0f, lowerZ),
|
||||||
|
new Vector3(1f, 0f, lowerZ),
|
||||||
|
new Vector3(1f, 1f, lowerZ),
|
||||||
|
new Vector3(0f, 1f, lowerZ),
|
||||||
|
};
|
||||||
|
Vector3[] upperVerts =
|
||||||
|
{
|
||||||
|
new Vector3(0f, 0f, upperZ),
|
||||||
|
new Vector3(1f, 0f, upperZ),
|
||||||
|
new Vector3(1f, 1f, upperZ),
|
||||||
|
new Vector3(0f, 1f, upperZ),
|
||||||
|
};
|
||||||
|
|
||||||
|
var resolved = new Dictionary<ushort, ResolvedPolygon>
|
||||||
|
{
|
||||||
|
[0] = new ResolvedPolygon
|
||||||
|
{
|
||||||
|
Vertices = lowerVerts,
|
||||||
|
Plane = new Plane(Vector3.UnitZ, -lowerZ),
|
||||||
|
NumPoints = 4,
|
||||||
|
SidesType = CullMode.None,
|
||||||
|
},
|
||||||
|
[1] = new ResolvedPolygon
|
||||||
|
{
|
||||||
|
Vertices = upperVerts,
|
||||||
|
Plane = new Plane(Vector3.UnitZ, -upperZ),
|
||||||
|
NumPoints = 4,
|
||||||
|
SidesType = CullMode.None,
|
||||||
|
},
|
||||||
|
};
|
||||||
|
|
||||||
|
return (root, resolved);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Build a Transition with WalkableAllowance set to FloorZ — what the
|
||||||
|
/// indoor walkable-plane synthesis uses.
|
||||||
|
/// </summary>
|
||||||
|
private static Transition BuildFloorZTransition()
|
||||||
|
{
|
||||||
|
var transition = new Transition();
|
||||||
|
transition.SpherePath.WalkableAllowance = PhysicsGlobals.FloorZ;
|
||||||
|
transition.SpherePath.WalkInterp = 1.0f;
|
||||||
|
return transition;
|
||||||
|
}
|
||||||
|
|
||||||
|
[Fact]
|
||||||
|
public void FindWalkableSphere_TwoFloors_FootBetween_PicksLowerFloor()
|
||||||
|
{
|
||||||
|
// Two floors at Z=0 and Z=3. Foot sphere center at Z=0.4 (radius 0.48).
|
||||||
|
// The sphere overlaps the lower floor (|center.Z - 0| = 0.4 < radius 0.48),
|
||||||
|
// so find_walkable can resolve a rest position against it.
|
||||||
|
// Upper floor at Z=3 is out of range (dist=2.6 >> radius 0.48).
|
||||||
|
// Expect: pick lower floor (id 0).
|
||||||
|
var (root, resolved) = BuildTwoFloorsBsp(lowerZ: 0f, upperZ: 3f);
|
||||||
|
var transition = BuildFloorZTransition();
|
||||||
|
|
||||||
|
var sphere = new Sphere { Origin = new Vector3(0.5f, 0.5f, 0.4f), Radius = 0.48f };
|
||||||
|
|
||||||
|
bool found = BSPQuery.FindWalkableSphere(
|
||||||
|
root, resolved, transition,
|
||||||
|
sphere,
|
||||||
|
probeDistance: 0.5f,
|
||||||
|
up: Vector3.UnitZ,
|
||||||
|
out var hitPoly,
|
||||||
|
out var hitPolyId,
|
||||||
|
out var adjustedCenter);
|
||||||
|
|
||||||
|
Assert.True(found);
|
||||||
|
Assert.Equal((ushort)0, hitPolyId);
|
||||||
|
Assert.NotNull(hitPoly);
|
||||||
|
Assert.Equal(1f, hitPoly!.Plane.Normal.Z, precision: 3); // horizontal floor: normal.Z = 1
|
||||||
|
}
|
||||||
|
|
||||||
|
[Fact]
|
||||||
|
public void FindWalkableSphere_OnlyUpperFloor_FootAbove_PicksUpperFloor()
|
||||||
|
{
|
||||||
|
// Two floors at Z=0 and Z=3. Foot sphere center at Z=3.4 (radius 0.48).
|
||||||
|
// The sphere overlaps the upper floor (|center.Z - 3| = 0.4 < radius 0.48).
|
||||||
|
// Lower floor at Z=0 is out of range (dist=3.4 >> radius 0.48).
|
||||||
|
// Expect: pick the upper floor (id 1).
|
||||||
|
var (root, resolved) = BuildTwoFloorsBsp(lowerZ: 0f, upperZ: 3f);
|
||||||
|
var transition = BuildFloorZTransition();
|
||||||
|
|
||||||
|
var sphere = new Sphere { Origin = new Vector3(0.5f, 0.5f, 3.4f), Radius = 0.48f };
|
||||||
|
|
||||||
|
bool found = BSPQuery.FindWalkableSphere(
|
||||||
|
root, resolved, transition,
|
||||||
|
sphere,
|
||||||
|
probeDistance: 0.5f,
|
||||||
|
up: Vector3.UnitZ,
|
||||||
|
out var hitPoly,
|
||||||
|
out var hitPolyId,
|
||||||
|
out _);
|
||||||
|
|
||||||
|
Assert.True(found);
|
||||||
|
Assert.Equal((ushort)1, hitPolyId);
|
||||||
|
Assert.NotNull(hitPoly);
|
||||||
|
}
|
||||||
|
|
||||||
|
[Fact]
|
||||||
|
public void FindWalkableSphere_NoWalkableInProbeRange_ReturnsFalse()
|
||||||
|
{
|
||||||
|
// Two floors at Z=0 and Z=3. Foot at Z=10 with radius 0.48 — out of
|
||||||
|
// sphere-overlap range for both polygons (|10-0|=10 >> 0.48, |10-3|=7 >> 0.48).
|
||||||
|
// find_walkable requires the sphere to overlap the polygon plane; neither
|
||||||
|
// floor is within overlap range, so no hit is found.
|
||||||
|
var (root, resolved) = BuildTwoFloorsBsp(lowerZ: 0f, upperZ: 3f);
|
||||||
|
var transition = BuildFloorZTransition();
|
||||||
|
|
||||||
|
var sphere = new Sphere { Origin = new Vector3(0.5f, 0.5f, 10f), Radius = 0.48f };
|
||||||
|
|
||||||
|
bool found = BSPQuery.FindWalkableSphere(
|
||||||
|
root, resolved, transition,
|
||||||
|
sphere,
|
||||||
|
probeDistance: 0.5f,
|
||||||
|
up: Vector3.UnitZ,
|
||||||
|
out var hitPoly,
|
||||||
|
out var hitPolyId,
|
||||||
|
out var adjustedCenter);
|
||||||
|
|
||||||
|
Assert.False(found);
|
||||||
|
Assert.Null(hitPoly);
|
||||||
|
Assert.Equal((ushort)0, hitPolyId);
|
||||||
|
Assert.Equal(sphere.Origin, adjustedCenter);
|
||||||
|
}
|
||||||
|
|
||||||
|
[Fact]
|
||||||
|
public void FindWalkableSphere_SteepPoly_RejectedByWalkableAllowance()
|
||||||
|
{
|
||||||
|
// One polygon with a steep normal (Z component ≈ 0.5 < FloorZ ≈ 0.6642).
|
||||||
|
// WalkableHitsSphere checks dp = dot(up, normal) > WalkableAllowance;
|
||||||
|
// dp = 0.5 <= FloorZ → not walkable. Sphere overlaps the plane so
|
||||||
|
// PolygonHitsSpherePrecise would pass, but the walkability gate fires first.
|
||||||
|
var center = new Vector3(0.5f, 0.5f, 0f);
|
||||||
|
var root = new PhysicsBSPNode
|
||||||
|
{
|
||||||
|
Type = BSPNodeType.Leaf,
|
||||||
|
BoundingSphere = new Sphere { Origin = center, Radius = 2f },
|
||||||
|
};
|
||||||
|
root.Polygons.Add(0);
|
||||||
|
|
||||||
|
// Plane tilted: normal has Z = 0.5 (60° slope). Build from two orthogonal verts.
|
||||||
|
var steepNormal = Vector3.Normalize(new Vector3(0f, MathF.Sqrt(0.75f), 0.5f));
|
||||||
|
// Vertices lying on the plane through the origin.
|
||||||
|
float rise = MathF.Sqrt(0.75f) / 0.5f; // how much Y-displacement equals 1 unit Z-rise
|
||||||
|
Vector3[] verts =
|
||||||
|
{
|
||||||
|
new Vector3(0f, 0f, 0f),
|
||||||
|
new Vector3(1f, 0f, 0f),
|
||||||
|
new Vector3(1f, 1f, rise),
|
||||||
|
new Vector3(0f, 1f, rise),
|
||||||
|
};
|
||||||
|
var resolved = new Dictionary<ushort, ResolvedPolygon>
|
||||||
|
{
|
||||||
|
[0] = new ResolvedPolygon
|
||||||
|
{
|
||||||
|
Vertices = verts,
|
||||||
|
Plane = new Plane(steepNormal, -Vector3.Dot(steepNormal, verts[0])),
|
||||||
|
NumPoints = 4,
|
||||||
|
SidesType = CullMode.None,
|
||||||
|
},
|
||||||
|
};
|
||||||
|
|
||||||
|
var transition = BuildFloorZTransition();
|
||||||
|
// Sphere overlapping the tilted plane at the origin side.
|
||||||
|
var sphere = new Sphere { Origin = new Vector3(0.5f, 0.5f, 0.3f), Radius = 0.48f };
|
||||||
|
|
||||||
|
bool found = BSPQuery.FindWalkableSphere(
|
||||||
|
root, resolved, transition,
|
||||||
|
sphere,
|
||||||
|
probeDistance: 0.5f,
|
||||||
|
up: Vector3.UnitZ,
|
||||||
|
out _,
|
||||||
|
out _,
|
||||||
|
out _);
|
||||||
|
|
||||||
|
Assert.False(found);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue