feat(R1-P3): verbatim apply_physics + Frame rotate/grotate
- CSequence.ApplyPhysics (0x00524ab0): copysign semantics — magnitude from the quantum arg, sign from the sign-source arg (call sites pass 1/framerate + signed elapsed); origin += velocity*signed, then rotate(omega*signed). - FrameOps.GRotate (0x005357a0): axis-angle quaternion (angle=|v|, half-angle sin/cos) PREMULTIPLIED onto the orientation — incremental rotation in WORLD space; |v|^2 < F_EPSILON^2 skipped. - FrameOps.Rotate (0x004525b0): local rotation vector mapped through the frame's local->global rotation, then GRotate. 7 numeric conformance tests (copysign matrix, global-space composition, local->global mapping equivalence, epsilon gate). Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
parent
778744bf3e
commit
5138b8fb01
3 changed files with 220 additions and 0 deletions
|
|
@ -253,6 +253,26 @@ public sealed class CSequence
|
||||||
/// <summary><c>get_curr_frame_number</c> (0x005249d0).</summary>
|
/// <summary><c>get_curr_frame_number</c> (0x005249d0).</summary>
|
||||||
public int GetCurrFrameNumber() => (int)Math.Floor(FrameNumber);
|
public int GetCurrFrameNumber() => (int)Math.Floor(FrameNumber);
|
||||||
|
|
||||||
|
// ── apply_physics (0x00524ab0, §19) ─────────────────────────────────
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// Accumulated-physics root motion: advance <paramref name="frame"/> by
|
||||||
|
/// <see cref="Velocity"/>/<see cref="Omega"/> over a signed quantum —
|
||||||
|
/// MAGNITUDE from <paramref name="quantum"/>, SIGN from
|
||||||
|
/// <paramref name="signSource"/> (retail copysign semantics; call sites
|
||||||
|
/// pass 1/framerate as magnitude and the signed elapsed time as sign).
|
||||||
|
/// </summary>
|
||||||
|
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 cursors for P4 (update_internal operates on nodes) ─────
|
||||||
|
|
||||||
internal LinkedListNode<AnimSequenceNode>? CurrAnimNode
|
internal LinkedListNode<AnimSequenceNode>? CurrAnimNode
|
||||||
|
|
|
||||||
58
src/AcDream.Core/Physics/Motion/FrameOps.cs
Normal file
58
src/AcDream.Core/Physics/Motion/FrameOps.cs
Normal file
|
|
@ -0,0 +1,58 @@
|
||||||
|
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|² < 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><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 = Quaternion.Normalize(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));
|
||||||
|
}
|
||||||
142
tests/AcDream.Core.Tests/Physics/Motion/CSequencePhysicsTests.cs
Normal file
142
tests/AcDream.Core.Tests/Physics/Motion/CSequencePhysicsTests.cs
Normal file
|
|
@ -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;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// R1-P3 — <c>CSequence::apply_physics</c> (0x00524ab0) +
|
||||||
|
/// <c>Frame::rotate</c>/<c>grotate</c> (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²).
|
||||||
|
/// </summary>
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
}
|
||||||
Loading…
Add table
Add a link
Reference in a new issue