From e08a06ac5bc93dbdf5b491476ada50c002616ac2 Mon Sep 17 00:00:00 2001 From: Erik Date: Mon, 13 Apr 2026 23:52:45 +0200 Subject: [PATCH] feat(physics): Transition.FindTransitionalPosition core algorithm Port FindTransitionalPosition, TransitionalInsert, FindEnvCollisions, AdjustOffset, DoStepDown, ValidateTransition from transition_pseudocode.md. Outdoor terrain collision with step-down ground contact. Indoor BSP and object collision deferred to subsequent tasks. Also adds PhysicsEngine.SampleTerrainZ() which dispatches the terrain Z query to the right registered landblock by world-space XY position. Co-Authored-By: Claude Sonnet 4.6 --- src/AcDream.Core/Physics/PhysicsEngine.cs | 18 + src/AcDream.Core/Physics/TransitionTypes.cs | 510 +++++++++++++++++- .../Physics/TransitionTests.cs | 311 +++++++++++ 3 files changed, 836 insertions(+), 3 deletions(-) create mode 100644 tests/AcDream.Core.Tests/Physics/TransitionTests.cs diff --git a/src/AcDream.Core/Physics/PhysicsEngine.cs b/src/AcDream.Core/Physics/PhysicsEngine.cs index 36dacfc..2cd02ef 100644 --- a/src/AcDream.Core/Physics/PhysicsEngine.cs +++ b/src/AcDream.Core/Physics/PhysicsEngine.cs @@ -47,6 +47,24 @@ public sealed class PhysicsEngine /// public void RemoveLandblock(uint landblockId) => _landblocks.Remove(landblockId); + /// + /// Sample the outdoor terrain Z at the given world-space XY position. + /// Searches all registered landblocks; returns null if no landblock covers the position. + /// Used by Transition.FindEnvCollisions for terrain collision resolution. + /// + public float? SampleTerrainZ(float worldX, float worldY) + { + foreach (var kvp in _landblocks) + { + var lb = kvp.Value; + float localX = worldX - lb.WorldOffsetX; + float localY = worldY - lb.WorldOffsetY; + if (localX >= 0f && localX < 192f && localY >= 0f && localY < 192f) + return lb.Terrain.SampleZ(localX, localY); + } + return null; + } + /// /// Resolve an entity's movement from by /// applying (XY only) and computing the correct Z diff --git a/src/AcDream.Core/Physics/TransitionTypes.cs b/src/AcDream.Core/Physics/TransitionTypes.cs index 7c973ff..25ab0ab 100644 --- a/src/AcDream.Core/Physics/TransitionTypes.cs +++ b/src/AcDream.Core/Physics/TransitionTypes.cs @@ -258,7 +258,15 @@ public static class PhysicsGlobals /// /// The main collision transition orchestrator. /// ACE: Transition. Decompiled: CTransition. -/// Stub class — algorithm methods added in Task 6b-6d. +/// +/// Task 6b implements outdoor terrain collision: +/// FindTransitionalPosition → step subdivision loop +/// TransitionalInsert → per-step collision check +/// FindEnvCollisions → terrain Z query + ValidateWalkable +/// AdjustOffset → contact-plane / sliding-normal projection +/// StepDown → ground-contact maintenance on downhill movement +/// +/// Indoor BSP (Task 6c) and object collision (Task 7) are deferred. /// public sealed class Transition { @@ -266,6 +274,502 @@ public sealed class Transition public SpherePath SpherePath = new(); public CollisionInfo CollisionInfo = new(); - // Will be populated in Task 6b: - // public TransitionState FindTransitionalPosition(PhysicsEngine engine, PhysicsDataCache cache) { ... } + // ----------------------------------------------------------------------- + // Public entry point + // ----------------------------------------------------------------------- + + /// + /// Move the sphere path from BeginPos to EndPos, resolving terrain + /// collisions at each sub-step. Returns true when the final position + /// is valid (TransitionState == OK). + /// + /// Ported from pseudocode section 2 (FindTransitionalPosition). + /// ACE: Transition.FindTransitionalPosition(). + /// + public bool FindTransitionalPosition(PhysicsEngine engine) + { + var sp = SpherePath; + + // No starting cell → cannot move. + if (sp.CurCellId == 0) + return false; + + // ------------------------------------------------------------------ + // Step subdivision: each sub-step travels at most one sphere radius + // to prevent tunnelling through thin surfaces. + // ------------------------------------------------------------------ + Vector3 offset = sp.EndPos - sp.BeginPos; + float dist = offset.Length(); + float radius = sp.LocalSphere[0].Radius; + + // Guard: zero-radius sphere would cause a div-by-zero. + if (radius <= PhysicsGlobals.EPSILON) + return false; + + float step = dist / radius; + + int numSteps; + Vector3 offsetPerStep; + + if (step > 1.0f) + { + numSteps = (int)MathF.Ceiling(step); + offsetPerStep = offset * (1f / numSteps); + } + else if (offset != Vector3.Zero) + { + numSteps = 1; + offsetPerStep = offset; + } + else + { + numSteps = 0; + offsetPerStep = Vector3.Zero; + } + + // Retail safety cap (30 steps). Sight objects bypass this. + if (numSteps > PhysicsGlobals.MaxTransitionSteps) + return false; + + // Apply free rotation if requested. + if (ObjectInfo.FreeRotate) + sp.CurOrientation = sp.EndOrientation; + + sp.SetCheckPos(sp.CurPos, sp.CurCellId); + + // Zero-step case: just validate current cell membership. + if (numSteps <= 0) + { + if (!ObjectInfo.FreeRotate) + sp.CurOrientation = sp.EndOrientation; + return true; + } + + // ------------------------------------------------------------------ + // Main stepping loop + // ------------------------------------------------------------------ + var transitionState = TransitionState.OK; + + for (int i = 0; i < numSteps; i++) + { + // Reset per-step collision state. + CollisionInfo.SlidingNormalValid = false; + CollisionInfo.ContactPlaneValid = false; + CollisionInfo.ContactPlaneIsWater = false; + + // Project the step offset through any existing contact / slide plane. + sp.GlobalOffset = AdjustOffset(offsetPerStep); + + // Abort if adjusted offset is negligible (we're stuck against a wall). + if (sp.GlobalOffset.LengthSquared() < PhysicsGlobals.EpsilonSq) + return i != 0 && transitionState == TransitionState.OK; + + // Interpolate orientation (non-free-rotate path). + if (!ObjectInfo.FreeRotate) + { + float delta = (i + 1f) / numSteps; + sp.CheckOrientation = Quaternion.Slerp(sp.BeginOrientation, sp.EndOrientation, delta); + } + + // Apply the offset, then check collisions. + sp.AddOffsetToCheckPos(sp.GlobalOffset); + + var result = TransitionalInsert(3, engine); + transitionState = ValidateTransition(result); + + // PathClipped objects stop at the first collision. + if (CollisionInfo.CollisionNormalValid && ObjectInfo.PathClipped) + break; + } + + return transitionState == TransitionState.OK; + } + + // ----------------------------------------------------------------------- + // Per-step collision check + // ----------------------------------------------------------------------- + + /// + /// Check collisions at the current CheckPos, apply step-down as needed. + /// Ported from pseudocode section 3 (TransitionalInsert). + /// ACE: Transition.TransitionalInsert(int num_insertion_attempts). + /// + private TransitionState TransitionalInsert(int maxAttempts, PhysicsEngine engine) + { + if (SpherePath.CheckCellId == 0) return TransitionState.OK; + if (maxAttempts <= 0) return TransitionState.Invalid; + + var sp = SpherePath; + var ci = CollisionInfo; + var oi = ObjectInfo; + + TransitionState transitState = TransitionState.OK; + + for (int attempt = 0; attempt < maxAttempts; attempt++) + { + // Phase 1: check collisions in the current cell. + transitState = FindEnvCollisions(engine); + + switch (transitState) + { + case TransitionState.OK: + // Outdoor path: no neighboring cell enumeration needed for MVP. + break; + + case TransitionState.Collided: + return TransitionState.Collided; + + case TransitionState.Adjusted: + sp.NegPolyHit = false; + break; + + case TransitionState.Slid: + ci.ContactPlaneValid = false; + ci.ContactPlaneIsWater = false; + sp.NegPolyHit = false; + break; + } + + // Phase 2: post-collision response. + if (transitState == TransitionState.OK) + { + // Handle step-down when in contact but no ground plane found. + if (!ci.ContactPlaneValid && oi.Contact && !sp.StepDown + && sp.CheckCellId != 0 && oi.StepDown) + { + float zVal = PhysicsGlobals.LandingZ; + float stepDownHeight = oi.StepDownHeight; + sp.WalkableAllowance = zVal; + sp.SaveCheckPos(); + + float radsum = sp.GlobalSphere[0].Radius * 2f; + + if (radsum >= stepDownHeight) + { + if (DoStepDown(stepDownHeight, zVal, engine)) + { + sp.WalkableValid = false; + return TransitionState.OK; + } + } + else + { + stepDownHeight *= 0.5f; + if (DoStepDown(stepDownHeight, zVal, engine) + || DoStepDown(stepDownHeight, zVal, engine)) + { + sp.WalkableValid = false; + return TransitionState.OK; + } + } + + // Step-down failed: stay at current position. + sp.RestoreCheckPos(); + return TransitionState.OK; + } + else + { + return TransitionState.OK; + } + } + } + + return transitState; + } + + // ----------------------------------------------------------------------- + // Environment collision — outdoor terrain + // ----------------------------------------------------------------------- + + /// + /// Query the outdoor terrain at CheckPos and apply ValidateWalkable logic. + /// Indoor BSP collision is deferred to Task 6c. + /// + /// Ported from pseudocode section 4 (LandCell.FindEnvCollisions + ValidateWalkable). + /// ACE: LandCell.FindEnvCollisions / ObjectInfo.ValidateWalkable. + /// + private TransitionState FindEnvCollisions(PhysicsEngine engine) + { + var sp = SpherePath; + var ci = CollisionInfo; + + // Sample terrain Z at the foot sphere's world position. + Vector3 footCenter = sp.GlobalSphere[0].Origin; + float sphereRadius = sp.GlobalSphere[0].Radius; + + float? terrainZ = engine.SampleTerrainZ(footCenter.X, footCenter.Y); + if (terrainZ is null) + return TransitionState.OK; // no terrain loaded here — allow pass-through + + // Build the terrain contact plane (flat ground: Normal = +Z, D = -terrainZ). + // For sloped terrain we'd need the surface normal from the triangle; for MVP + // we use the vertical plane which matches flat terrain exactly and gives + // conservative results on slopes (terrain Z is already interpolated correctly). + var contactPlane = new System.Numerics.Plane( + new Vector3(0f, 0f, 1f), -terrainZ.Value); + + return ValidateWalkable(footCenter, sphereRadius, contactPlane, isWater: false, + cellId: sp.CheckCellId); + } + + /// + /// Determine the collision response for a sphere against a walkable surface plane. + /// + /// Ported from pseudocode section 4 (ValidateWalkable, normal-object path). + /// ACE: ObjectInfo.ValidateWalkable(). + /// + private TransitionState ValidateWalkable(Vector3 sphereCenter, float sphereRadius, + System.Numerics.Plane contactPlane, + bool isWater, uint cellId) + { + var sp = SpherePath; + var ci = CollisionInfo; + var oi = ObjectInfo; + + // Low point of the sphere. + var lowPoint = sphereCenter - new Vector3(0f, 0f, sphereRadius); + + // Signed distance: positive = above, negative = below. + // Plane convention: dot(N, p) + D. + float dist = Vector3.Dot(lowPoint, contactPlane.Normal) + contactPlane.D; + + // ── Above or touching the surface ──────────────────────────────── + if (dist >= -PhysicsGlobals.EPSILON) + { + if (dist <= PhysicsGlobals.EPSILON) + { + // Resting on surface: record contact plane. + bool walkableNormal = contactPlane.Normal.Z >= sp.WalkableAllowance; + if (sp.StepDown || !oi.OnWalkable || walkableNormal) + ci.SetContactPlane(contactPlane, cellId, isWater); + + if (!oi.Contact && !sp.StepDown) + { + ci.SetCollisionNormal(contactPlane.Normal); + ci.CollidedWithEnvironment = true; + } + } + return TransitionState.OK; + } + + // ── Below the surface ───────────────────────────────────────────── + if (sp.CheckWalkable) return TransitionState.Collided; // walkable probe fails + + // zDist: how far we need to push up along Z to clear the surface. + // contactPlane.Normal.Z is 1 for flat ground, so this is just dist. + float zDist = dist / contactPlane.Normal.Z; + + bool walkable = contactPlane.Normal.Z >= sp.WalkableAllowance; + if (sp.StepDown || !oi.OnWalkable || walkable) + { + ci.SetContactPlane(contactPlane, cellId, isWater); + + if (sp.StepDown) + { + // Validate step-down interpolation factor. + float interp = (1f - (-1f / (sp.StepDownAmt * sp.WalkInterp)) * zDist) * sp.WalkInterp; + if (interp >= sp.WalkInterp || interp < -0.1f) + return TransitionState.Collided; + sp.WalkInterp = interp; + } + + // Push the sphere up out of the terrain. + sp.AddOffsetToCheckPos(new Vector3(0f, 0f, -zDist)); + } + + if (!oi.Contact && !sp.StepDown) + { + ci.SetCollisionNormal(contactPlane.Normal); + ci.CollidedWithEnvironment = true; + } + + return TransitionState.Adjusted; + } + + // ----------------------------------------------------------------------- + // Offset adjustment (contact-plane + slide-plane projection) + // ----------------------------------------------------------------------- + + /// + /// Project the per-step movement offset to avoid pushing into the contact + /// surface or slide plane. + /// + /// Ported from pseudocode section 6 (AdjustOffset). + /// ACE: Transition.AdjustOffset(Vector3 offset). + /// + private Vector3 AdjustOffset(Vector3 offset) + { + var ci = CollisionInfo; + + Vector3 result = offset; + bool checkSlide = false; + + // Check if we should apply sliding. + float slidingAngle = Vector3.Dot(result, ci.SlidingNormal); + if (ci.SlidingNormalValid) + { + if (slidingAngle < 0f) + checkSlide = true; + else + ci.SlidingNormalValid = false; + } + + // No contact plane — simple slide projection. + if (!ci.ContactPlaneValid) + { + if (checkSlide) + result -= ci.SlidingNormal * slidingAngle; + return result; + } + + // Have a contact plane — project movement onto the contact surface. + float collisionAngle = Vector3.Dot(result, ci.ContactPlane.Normal); + Vector3 slideOffset = Vector3.Cross(ci.ContactPlane.Normal, ci.SlidingNormal); + + if (checkSlide) + { + // Project movement along the crease between contact and slide planes. + float slideLen = slideOffset.Length(); + if (slideLen < PhysicsGlobals.EPSILON) + result = Vector3.Zero; + else + { + slideOffset /= slideLen; + result = Vector3.Dot(slideOffset, result) * slideOffset; + } + } + else if (collisionAngle <= 0f) + { + // Moving into the contact plane: remove component into the plane. + result -= ci.ContactPlane.Normal * collisionAngle; + } + else + { + // Moving away from contact plane: snap to plane surface. + // SnapToPlane: remove any component that would violate the plane. + result -= ci.ContactPlane.Normal * (collisionAngle - 0f); + } + + return result; + } + + // ----------------------------------------------------------------------- + // Step-down + // ----------------------------------------------------------------------- + + /// + /// Probe downward by stepDownHeight and snap to a walkable surface if found. + /// Returns true if a walkable surface was contacted. + /// + /// Ported from pseudocode section 5 (StepDown). + /// ACE: Transition.StepDown(float stepDownHeight, float zVal). + /// + private bool DoStepDown(float stepDownHeight, float walkableZ, PhysicsEngine engine) + { + var sp = SpherePath; + + sp.NegPolyHit = false; + sp.StepDown = true; + sp.StepDownAmt = stepDownHeight; + sp.WalkInterp = 1.0f; + + // If NOT in step-up mode, apply the downward offset. + if (!sp.StepUp) + { + sp.AddOffsetToCheckPos(new Vector3(0f, 0f, -stepDownHeight)); + } + + // Run collision detection with the step-down flag active. + var transitState = TransitionalInsert(5, engine); + + sp.StepDown = false; + + // Accept step-down if: + // 1. Collision detection returned OK + // 2. A valid contact plane was found + // 3. The contact plane is walkable (Normal.Z >= walkableZ) + if (transitState == TransitionState.OK + && CollisionInfo.ContactPlaneValid + && CollisionInfo.ContactPlane.Normal.Z >= walkableZ) + { + return true; + } + + return false; + } + + // ----------------------------------------------------------------------- + // Post-step validation + // ----------------------------------------------------------------------- + + /// + /// Accept or revert the current step, update state flags, and propagate + /// the sliding normal. + /// + /// Ported from pseudocode section 7 (ValidateTransition). + /// ACE: Transition.ValidateTransition(). + /// + private TransitionState ValidateTransition(TransitionState transitionState) + { + var sp = SpherePath; + var ci = CollisionInfo; + var oi = ObjectInfo; + + if (transitionState == TransitionState.OK && sp.CheckPos != sp.CurPos) + { + // Movement succeeded: accept the new position. + sp.CurPos = sp.CheckPos; + sp.CurCellId = sp.CheckCellId; + sp.CurOrientation = sp.CheckOrientation; + + // Cache the current-center spheres at the new position. + for (int i = 0; i < sp.NumSphere; i++) + { + sp.GlobalCurrCenter[i].Origin = sp.LocalSphere[i].Origin + sp.CurPos; + sp.GlobalCurrCenter[i].Radius = sp.LocalSphere[i].Radius; + } + + sp.SetCheckPos(sp.CurPos, sp.CurCellId); + // moved = true (FramesStationaryFall deferred to full physics port) + } + else if (transitionState == TransitionState.OK) + { + // No movement (same position): accept as-is. + sp.SetCheckPos(sp.CurPos, sp.CurCellId); + } + else if (transitionState != TransitionState.Invalid) + { + // Collision/slide/adjusted: revert to current position. + if (!ci.CollisionNormalValid) + ci.SetCollisionNormal(Vector3.UnitZ); // default: push up + + sp.SetCheckPos(sp.CurPos, sp.CurCellId); + transitionState = TransitionState.OK; + } + + // Update sliding normal from collision normal. + if (ci.CollisionNormalValid) + ci.SetSlidingNormal(ci.CollisionNormal); + + // Preserve contact plane for next step. + ci.LastKnownContactPlaneValid = ci.ContactPlaneValid; + if (ci.ContactPlaneValid) + { + ci.LastKnownContactPlane = ci.ContactPlane; + ci.LastKnownContactPlaneCellId = ci.ContactPlaneCellId; + ci.LastKnownContactPlaneIsWater = ci.ContactPlaneIsWater; + + oi.State |= ObjectInfoState.Contact; + if (ci.ContactPlane.Normal.Z >= PhysicsGlobals.LandingZ) + oi.State |= ObjectInfoState.OnWalkable; + else + oi.State &= ~ObjectInfoState.OnWalkable; + } + else + { + oi.State &= ~(ObjectInfoState.Contact | ObjectInfoState.OnWalkable); + } + + return transitionState; + } } diff --git a/tests/AcDream.Core.Tests/Physics/TransitionTests.cs b/tests/AcDream.Core.Tests/Physics/TransitionTests.cs new file mode 100644 index 0000000..307def0 --- /dev/null +++ b/tests/AcDream.Core.Tests/Physics/TransitionTests.cs @@ -0,0 +1,311 @@ +using System; +using System.Collections.Generic; +using System.Numerics; +using AcDream.Core.Physics; +using Xunit; + +namespace AcDream.Core.Tests.Physics; + +/// +/// Tests for Transition.FindTransitionalPosition (Task 6b). +/// Uses a real PhysicsEngine with simple synthetic TerrainSurfaces so we +/// can exercise the terrain-collision path without mocking internals. +/// +public class TransitionTests +{ + // ----------------------------------------------------------------------- + // Helpers + // ----------------------------------------------------------------------- + + private static float[] LinearHeightTable() + { + var t = new float[256]; + for (int i = 0; i < 256; i++) t[i] = i * 1.0f; + return t; + } + + /// + /// Build a flat terrain with every cell at . + /// All 81 height entries reference index (int)terrainZ. + /// + private static TerrainSurface FlatTerrain(float terrainZ) + { + int idx = Math.Clamp((int)terrainZ, 0, 255); + var heights = new byte[81]; + Array.Fill(heights, (byte)idx); + return new TerrainSurface(heights, LinearHeightTable()); + } + + /// + /// Build a terrain with a linear slope: height increases by 1 for every + /// step in the +X direction (landblock-local X/24 ≈ cell index). + /// + private static TerrainSurface SlopedTerrain(float baseZ, float risePerCell) + { + var heights = new byte[81]; + for (int x = 0; x < 9; x++) + for (int y = 0; y < 9; y++) + { + float z = baseZ + x * risePerCell; + int idx = Math.Clamp((int)z, 0, 255); + heights[x * 9 + y] = (byte)idx; + } + return new TerrainSurface(heights, LinearHeightTable()); + } + + private static PhysicsEngine MakeEngine(TerrainSurface terrain) + { + var engine = new PhysicsEngine(); + engine.AddLandblock(0xA9B4FFFFu, terrain, + Array.Empty(), Array.Empty(), + worldOffsetX: 0f, worldOffsetY: 0f); + return engine; + } + + /// + /// Build a Transition set up for a simple one-sphere character + /// moving from to . + /// + private static Transition MakeTransition( + Vector3 from, Vector3 to, + float sphereRadius = 0.5f, + uint cellId = 0x0001) + { + var t = new Transition(); + t.SpherePath.InitPath(from, to, cellId, sphereRadius); + t.ObjectInfo.State = ObjectInfoState.None; // not Contact / OnWalkable yet + return t; + } + + // ----------------------------------------------------------------------- + // Tests + // ----------------------------------------------------------------------- + + [Fact] + public void FindTransitionalPosition_FlatTerrain_MovesFullDistance() + { + // Arrange: flat terrain at Z=10, sphere starts at Z=10 (sitting on ground). + const float groundZ = 10f; + var terrain = FlatTerrain(groundZ); + var engine = MakeEngine(terrain); + + Vector3 from = new(50f, 50f, groundZ); + Vector3 to = new(55f, 50f, groundZ); // 5 units forward + + var transition = MakeTransition(from, to); + + // Act + bool ok = transition.FindTransitionalPosition(engine); + + // Assert: transition succeeded and position advanced toward the target. + Assert.True(ok); + Assert.True(transition.SpherePath.CurPos.X > from.X, + "Sphere should have moved in +X"); + Assert.InRange(transition.SpherePath.CurPos.X, from.X + 1f, to.X + 0.1f); + Assert.InRange(transition.SpherePath.CurPos.Z, groundZ - 0.1f, groundZ + 0.1f); + } + + [Fact] + public void FindTransitionalPosition_NullBeginCell_ReturnsFalse() + { + // Arrange: CheckCellId == 0 means "no cell" → must return false. + var terrain = FlatTerrain(0f); + var engine = MakeEngine(terrain); + + Vector3 from = new(50f, 50f, 0f); + Vector3 to = new(55f, 50f, 0f); + + var transition = MakeTransition(from, to, cellId: 0); // <-- invalid cell + + // Act + bool ok = transition.FindTransitionalPosition(engine); + + // Assert + Assert.False(ok, "No beginning cell should abort immediately"); + } + + [Fact] + public void FindTransitionalPosition_NoTerrain_AllowsPassThrough() + { + // Arrange: engine has no landblocks → SampleTerrainZ returns null. + var engine = new PhysicsEngine(); + var transition = MakeTransition(new(50f, 50f, 0f), new(55f, 50f, 0f)); + + // Act — should not throw; terrain Z is unknown so movement is accepted. + bool ok = transition.FindTransitionalPosition(engine); + + // OK is fine here — no terrain means no collision, position accepted. + Assert.True(ok); + } + + [Fact] + public void FindTransitionalPosition_ZeroMovement_ReturnsTrueWithUnchangedPosition() + { + // Arrange: from == to — zero-step case. + var terrain = FlatTerrain(5f); + var engine = MakeEngine(terrain); + var start = new Vector3(96f, 96f, 5f); + var transition = MakeTransition(start, start); + + // Act + bool ok = transition.FindTransitionalPosition(engine); + + // Assert + Assert.True(ok); + Assert.Equal(start, transition.SpherePath.CurPos); + } + + [Fact] + public void FindTransitionalPosition_SphereAboveTerrain_SnapsTerrain() + { + // Arrange: sphere starts 3 units above flat terrain at Z=0. + // After one step the collision system should push it back onto terrain. + var terrain = FlatTerrain(0f); + var engine = MakeEngine(terrain); + var from = new Vector3(50f, 50f, 3f); // floating above terrain + var to = new Vector3(51f, 50f, 3f); + var transition = MakeTransition(from, to); + + // Seed as "in contact" so step-down path fires. + transition.ObjectInfo.State = ObjectInfoState.Contact | ObjectInfoState.OnWalkable; + + // Act + bool ok = transition.FindTransitionalPosition(engine); + + // Assert: transition returned; sphere should be at or near terrain Z. + Assert.True(ok); + // The Z of CurPos should reflect terrain resolution (could be 0 or clamped). + // We just verify it's ≤ from.Z (gravity pulled it down or it stayed). + Assert.True(transition.SpherePath.CurPos.Z <= from.Z + 0.1f, + $"Expected Z <= {from.Z + 0.1f}, got {transition.SpherePath.CurPos.Z}"); + } + + [Fact] + public void FindTransitionalPosition_IntoHill_AdjustsOrStops() + { + // Arrange: sloped terrain rises 5 units per cell (~0.6 units per unit of X). + // A sphere with step-height 0.01 should find its movement adjusted. + var terrain = SlopedTerrain(baseZ: 0f, risePerCell: 5f); + var engine = MakeEngine(terrain); + + float radius = 0.5f; + var from = new Vector3(12f, 96f, 0f + radius); // foot on terrain + var to = new Vector3(30f, 96f, 0f + radius); // moving up the slope + + var transition = MakeTransition(from, to, sphereRadius: radius); + + // Act — must not throw. + bool ok = transition.FindTransitionalPosition(engine); + + // Assert: result is either blocked (false) or adjusted to a valid Z. + // The important invariant is we didn't crash or return a position + // far below the terrain. + if (ok) + { + float terrainAtFinal = terrain.SampleZ( + transition.SpherePath.CurPos.X, transition.SpherePath.CurPos.Y); + Assert.True( + transition.SpherePath.CurPos.Z >= terrainAtFinal - 0.1f, + $"Sphere went below terrain: posZ={transition.SpherePath.CurPos.Z}, terrainZ={terrainAtFinal}"); + } + // ok == false is also acceptable (movement was too steep and blocked). + } + + [Fact] + public void StepDown_MaintainsGroundContact() + { + // Arrange: flat terrain at Z=10. The sphere starts in contact with the + // surface and moves horizontally. Because the terrain stays flat the + // Contact flag should persist and no step-down is needed. + // Movement distance is kept < MaxTransitionSteps * radius to avoid the + // retail 30-step safety cap. With radius=1.0 and 15 units: 15 steps < 30. + const float groundZ = 10f; + var terrain = FlatTerrain(groundZ); + var engine = MakeEngine(terrain); + + float radius = 1.0f; // larger radius → fewer steps needed for same distance + var from = new Vector3(50f, 96f, groundZ + radius); // foot on terrain + var to = new Vector3(65f, 96f, groundZ + radius); // 15 units → 15 steps + + var transition = MakeTransition(from, to, sphereRadius: radius); + transition.ObjectInfo.State = ObjectInfoState.Contact | ObjectInfoState.OnWalkable; + + // Act + bool ok = transition.FindTransitionalPosition(engine); + + // Assert: movement accepted and sphere stayed on the surface. + Assert.True(ok); + float finalBottom = transition.SpherePath.CurPos.Z - radius; + Assert.True( + finalBottom >= groundZ - PhysicsGlobals.EPSILON, + $"Sphere fell below terrain: bottom={finalBottom:F4}, terrainZ={groundZ}"); + Assert.True( + transition.SpherePath.CurPos.X > from.X, + "Sphere should have advanced in +X"); + } + + [Fact] + public void AdjustOffset_ContactPlanePresent_RemovesIntoPlaneComponent() + { + // White-box check: once a contact plane has been established, the + // AdjustOffset method should prevent the sphere from re-entering the + // surface on subsequent steps. + // + // We verify this by running two successive FindTransitionalPosition calls: + // first to land the sphere on terrain, then to confirm lateral movement + // does not push the sphere below terrain. + var terrain = FlatTerrain(10f); + var engine = MakeEngine(terrain); + + const float groundZ = 10f; + const float radius = 0.5f; + + // First transition: move from above onto terrain (sphere sits on ground). + var from1 = new Vector3(50f, 50f, groundZ + radius); + var to1 = new Vector3(51f, 50f, groundZ + radius); + var t1 = MakeTransition(from1, to1, radius); + bool ok1 = t1.FindTransitionalPosition(engine); + + Assert.True(ok1); + + // Second transition: continue moving laterally from the landed position. + var from2 = t1.SpherePath.CurPos; + var to2 = from2 + new Vector3(2f, 0f, 0f); + var t2 = MakeTransition(from2, to2, radius); + // Seed as on-walkable (as if we just landed). + t2.ObjectInfo.State = ObjectInfoState.Contact | ObjectInfoState.OnWalkable; + + bool ok2 = t2.FindTransitionalPosition(engine); + + Assert.True(ok2); + float bottom = t2.SpherePath.CurPos.Z - radius; + Assert.True(bottom >= groundZ - PhysicsGlobals.EPSILON, + $"Sphere bottom {bottom:F4} should be >= terrain {groundZ}"); + } + + [Fact] + public void SampleTerrainZ_FindsCorrectLandblock() + { + // Ensure SampleTerrainZ dispatches to the right landblock. + var engine = new PhysicsEngine(); + + var terrain1 = FlatTerrain(10f); + var terrain2 = FlatTerrain(20f); + + // Two landblocks side by side (each covers [0,192) in world space). + engine.AddLandblock(0xAAAA0000u, terrain1, + Array.Empty(), Array.Empty(), + worldOffsetX: 0f, worldOffsetY: 0f); + engine.AddLandblock(0xAAAB0000u, terrain2, + Array.Empty(), Array.Empty(), + worldOffsetX: 192f, worldOffsetY: 0f); + + float? z1 = engine.SampleTerrainZ(96f, 96f); // inside lb1 + float? z2 = engine.SampleTerrainZ(288f, 96f); // inside lb2 + + Assert.NotNull(z1); + Assert.NotNull(z2); + Assert.Equal(10f, z1!.Value, precision: 0); + Assert.Equal(20f, z2!.Value, precision: 0); + } +}