acdream/src/AcDream.Core/Physics/Motion/MoveToMath.cs

using System;
using System.Numerics;

namespace AcDream.Core.Physics.Motion;

/// <summary>
/// R4-V1 — pure-math free functions consumed by the (future) MoveToManager
/// port: <c>heading_diff</c>, <c>heading_greater</c>, <c>Position::heading</c>
/// / <c>Frame::get_heading</c> / <c>Frame::set_heading</c>, and
/// <c>Position::cylinder_distance</c>. No GL/App dependency — Core-only,
/// per the Code Structure Rules. NAME WATCH: this file (not
/// <c>MoveToManager.cs</c>, per r4-port-plan.md §3 "New code target") is the
/// R4-V1 deliverable; the manager itself is R4-V2.
/// </summary>
public static class MoveToMath
{
    /// <summary>
    /// Universal heading/distance epsilon (same literal as R3's A5/A6 —
    /// r4-moveto-decomp.md §12 constants inventory).
    /// </summary>
    public const float Epsilon = 0.000199999995f;

    /// <summary>
    /// Retail <c>heading_diff</c> (<c>0x00528fb0</c>, free function, raw
    /// 306327-306347), PINNED by direct disassembly of the PDB-matched
    /// retail binary (ghidra-confirmations.md §P3 — the strongest evidence
    /// tier in the R4 pin set):
    /// <code>
    /// d = h1 - h2;
    /// if (fabs(h1 - h2) &lt; F_EPSILON) d = 0;
    /// if (d &lt; -F_EPSILON) d += 360;
    /// if (F_EPSILON &lt; d &amp;&amp; turnCmd != TurnRight) d = 360 - d;   // the mirror
    /// return d;
    /// </code>
    /// The mirror gates on the turn command NOT being TurnRight
    /// (0x6500000d) — TurnLeft (and any other command) measures the
    /// COMPLEMENTARY angle. This CONTRADICTS r4-moveto-decomp.md §5g's
    /// "arg3 UNUSED" claim, which the Ghidra disassembly pin overrides
    /// (V0-pins.md §P3 adjudication). Call sites: <c>BeginTurnToHeading</c>
    /// passes the CONSTANT TurnRight (mirror explicitly disabled — the
    /// direction pick stays the ≤180 test elsewhere); <c>HandleTurnToHeading</c>
    /// passes the LIVE <c>current_command</c> (can be TurnLeft).
    /// </summary>
    /// <param name="h1">First heading, degrees.</param>
    /// <param name="h2">Second heading, degrees.</param>
    /// <param name="turnCmd">The active turn command id — gates the mirror.</param>
    /// <returns>Normalized heading difference, degrees.</returns>
    public static float HeadingDiff(float h1, float h2, uint turnCmd)
    {
        float d = h1 - h2;
        if (MathF.Abs(h1 - h2) < Epsilon)
        {
            d = 0f;
        }
        if (d < -Epsilon)
        {
            d += 360f;
        }
        if (Epsilon < d && turnCmd != MotionCommand.TurnRight)
        {
            d = 360f - d;
        }
        return d;
    }

    /// <summary>
    /// Retail <c>heading_greater</c> (<c>00528f60</c>, free function, raw
    /// 306281-306323), verbatim per r4-moveto-decomp.md §5f:
    /// <code>
    /// if (fabs(a - b) &gt; 180) greater = (b &gt; a);   // wrapped case: compare flipped
    /// else                     greater = (a &gt; b);
    /// if (turnCmd == TurnRight) return greater;
    /// return !greater;                           // TurnLeft (and any other cmd): inverted
    /// </code>
    /// "Has the turn passed the target heading" — direction-aware,
    /// 360°-wrap-aware. The visible TurnRight-arg idiom: the gate is
    /// <c>== TurnRight</c> (not <c>!= TurnRight</c> as in
    /// <see cref="HeadingDiff"/>'s mirror) — every OTHER command inverts,
    /// not just TurnLeft specifically.
    /// </summary>
    public static bool HeadingGreater(float a, float b, uint turnCmd)
    {
        bool greater = MathF.Abs(a - b) > 180f
            ? b > a
            : a > b;

        return turnCmd == MotionCommand.TurnRight ? greater : !greater;
    }

    /// <summary>
    /// Retail <c>Position::heading(from, to)</c> (<c>0x005a9520</c>, raw
    /// 438288-438290), PINNED per V0-pins.md §P5: compass degrees, 0 =
    /// North (+Y), 90 = East (+X), CLOCKWISE, range [0,360).
    /// <code>
    /// heading(from, to) = (450 - atan2Deg(dy, dx)) % 360
    /// </code>
    /// Golden cardinals: N(0,+1)→0, E(+1,0)→90, S(0,-1)→180, W(-1,0)→270.
    /// Horizontal (X/Y) only — Z (height) does not participate, matching
    /// retail's compass-heading semantics. An in-tree twin of this formula
    /// already exists at <c>SceneryHelpers.cs:75</c> (render-side,
    /// independently verified — not reused directly to keep this file
    /// GL-free per the Code Structure Rules, but the formula is identical).
    /// </summary>
    public static float PositionHeading(Vector3 from, Vector3 to)
    {
        float dx = to.X - from.X;
        float dy = to.Y - from.Y;
        float headingDeg = 450f - MathF.Atan2(dy, dx) * (180f / MathF.PI);
        headingDeg %= 360f;
        if (headingDeg < 0f) headingDeg += 360f;
        return headingDeg;
    }

    /// <summary>
    /// Retail <c>Frame::get_heading</c> (<c>0x00535760</c>, raw 319781) —
    /// extracts the compass heading (P5 convention) from a body orientation
    /// quaternion. <b>The packer-reuse trap (V0-pins §P5 correction):</b>
    /// acdream's outbound packer (<c>GameWindow.YawToAcQuaternion</c>) is
    /// wire-correct at the QUATERNION level but its internal scalar
    /// intermediate (<c>headingDeg = 180 - yawDeg</c>) is holtburger's
    /// SHIFTED convention, not retail's. This method uses the CORRECT
    /// scalar bridge derived from acdream's own body convention
    /// (<c>PlayerMovementController.cs:1022-1025</c>: <c>Orientation =
    /// AxisAngle(Z, Yaw - PI/2)</c>, local-forward = +Y, Yaw=0 faces +X):
    /// world-forward = <c>(cos Yaw, sin Yaw)</c>, so
    /// <c>YawDeg = atan2Deg(forward.Y, forward.X)</c> and
    /// <c>heading = (90 - YawDeg) mod 360</c> — the exact inverse of
    /// <see cref="SetHeading"/>. Identity quaternion (Yaw=PI/2, i.e. facing
    /// +Y/North) → heading 0, matching P5's "identity quaternion faces
    /// heading 0" pin.
    /// </summary>
    public static float GetHeading(Quaternion orientation)
    {
        var forward = Vector3.Transform(new Vector3(0f, 1f, 0f), orientation);
        float yawDeg = MathF.Atan2(forward.Y, forward.X) * (180f / MathF.PI);
        float headingDeg = 90f - yawDeg;
        headingDeg %= 360f;
        if (headingDeg < 0f) headingDeg += 360f;
        return headingDeg;
    }

    /// <summary>
    /// Retail <c>Frame::set_heading</c> (<c>0x00535e40</c>, raw
    /// 320055-320066) — builds a body orientation quaternion facing
    /// <paramref name="headingDeg"/> (P5 compass convention), preserving
    /// acdream's body-orientation convention (rotation about world Z only;
    /// <paramref name="baseOrientation"/>'s pitch/roll, if any, is
    /// discarded — matching retail's <c>set_heading</c>, which is a pure
    /// yaw-about-Z setter). Exact inverse of <see cref="GetHeading"/>:
    /// <c>YawDeg = 90 - headingDeg</c>, then <c>Orientation =
    /// AxisAngle(Z, Yaw - PI/2)</c> per
    /// <c>PlayerMovementController.cs:1025</c>'s convention.
    /// </summary>
    /// <param name="baseOrientation">Unused beyond signature parity with
    /// the render-side <c>SceneryHelpers.SetHeading</c> twin — retail's
    /// <c>set_heading</c> is a pure yaw-about-Z setter with no dependency
    /// on the prior orientation's roll/pitch component in the body-frame
    /// convention this port uses.</param>
    /// <param name="headingDeg">Desired compass heading, degrees.</param>
    public static Quaternion SetHeading(Quaternion baseOrientation, float headingDeg)
    {
        _ = baseOrientation;
        float yawDeg = 90f - headingDeg;
        float yaw = yawDeg * (MathF.PI / 180f);
        return Quaternion.CreateFromAxisAngle(Vector3.UnitZ, yaw - MathF.PI / 2f);
    }

    /// <summary>
    /// Retail <c>Position::cylinder_distance</c>, the pure-math shape
    /// consumed by <c>MoveToManager::GetCurrentDistance</c>
    /// (<c>005291b0</c>, r4-moveto-decomp.md §5a) when <c>use_spheres</c>
    /// (wire bit 0x400) is set — object moves use edge-to-edge cylinder
    /// distance; position moves use plain center (Euclidean) distance
    /// instead (not ported here — <c>Vector3.Distance</c> already covers
    /// it). BN garbles the x87 plumbing in the raw, so the exact
    /// radius-combination arithmetic is not directly visible; ported per
    /// the PDB argument ORDER (own radius/height/position, target
    /// radius/height/position) with the standard cylinder-distance shape:
    /// planar (X/Y) center distance minus the sum of both radii, clamped
    /// at zero (overlapping cylinders report 0, never negative). Height is
    /// accepted for signature parity with the retail call (own/target
    /// height feed the caller's contact-plane logic elsewhere) but does
    /// NOT participate in this edge-to-edge planar computation — matching
    /// retail's use of cylinder_distance purely for the horizontal arrival
    /// gate.
    /// </summary>
    public static float CylinderDistance(
        float ownRadius, float ownHeight, Vector3 ownPos,
        float targetRadius, float targetHeight, Vector3 targetPos)
    {
        _ = ownHeight;
        _ = targetHeight;

        float dx = targetPos.X - ownPos.X;
        float dy = targetPos.Y - ownPos.Y;
        float centerDist = MathF.Sqrt(dx * dx + dy * dy);

        float edgeDist = centerDist - ownRadius - targetRadius;
        return edgeDist > 0f ? edgeDist : 0f;
    }
}