acdream/src/AcDream.Core/Physics/PhysicsBody.cs

using System;
using System.Numerics;

namespace AcDream.Core.Physics;

// ────────────────────────────────────────────────────────────────────────────
// PhysicsBody — C# port of CPhysicsObj's core simulation from acclient.exe.
//
// Source addresses (chunk_00510000.c, chunk_00500000.c):
//   FUN_005111d0  UpdatePhysicsInternal  — Euler integration
//   FUN_00511420  calc_acceleration      — gravity / grounded acceleration
//   FUN_00511ec0  set_velocity           — store + clamp to MaxVelocity
//   FUN_00511fa0  set_local_velocity     — body→world transform then set_velocity
//   FUN_00511de0  set_on_walkable        — set/clear OnWalkable transient flag
//   FUN_0050f940  calc_friction          — ground-contact friction
//   FUN_00515020  update_object          — per-frame top-level driver
//
// Cross-checked against ACE PhysicsObj.cs and PhysicsGlobals.cs.
// ────────────────────────────────────────────────────────────────────────────

/// <summary>
/// State flags stored at struct offset +0xA8 (PhysicsState).
/// Only the flags relevant to this simulation layer are included.
/// </summary>
[Flags]
public enum PhysicsStateFlags : uint
{
    None           = 0,
    Static         = 0x00000001,  // bit 0  — never moves
    Ethereal       = 0x00000004,  // bit 2  — no collision
    ReportCollisions = 0x00000010,
    Gravity        = 0x00000400,  // bit 10 — apply downward gravity
    Hidden         = 0x00001000,
    /// <summary>
    /// A6.P7 (2026-05-25): retail HAS_PHYSICS_BSP_PS bit
    /// (acclient.h:2833). When set, the entity exposes a per-Setup
    /// BSP collision mesh; retail's
    /// <c>CPhysicsObj::FindObjCollisions</c> at
    /// acclient_2013_pseudo_c.txt:276861 dispatches the entity's
    /// collision queries to the BSP path EXCLUSIVELY for non-PvP,
    /// non-missile movers — the foot cylinder and per-Setup spheres
    /// are NEVER tested in this case. Closed cottage doors have
    /// state 0x10008 (STATIC | REPORT_COLLISIONS | HAS_PHYSICS_BSP).
    /// ACE name: <c>PhysicsState.HasPhysicsBSP</c>.
    /// </summary>
    HasPhysicsBsp  = 0x00010000,  // bit 16 — retail HAS_PHYSICS_BSP_PS
    /// <summary>
    /// L.3a (2026-04-30): retail INELASTIC_PS bit (acclient.h:2834).
    /// When set, wall-collisions zero the velocity instead of reflecting.
    /// Used by spell projectiles and missiles that should embed/explode on
    /// impact rather than bounce. The player NEVER has this flag set —
    /// player wall-hits use the reflection path with elasticity ~0.05.
    /// </summary>
    Inelastic      = 0x00020000,  // bit 17 — retail INELASTIC_PS
    Sledding       = 0x00800000,  // bit 23 — sledding (modified friction)
}

/// <summary>
/// Transient-state flags stored at struct offset +0xAC (TransientState).
/// These are cleared/set each frame and must not be saved to disk.
/// </summary>
[Flags]
public enum TransientStateFlags : uint
{
    None       = 0,
    Contact    = 0x00000001,  // bit 0 — touching any surface
    OnWalkable = 0x00000002,  // bit 1 — standing on a walkable surface
    Sliding    = 0x00000004,  // bit 2 — carry sliding normal into next transition
    Active     = 0x00000080,  // bit 7 — object needs per-frame update
}

/// <summary>
/// Port of CPhysicsObj's core simulation state and Euler integration.
/// Holds the fields at the struct offsets documented in acclient_function_map.md
/// and implements the seven methods listed in the task spec.
/// </summary>
public sealed class PhysicsBody
{
    // ── constants ──────────────────────────────────────────────────────────
    // From PhysicsGlobals.cs / confirmed by DAT_007c78a4 reference in decompiled code.
    public const float MaxVelocity        = 50.0f;
    public const float MaxVelocitySquared = MaxVelocity * MaxVelocity;
    public const float Gravity            = -9.8f;      // DAT_0082223c in FUN_00511420
    public const float SmallVelocity      = 0.25f;
    public const float SmallVelocitySquared = SmallVelocity * SmallVelocity;
    public const float DefaultFriction    = 0.95f;
    public const float MinQuantum         = 1.0f / 30.0f;   // ~0.0333 s
    public const float MaxQuantum         = 0.1f;            // 10 fps lower bound
    public const float HugeQuantum        = 2.0f;            // discard stale dt

    // ── struct fields ──────────────────────────────────────────────────────
    // Offsets from acclient_function_map.md §PhysicsObj Struct Layout.

    /// <summary>World-space position (no offset in struct — frame origin).</summary>
    public Vector3 Position { get; set; }

    /// <summary>Orientation quaternion (struct offsets 0x60–0x80 column matrix).</summary>
    public Quaternion Orientation { get; set; } = Quaternion.Identity;

    /// <summary>World-space velocity (+0xE0/E4/E8).</summary>
    public Vector3 Velocity { get; set; }

    /// <summary>World-space acceleration (+0xEC/F0/F4).</summary>
    public Vector3 Acceleration { get; set; }

    /// <summary>Angular velocity in radians/s (+0xF8/FC/100).</summary>
    public Vector3 Omega { get; set; }

    /// <summary>Ground contact-plane normal (+0x130/134/138).</summary>
    public Vector3 GroundNormal { get; set; } = Vector3.UnitZ;

    /// <summary>Last wall/object sliding normal (retail transient Sliding state).</summary>
    public Vector3 SlidingNormal { get; set; }

    // ── persisted contact-plane state (retail PhysicsObj fields) ───────────
    //
    // Retail's PhysicsObj carries its last contact plane FORWARD across frames.
    // When PhysicsObj.transition(oldPos, newPos) creates a new Transition, it
    // seeds CollisionInfo.ContactPlane from these fields via InitContactPlane
    // (see ACE PhysicsObj.cs:2586-2621 get_object_info). That seed is what lets
    // AdjustOffset project horizontal velocity onto the slope surface on the
    // first step — without it, a freshly-allocated Transition has no plane,
    // so running on a slope proceeds purely horizontally and the sphere
    // floats above the terrain (step-down budget is only ~4 cm per tick).
    //
    // ACE field names: PhysicsObj.ContactPlane / ContactPlaneCellID.

    /// <summary>Whether <see cref="ContactPlane"/> currently holds a valid plane.</summary>
    public bool ContactPlaneValid { get; set; }

    /// <summary>Most recent walkable contact plane (world-space).
    /// Updated at the end of every ResolveWithTransition call that found ground.</summary>
    public System.Numerics.Plane ContactPlane { get; set; }

    /// <summary>Full 32-bit cell id of the cell that owns <see cref="ContactPlane"/>.</summary>
    public uint ContactPlaneCellId { get; set; }

    /// <summary>Whether the contact plane is a water surface (affects step behavior).</summary>
    public bool ContactPlaneIsWater { get; set; }

    /// <summary>Whether the previous walkable polygon is available for edge slide.</summary>
    public bool WalkablePolygonValid { get; set; }

    /// <summary>Most recent walkable polygon plane (world-space).</summary>
    public System.Numerics.Plane WalkablePlane { get; set; }

    /// <summary>Most recent walkable polygon vertices (world-space).</summary>
    public Vector3[]? WalkableVertices { get; set; }

    /// <summary>Up vector used by the most recent walkable polygon probe.</summary>
    public Vector3 WalkableUp { get; set; } = Vector3.UnitZ;

    /// <summary>Elasticity coefficient (+0xB0).</summary>
    public float Elasticity { get; set; } = 0.05f;

    /// <summary>Friction coefficient (0 = frictionless, 1 = instant stop).</summary>
    public float Friction { get; set; } = DefaultFriction;

    /// <summary>Physics state flags (+0xA8).</summary>
    public PhysicsStateFlags State { get; set; }
        = PhysicsStateFlags.Gravity | PhysicsStateFlags.ReportCollisions;

    /// <summary>Transient state flags (+0xAC). Cleared each frame as needed.</summary>
    public TransientStateFlags TransientState { get; set; }

    /// <summary>Last simulation time used to compute dt (+0xD8).</summary>
    public double LastUpdateTime { get; set; }

    // ── convenience helpers ────────────────────────────────────────────────

    public bool HasGravity  => State.HasFlag(PhysicsStateFlags.Gravity);
    public bool OnWalkable  => TransientState.HasFlag(TransientStateFlags.OnWalkable);
    public bool IsActive    => TransientState.HasFlag(TransientStateFlags.Active);
    public bool InContact   => TransientState.HasFlag(TransientStateFlags.Contact);

    // ── FUN_00511420 ───────────────────────────────────────────────────────

    /// <summary>
    /// Set Acceleration (and Omega) based on current contact state and flags.
    ///
    /// Decompiled logic (FUN_00511420):
    ///   If Contact AND OnWalkable AND NOT Sledding → zero everything (grounded, no drift).
    ///   Else if Gravity flag → Accel = (0, 0, -9.8).
    ///   Else → zero acceleration.
    ///
    /// The check order in the decompile is:
    ///   (TransientState & 1) != 0 → Contact
    ///   (TransientState & 2) != 0 → OnWalkable
    ///   (State & 0x800000) == 0   → NOT Sledding
    /// </summary>
    public void calc_acceleration()
    {
        if (TransientState.HasFlag(TransientStateFlags.Contact) &&
            TransientState.HasFlag(TransientStateFlags.OnWalkable) &&
            !State.HasFlag(PhysicsStateFlags.Sledding))
        {
            Acceleration = Vector3.Zero;
            Omega = Vector3.Zero;
            return;
        }

        if (State.HasFlag(PhysicsStateFlags.Gravity))
            Acceleration = new Vector3(0f, 0f, Gravity);
        else
            Acceleration = Vector3.Zero;
    }

    // ── FUN_00511ec0 ───────────────────────────────────────────────────────

    /// <summary>
    /// Store a new world-space velocity and clamp its magnitude to MaxVelocity.
    ///
    /// Decompiled logic (FUN_00511ec0):
    ///   velocity = newVelocity
    ///   if |velocity|² > MaxVelocity²:
    ///       normalize then scale by MaxVelocity  (FUN_00452440 = normalize + scalar)
    ///   Set Active transient flag.
    /// </summary>
    public void set_velocity(Vector3 newVelocity)
    {
        Velocity = newVelocity;

        float mag2 = Velocity.LengthSquared();
        if (mag2 > MaxVelocitySquared)
        {
            // Normalize then scale — matches the decompile's FUN_00452440 call
            // which normalizes the vector then multiplies by _DAT_007c78a4 (MaxVelocity).
            Velocity = Vector3.Normalize(Velocity) * MaxVelocity;
        }

        // Set Active flag (bit 7 of TransientState, offset +0xAC).
        TransientState |= TransientStateFlags.Active;
    }

    // ── FUN_00511fa0 ───────────────────────────────────────────────────────

    /// <summary>
    /// Transform a body-local velocity vector into world space using the
    /// orientation quaternion, then call set_velocity.
    ///
    /// Decompiled logic (FUN_00511fa0):
    ///   The orientation is stored as a 3x3 column matrix at offsets 0x60–0x80
    ///   (9 floats). The transform is a straightforward matrix×vector multiply:
    ///     worldX = col0.x*localX + col1.x*localY + col2.x*localZ
    ///     worldY = col0.y*localX + col1.y*localY + col2.y*localZ
    ///     worldZ = col0.z*localX + col1.z*localY + col2.z*localZ
    ///   We replicate this as a Quaternion rotation, which is equivalent.
    /// </summary>
    public void set_local_velocity(Vector3 localVelocity)
    {
        var worldVelocity = Vector3.Transform(localVelocity, Orientation);
        set_velocity(worldVelocity);
    }

    // ── FUN_00511de0 ───────────────────────────────────────────────────────

    /// <summary>
    /// Set or clear the OnWalkable transient flag (bit 1 of TransientState at
    /// +0xAC), then recompute acceleration.
    ///
    /// Decompiled logic (FUN_00511de0):
    ///   if param_2 == 0: TransientState &amp;= ~0x02  (clear OnWalkable)
    ///   else:            TransientState |= 0x02   (set OnWalkable)
    ///   call calc_acceleration()
    /// </summary>
    public void set_on_walkable(bool isOnWalkable)
    {
        if (isOnWalkable)
            TransientState |= TransientStateFlags.OnWalkable;
        else
            TransientState &= ~TransientStateFlags.OnWalkable;

        calc_acceleration();
    }

    // ── FUN_0050f940 ───────────────────────────────────────────────────────

    /// <summary>
    /// Apply friction deceleration to the velocity when the body is standing
    /// on a walkable surface.
    ///
    /// Decompiled logic (FUN_0050f940):
    ///   if NOT OnWalkable → return
    ///   fVar1 = dot(groundNormal, velocity)
    ///   if fVar1 &lt; 0:
    ///     velocity -= fVar1 * groundNormal   (remove inward normal component)
    ///     scalar = pow(1 - friction, dt)
    ///     velocity *= scalar
    ///
    /// The threshold (0.0 from _DAT_007c78a0) means any velocity with a
    /// downward component relative to the normal gets friction applied.
    /// Positive dot means moving away from the surface — no friction.
    ///
    /// Cross-checked with ACE PhysicsObj.calc_friction which uses 0.25f as
    /// the threshold instead; the decompile uses 0.0.  We match the decompile.
    ///
    /// L.3c attempt (2026-04-30, REVERTED): tried bumping to 0.25f per
    /// retail acclient_2013_pseudo_c.txt:276705. Build green but
    /// PlayerMovementControllerTests showed forward locomotion dropping
    /// from ~3m/s to ~0.16m/s — friction now hammers normal walking.
    /// Retail's friction block is gated by an additional state check at
    /// line 276702 (`(this->state & ...) == 0`) that we didn't decode
    /// fully; locomotion is probably skipped from the friction path
    /// while actively walking. Filed as L.3c-followup; keeping the
    /// matching-the-decompile-as-read 0.0 threshold for now.
    /// </summary>
    public void calc_friction(float dt, float velocityMag2)
    {
        if (!TransientState.HasFlag(TransientStateFlags.OnWalkable))
            return;

        float dot = Vector3.Dot(GroundNormal, Velocity);
        if (dot >= 0f)
            return;

        // Remove the component of velocity that presses into the ground normal.
        Velocity -= dot * GroundNormal;

        float friction = Friction;

        // Sledding modifies friction thresholds (from ACE cross-check).
        if (State.HasFlag(PhysicsStateFlags.Sledding))
        {
            if (velocityMag2 < 1.5625f)        // 1.25² — slow sled
                friction = 1.0f;
            else if (velocityMag2 >= 6.25f && GroundNormal.Z > 0.99999536f) // near-flat
                friction = 0.2f;
        }

        // Exponential decay: vel *= (1 - friction)^dt
        float scalar = MathF.Pow(1.0f - friction, dt);
        Velocity *= scalar;
    }

    // ── FUN_005111d0 ───────────────────────────────────────────────────────

    /// <summary>
    /// Euler integration step for one quantum dt.
    ///
    /// Decompiled logic (FUN_005111d0):
    ///   velocity_mag2 = |velocity|²
    ///   if velocity_mag2 == 0:
    ///     if no MovementManager AND OnWalkable → clear Active flag
    ///   else:
    ///     if velocity_mag2 > MaxVelocitySquared: normalize * MaxVelocity
    ///     calc_friction(dt, velocity_mag2)
    ///     if velocity_mag2 &lt; SmallVelocitySquared: zero velocity
    ///     position += velocity * dt + 0.5 * acceleration * dt²
    ///   velocity += acceleration * dt
    ///   Apply angular delta: orientation rotated by omega * dt
    /// </summary>
    public void UpdatePhysicsInternal(float dt)
    {
        float velocityMag2 = Velocity.LengthSquared();

        if (velocityMag2 <= 0f)
        {
            // No movement manager equivalent here; just clear Active if grounded.
            if (TransientState.HasFlag(TransientStateFlags.OnWalkable))
                TransientState &= ~TransientStateFlags.Active;
        }
        else
        {
            // Clamp velocity magnitude to MaxVelocity.
            if (velocityMag2 > MaxVelocitySquared)
            {
                Velocity = Vector3.Normalize(Velocity) * MaxVelocity;
                velocityMag2 = MaxVelocitySquared;
            }

            calc_friction(dt, velocityMag2);

            // If velocity fell below the "small" threshold after friction, stop.
            // Only apply when grounded — while airborne, gravity must accumulate
            // even when velocity is near zero (e.g., at jump apex).
            if (velocityMag2 - SmallVelocitySquared < 0.0002f
                && TransientState.HasFlag(TransientStateFlags.OnWalkable))
                Velocity = Vector3.Zero;

            // Euler integration: position += v*dt + 0.5*a*dt²
            Position += Velocity * dt + Acceleration * (0.5f * dt * dt);
        }

        // velocity += acceleration * dt  (done unconditionally in decompile)
        Velocity += Acceleration * dt;

        // Angular integration: apply omega rotation.
        // omega * dt gives the angle-axis delta rotation.
        float omegaLen = Omega.Length();
        if (omegaLen > 1e-6f)
        {
            float angle = omegaLen * dt;
            Quaternion deltaRot = Quaternion.CreateFromAxisAngle(Omega / omegaLen, angle);
            Orientation = Quaternion.Normalize(Quaternion.Multiply(Orientation, deltaRot));
        }
    }

    // ── FUN_00515020 ───────────────────────────────────────────────────────

    /// <summary>
    /// Per-frame top-level driver.  Computes dt from the wall clock versus
    /// LastUpdateTime, clamping to [MinQuantum, HugeQuantum], then calls
    /// calc_acceleration and UpdatePhysicsInternal.
    ///
    /// Decompiled logic (FUN_00515020):
    ///   if parent-attached (offset +0x40 != 0) → return
    ///   dVar1 = currentTime - LastUpdateTime
    ///   if dVar1 &lt; MinQuantum → return   (too short — skip)
    ///   if dVar1 > HugeQuantum → update timestamp and return   (stale — discard)
    ///   while dVar1 > MaxQuantum: simulate MaxQuantum step, subtract
    ///   if dVar1 > MinQuantum: simulate remainder
    ///   LastUpdateTime = currentTime
    ///
    /// The caller passes currentTime; the object does not read a global clock
    /// directly in this port so tests can drive the clock explicitly.
    /// </summary>
    public void update_object(double currentTime)
    {
        double deltaTime = currentTime - LastUpdateTime;

        // dt too small — nothing to simulate yet
        if (deltaTime < MinQuantum)
            return;

        // Stale / first frame — just consume the time without simulating
        if (deltaTime > HugeQuantum)
        {
            LastUpdateTime = currentTime;
            return;
        }

        // Sub-step: break large dt into MaxQuantum chunks
        while (deltaTime > MaxQuantum)
        {
            calc_acceleration();
            UpdatePhysicsInternal(MaxQuantum);
            deltaTime -= MaxQuantum;
        }

        // Simulate the remainder
        if (deltaTime > MinQuantum)
        {
            calc_acceleration();
            UpdatePhysicsInternal((float)deltaTime);
        }

        LastUpdateTime = currentTime;
    }
}