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

using System;
using System.Numerics;
using DatReaderWriter.Types;

namespace AcDream.Core.Physics.Motion;

/// <summary>
/// R1-P3 — verbatim ports of retail's <c>Frame</c> rotation helpers used by
/// the CSequence physics path (oracle: raw decomp reads 2026-07-02).
///
/// <list type="bullet">
/// <item><c>Frame::grotate</c> (0x005357a0, pc:319782): build the
///   axis-angle quaternion from a GLOBAL rotation vector (angle = |v|,
///   axis = v/|v|, half-angle sin/cos) and PREMULTIPLY it onto the frame's
///   orientation — the incremental rotation is applied in world space.
///   Rotations with |v|² &lt; F_EPSILON² are skipped.</item>
/// <item><c>Frame::rotate</c> (0x004525b0, pc:91477): map the LOCAL
///   rotation vector through the frame's local→global rotation
///   (m_fl2gv — our quaternion), then <c>grotate</c>.</item>
/// </list>
/// </summary>
public static class FrameOps
{
    /// <summary>Retail F_EPSILON (0.000199999995f); grotate gates on its
    /// square against |v|².</summary>
    public const float FEpsilon = 0.000199999995f;

    /// <summary>
    /// Retail <c>Frame::set_rotate</c> (0x00535080). The candidate is
    /// normalized in extended precision, then the complete frame is checked
    /// by <c>Frame::IsValid</c> (0x00534ED0). If that check fails, retail
    /// restores the previous quaternion instead of allowing NaNs to poison
    /// the live object transform.
    /// </summary>
    public static Quaternion SetRotate(
        Vector3 frameOrigin,
        Quaternion previous,
        Quaternion candidate)
    {
        double lengthSquared =
            ((double)candidate.W * candidate.W)
            + ((double)candidate.X * candidate.X)
            + ((double)candidate.Y * candidate.Y)
            + ((double)candidate.Z * candidate.Z);
        double inverseLength = 1.0 / Math.Sqrt(lengthSquared);
        var normalized = new Quaternion(
            (float)(candidate.X * inverseLength),
            (float)(candidate.Y * inverseLength),
            (float)(candidate.Z * inverseLength),
            (float)(candidate.W * inverseLength));

        if (float.IsNaN(frameOrigin.X)
            || float.IsNaN(frameOrigin.Y)
            || float.IsNaN(frameOrigin.Z)
            || float.IsNaN(normalized.W)
            || float.IsNaN(normalized.X)
            || float.IsNaN(normalized.Y)
            || float.IsNaN(normalized.Z))
        {
            return previous;
        }

        float normalizedLengthSquared = normalized.LengthSquared();
        return !float.IsNaN(normalizedLengthSquared)
               && MathF.Abs(normalizedLengthSquared - 1f) < FEpsilon * 5f
            ? normalized
            : previous;
    }

    /// <summary><c>Frame::grotate</c> — incremental WORLD-space rotation.</summary>
    public static void GRotate(Frame frame, Vector3 rotationGlobal)
    {
        float magSq = rotationGlobal.LengthSquared();
        if (magSq < FEpsilon * FEpsilon)
            return;

        float angle = MathF.Sqrt(magSq);
        float invMag = 1f / angle;
        float half = angle * 0.5f;
        float s = MathF.Sin(half);
        float c = MathF.Cos(half);

        // Retail's set_rotate receives the raw Hamilton product r ⊗ q
        // (r = the new axis-angle quat, q = the current orientation) —
        // rotation applied in GLOBAL space. System.Numerics
        // Quaternion.Multiply(r, q) is that product with
        // Transform(v, r*q) == r-applied-after-q. set_rotate renormalizes.
        var r = new Quaternion(
            rotationGlobal.X * s * invMag,
            rotationGlobal.Y * s * invMag,
            rotationGlobal.Z * s * invMag,
            c);
        frame.Orientation = SetRotate(
            frame.Origin,
            frame.Orientation,
            Quaternion.Multiply(r, frame.Orientation));
    }

    /// <summary><c>Frame::rotate</c> — LOCAL rotation vector, mapped to
    /// global through the orientation, then <see cref="GRotate"/>.</summary>
    public static void Rotate(Frame frame, Vector3 rotationLocal)
        => GRotate(frame, Vector3.Transform(rotationLocal, frame.Orientation));

    /// <summary>
    /// <c>Frame::combine</c> as used by the CSequence pose path (ACE
    /// <c>AFrame.Combine</c>, verified against the pre-R1 acdream port):
    /// apply the pose — origin += rotate(pos.Origin by orientation), then
    /// orientation ∘= pos.Orientation.
    /// </summary>
    public static void Combine(Frame frame, Frame pos)
    {
        frame.Origin += Vector3.Transform(pos.Origin, frame.Orientation);
        frame.Orientation = SetRotate(
            frame.Origin,
            frame.Orientation,
            frame.Orientation * pos.Orientation);
    }

    /// <summary>
    /// <c>Frame::subtract1</c> — un-apply the pose: orientation ∘=
    /// conj(pos.Orientation) FIRST, then origin −= rotate(pos.Origin by the
    /// UPDATED orientation) (inverse order of <see cref="Combine"/>).
    /// </summary>
    public static void Subtract1(Frame frame, Frame pos)
    {
        frame.Orientation = SetRotate(
            frame.Origin,
            frame.Orientation,
            frame.Orientation * Quaternion.Conjugate(pos.Orientation));
        frame.Origin -= Vector3.Transform(pos.Origin, frame.Orientation);
    }
}