diff --git a/src/AcDream.Core/Physics/Motion/CSequence.cs b/src/AcDream.Core/Physics/Motion/CSequence.cs index 4a41c2de..06689e0b 100644 --- a/src/AcDream.Core/Physics/Motion/CSequence.cs +++ b/src/AcDream.Core/Physics/Motion/CSequence.cs @@ -253,6 +253,26 @@ public sealed class CSequence /// get_curr_frame_number (0x005249d0). public int GetCurrFrameNumber() => (int)Math.Floor(FrameNumber); + // ── apply_physics (0x00524ab0, §19) ───────────────────────────────── + + /// + /// Accumulated-physics root motion: advance by + /// / over a signed quantum — + /// MAGNITUDE from , SIGN from + /// (retail copysign semantics; call sites + /// pass 1/framerate as magnitude and the signed elapsed time as sign). + /// + public void ApplyPhysics(Frame frame, double quantum, double signSource) + { + double signed = Math.Abs(quantum); + if (signSource < 0.0) + signed = -signed; + + float sq = (float)signed; + frame.Origin += Velocity * sq; + FrameOps.Rotate(frame, Omega * sq); + } + // ── internal cursors for P4 (update_internal operates on nodes) ───── internal LinkedListNode? CurrAnimNode diff --git a/src/AcDream.Core/Physics/Motion/FrameOps.cs b/src/AcDream.Core/Physics/Motion/FrameOps.cs new file mode 100644 index 00000000..650d90b3 --- /dev/null +++ b/src/AcDream.Core/Physics/Motion/FrameOps.cs @@ -0,0 +1,58 @@ +using System; +using System.Numerics; +using DatReaderWriter.Types; + +namespace AcDream.Core.Physics.Motion; + +/// +/// R1-P3 — verbatim ports of retail's Frame rotation helpers used by +/// the CSequence physics path (oracle: raw decomp reads 2026-07-02). +/// +/// +/// Frame::grotate (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|² < F_EPSILON² are skipped. +/// Frame::rotate (0x004525b0, pc:91477): map the LOCAL +/// rotation vector through the frame's local→global rotation +/// (m_fl2gv — our quaternion), then grotate. +/// +/// +public static class FrameOps +{ + /// Retail F_EPSILON (0.000199999995f); grotate gates on its + /// square against |v|². + public const float FEpsilon = 0.000199999995f; + + /// Frame::grotate — incremental WORLD-space rotation. + 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 = Quaternion.Normalize(Quaternion.Multiply(r, frame.Orientation)); + } + + /// Frame::rotate — LOCAL rotation vector, mapped to + /// global through the orientation, then . + public static void Rotate(Frame frame, Vector3 rotationLocal) + => GRotate(frame, Vector3.Transform(rotationLocal, frame.Orientation)); +} diff --git a/tests/AcDream.Core.Tests/Physics/Motion/CSequencePhysicsTests.cs b/tests/AcDream.Core.Tests/Physics/Motion/CSequencePhysicsTests.cs new file mode 100644 index 00000000..a1d4c972 --- /dev/null +++ b/tests/AcDream.Core.Tests/Physics/Motion/CSequencePhysicsTests.cs @@ -0,0 +1,142 @@ +using System; +using System.Numerics; +using AcDream.Core.Physics; +using AcDream.Core.Physics.Motion; +using DatReaderWriter.Types; + +namespace AcDream.Core.Tests.Physics.Motion; + +/// +/// R1-P3 — CSequence::apply_physics (0x00524ab0) + +/// Frame::rotate/grotate (0x004525b0/0x005357a0) verbatim +/// (gap G7's math half). Oracle: r1-csequence-decomp.md §19 + raw decomp +/// reads of rotate/grotate this session. +/// +/// KEY SEMANTICS: +/// - apply_physics takes MAGNITUDE from arg3 and SIGN from arg4 +/// (copysign): origin += velocity·signed; rotate(omega·signed); +/// - rotate() maps the LOCAL rotation vector through the frame's +/// orientation to GLOBAL, then grotate premultiplies the axis-angle +/// quaternion (rotation applied in world space); +/// - grotate skips rotations with |v|² < F_EPSILON² (0.000199999995²). +/// +public class CSequencePhysicsTests +{ + private sealed class NullLoader : IAnimationLoader + { + public DatReaderWriter.DBObjs.Animation? LoadAnimation(uint id) => null; + } + + private static void AssertVec(Vector3 expected, Vector3 actual, float tol = 1e-5f) + { + Assert.True((expected - actual).Length() < tol, + $"expected {expected}, got {actual}"); + } + + [Fact] + public void ApplyPhysics_PositiveSign_AddsVelocityTimesQuantum() + { + var seq = new CSequence(new NullLoader()); + seq.SetVelocity(new Vector3(2, 0, 0)); + var frame = new Frame { Origin = new Vector3(1, 1, 1), Orientation = Quaternion.Identity }; + + seq.ApplyPhysics(frame, quantum: 0.5, signSource: 1.0); + + AssertVec(new Vector3(2, 1, 1), frame.Origin); + } + + [Fact] + public void ApplyPhysics_CopySign_MagnitudeFromQuantum_SignFromSource() + { + // signed_quantum = copysign(fabs(quantum), sign_source): a NEGATIVE + // quantum with a POSITIVE sign source still moves forward; a + // negative sign source reverses. + var seq = new CSequence(new NullLoader()); + seq.SetVelocity(new Vector3(2, 0, 0)); + + var f1 = new Frame { Origin = Vector3.Zero, Orientation = Quaternion.Identity }; + seq.ApplyPhysics(f1, quantum: -0.5, signSource: 1.0); + AssertVec(new Vector3(1, 0, 0), f1.Origin); + + var f2 = new Frame { Origin = Vector3.Zero, Orientation = Quaternion.Identity }; + seq.ApplyPhysics(f2, quantum: 0.5, signSource: -1.0); + AssertVec(new Vector3(-1, 0, 0), f2.Origin); + } + + [Fact] + public void ApplyPhysics_OmegaRotatesFrame_GlobalZ() + { + // omega = (0,0,π) for 0.5s → 90° about global Z: local +X maps to +Y. + var seq = new CSequence(new NullLoader()); + seq.SetOmega(new Vector3(0, 0, MathF.PI)); + var frame = new Frame { Origin = Vector3.Zero, Orientation = Quaternion.Identity }; + + seq.ApplyPhysics(frame, quantum: 0.5, signSource: 1.0); + + AssertVec(new Vector3(0, 1, 0), Vector3.Transform(Vector3.UnitX, frame.Orientation)); + } + + [Fact] + public void GRotate_ComposesInGlobalSpace() + { + // Frame already rotated 90° about Z; grotate 90° about GLOBAL X. + // local +X: q maps it to +Y; global X-rot then maps +Y to +Z. + var frame = new Frame + { + Origin = Vector3.Zero, + Orientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI / 2f), + }; + + FrameOps.GRotate(frame, new Vector3(MathF.PI / 2f, 0, 0)); + + AssertVec(new Vector3(0, 0, 1), Vector3.Transform(Vector3.UnitX, frame.Orientation)); + } + + [Fact] + public void Rotate_LocalVector_MappedThroughOrientation() + { + // Frame rotated 90° about Z: a LOCAL X-axis rotation is a GLOBAL + // Y-axis rotation. rotate(local πX/2) on this frame must equal + // grotate(global πY/2). + var q0 = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI / 2f); + var viaLocal = new Frame { Origin = Vector3.Zero, Orientation = q0 }; + FrameOps.Rotate(viaLocal, new Vector3(MathF.PI / 2f, 0, 0)); + + var viaGlobal = new Frame { Origin = Vector3.Zero, Orientation = q0 }; + FrameOps.GRotate(viaGlobal, new Vector3(0, MathF.PI / 2f, 0)); + + AssertVec( + Vector3.Transform(Vector3.UnitX, viaGlobal.Orientation), + Vector3.Transform(Vector3.UnitX, viaLocal.Orientation)); + AssertVec( + Vector3.Transform(Vector3.UnitZ, viaGlobal.Orientation), + Vector3.Transform(Vector3.UnitZ, viaLocal.Orientation)); + } + + [Fact] + public void GRotate_TinyRotation_Skipped() + { + // |v|² < F_EPSILON² → no-op (0x005357a0 early return). + var frame = new Frame { Origin = Vector3.Zero, Orientation = Quaternion.Identity }; + FrameOps.GRotate(frame, new Vector3(1e-5f, 0, 0)); + Assert.Equal(Quaternion.Identity, frame.Orientation); + } + + [Fact] + public void ApplyPhysics_ZeroPhysics_NoChange() + { + var seq = new CSequence(new NullLoader()); + var frame = new Frame + { + Origin = new Vector3(3, 4, 5), + Orientation = Quaternion.CreateFromAxisAngle(Vector3.UnitY, 0.3f), + }; + var beforeO = frame.Origin; + var beforeQ = frame.Orientation; + + seq.ApplyPhysics(frame, 1.0, 1.0); + + Assert.Equal(beforeO, frame.Origin); + Assert.Equal(beforeQ, frame.Orientation); + } +}