diff --git a/src/AcDream.Core/Physics/BSPQuery.cs b/src/AcDream.Core/Physics/BSPQuery.cs index c58594e..48bf975 100644 --- a/src/AcDream.Core/Physics/BSPQuery.cs +++ b/src/AcDream.Core/Physics/BSPQuery.cs @@ -1130,6 +1130,89 @@ public static class BSPQuery 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). + // ------------------------------------------------------------------------- + + /// + /// "Stand here, find my contact plane" entry point over the BSPNode/BSPLeaf + /// find_walkable BSP traversal. Probes downward by + /// along and returns the closest walkable polygon the + /// sphere would rest on, with the sphere's center adjusted to lie on that plane. + /// + /// + /// Wraps the existing private — 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). + /// + /// + /// + /// Intended call site: indoor walkable-plane synthesis in + /// Transition.TryFindIndoorWalkablePlane 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 + /// () and does not use this. + /// + /// + /// + /// The caller is responsible for setting transition.SpherePath.WalkableAllowance + /// to the desired walkability threshold (typically ) + /// before calling, and restoring it after. + /// + /// + /// Root of the cell's PhysicsBSP. + /// Pre-resolved polygon dictionary from PhysicsDataCache. + /// Current transition (read for WalkableAllowance / walk_interp). + /// Foot sphere in cell-local space. + /// Downward probe distance in meters. Typical: 0.5f. + /// Up vector in cell-local space (typically Vector3.UnitZ). + /// Output: the walkable polygon found, or null on miss. + /// Output: polygon id (dictionary key) of the hit. Zero on miss. + /// + /// Output: sphere center adjusted onto the polygon plane. Equal to input + /// sphere.Origin on miss. + /// + /// True if a walkable polygon was found; false otherwise. + public static bool FindWalkableSphere( + PhysicsBSPNode? root, + Dictionary 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 // ACE: BSPTree.cs step_sphere_up diff --git a/tests/AcDream.Core.Tests/Physics/BSPQueryTests.cs b/tests/AcDream.Core.Tests/Physics/BSPQueryTests.cs index 3192126..1666263 100644 --- a/tests/AcDream.Core.Tests/Physics/BSPQueryTests.cs +++ b/tests/AcDream.Core.Tests/Physics/BSPQueryTests.cs @@ -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) + // ========================================================================= + + /// + /// 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. + /// + private static (PhysicsBSPNode root, Dictionary 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 + { + [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); + } + + /// + /// Build a Transition with WalkableAllowance set to FloorZ — what the + /// indoor walkable-plane synthesis uses. + /// + 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 + { + [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); + } }