acdream/tests/AcDream.Core.Tests/Physics/Motion/MoveToMathPositionHeadingTests.cs

using System.Numerics;
using AcDream.Core.Physics;
using AcDream.Core.Physics.Motion;
using Xunit;

namespace AcDream.Core.Tests.Physics.Motion;

/// <summary>
/// R4-V1 — <c>Position::heading</c> / <c>Frame::get_heading</c> /
/// <c>Frame::set_heading</c>, per V0-pins.md §P5 (PINNED — compass degrees,
/// 0 = North (+Y), 90 = East (+X), CLOCKWISE, [0,360); identity quaternion
/// faces heading 0):
/// <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.
///
/// <para>
/// <b>The scalar-convention trap (V0-pins §P5 correction):</b>
/// <see cref="MoveToMath.GetHeading"/> must use the correct scalar bridge
/// from acdream yaw (yaw=0 faces +X, per
/// <c>PlayerMovementController.cs:1022-1025</c>): <c>heading = (90 -
/// yawDeg) mod 360</c> — NOT <c>180 - yawDeg</c>.
/// </para>
/// </summary>
public sealed class MoveToMathPositionHeadingTests
{
    private const float Tol = 0.01f;

    // ── PositionHeading: the four cardinal offsets ─────────────────────────

    [Fact]
    public void North_PlusY_IsZero()
    {
        float h = MoveToMath.PositionHeading(Vector3.Zero, new Vector3(0f, 1f, 0f));
        Assert.Equal(0f, h, 2);
    }

    [Fact]
    public void East_PlusX_Is90()
    {
        float h = MoveToMath.PositionHeading(Vector3.Zero, new Vector3(1f, 0f, 0f));
        Assert.Equal(90f, h, 2);
    }

    [Fact]
    public void South_MinusY_Is180()
    {
        float h = MoveToMath.PositionHeading(Vector3.Zero, new Vector3(0f, -1f, 0f));
        Assert.Equal(180f, h, 2);
    }

    [Fact]
    public void West_MinusX_Is270()
    {
        float h = MoveToMath.PositionHeading(Vector3.Zero, new Vector3(-1f, 0f, 0f));
        Assert.Equal(270f, h, 2);
    }

    [Fact]
    public void Heading_IsAlways_InZeroToThreeSixtyRange()
    {
        // NE diagonal
        float h = MoveToMath.PositionHeading(Vector3.Zero, new Vector3(1f, 1f, 0f));
        Assert.InRange(h, 0f, 360f);
        Assert.Equal(45f, h, 2);
    }

    [Fact]
    public void Heading_IgnoresZ_HorizontalOnly()
    {
        float h1 = MoveToMath.PositionHeading(new Vector3(0, 0, 5f), new Vector3(1f, 0f, -10f));
        float h2 = MoveToMath.PositionHeading(new Vector3(0, 0, -3f), new Vector3(1f, 0f, 100f));
        Assert.Equal(h1, h2, 2);
        Assert.Equal(90f, h1, 2);
    }

    // ── GetHeading: extracts heading from a body orientation quaternion ────

    [Fact]
    public void GetHeading_IdentityQuaternion_FacesHeadingZero()
    {
        // Identity quaternion → acdream yaw = 0 → +X-facing in our
        // convention, which decodes to AC heading 90 per the corrected
        // scalar bridge... BUT the identity quaternion in acdream's body
        // frame corresponds to yaw = -PI/2 relative to +Y-forward (see
        // PlayerMovementController.cs:1025: Orientation = AxisAngle(Yaw -
        // PI/2)). GetHeading must invert that exact convention: identity
        // orientation (no rotation applied) means Yaw=PI/2 was baked in,
        // which is heading 0 — matching P5's "identity quaternion faces
        // heading 0" pin.
        float h = MoveToMath.GetHeading(Quaternion.Identity);
        Assert.Equal(0f, h, 1);
    }

    [Fact]
    public void GetHeading_SetHeading_RoundTrips_Cardinals()
    {
        foreach (float heading in new[] { 0f, 90f, 180f, 270f, 45f, 359f })
        {
            var q = MoveToMath.SetHeading(Quaternion.Identity, heading);
            float back = MoveToMath.GetHeading(q);
            float diff = MathF.Abs(back - heading);
            if (diff > 180f) diff = 360f - diff;
            Assert.True(diff < 0.5f, $"heading {heading} round-tripped to {back}");
        }
    }

    [Fact]
    public void SetHeading_North_ProducesForwardVectorFacingPlusY()
    {
        var q = MoveToMath.SetHeading(Quaternion.Identity, 0f);
        var forward = Vector3.Transform(new Vector3(0f, 1f, 0f), q);
        Assert.True(forward.Y > 0.9f, $"expected +Y forward, got {forward}");
    }

    [Fact]
    public void SetHeading_East_ProducesForwardVectorFacingPlusX()
    {
        var q = MoveToMath.SetHeading(Quaternion.Identity, 90f);
        var forward = Vector3.Transform(new Vector3(0f, 1f, 0f), q);
        Assert.True(forward.X > 0.9f, $"expected +X forward, got {forward}");
    }
}