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:
Erik
parent
ff548b962c
commit
7f55e14cd7
2 changed files with 290 additions and 0 deletions
|
|
@ -204,4 +204,211 @@ public class BSPQueryTests
|
|||
Assert.Null(cache.GetGfxObj(0x01000001u));
|
||||
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