2022 개정 교육과정 물리-화학-지구과학-수학-정보 융합 국방 AI 온디바이스 시뮬레이터 및 1,000Hz 0-Alloc C# 고성능 멀티바디 피직스 엔진

using System;
using System.Numerics;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Education.StemPhysics.MultiBody
{
    public readonly record struct RigidBodyState(
        Vector3 Position,
        Vector3 Velocity,
        Quaternion Orientation,
        Vector3 AngularVelocity,
        float MassKg,
        Vector3 InertiaTensorDiag
    );

    public ref struct PhysicsIntegratorOutput
    {
        public Vector3 NextPosition;
        public Vector3 NextVelocity;
        public Quaternion NextOrientation;
        public Vector3 NextAngularVelocity;
        public float KineticEnergyJoules;
    }

    public static class StemGrandPhysicsCore
    {
        private static readonly Vector3 Gravity = new(0, 0, -9.80665f);

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void StepSymplecticVerlet(
            ref readonly RigidBodyState body,
            Vector3 appliedForceNewton,
            Vector3 appliedTorqueNm,
            float dtSeconds,
            ref PhysicsIntegratorOutput output)
        {
            float invMass = 1.0f / body.MassKg;

            // 1. 선형 가속도 및 심플렉틱 속도-위치 갱신 (에너지 보존 베를레 적분)
            Vector3 accel = (appliedForceNewton * invMass) + Gravity;
            Vector3 nextVel = body.Velocity + (accel * dtSeconds);
            Vector3 nextPos = body.Position + (nextVel * dtSeconds);

            // 2. 각가속도 및 오일러 회전 (dOmega = I^-1 * (Torque - Omega x (I * Omega)))
            Vector3 iOmega = new(
                body.AngularVelocity.X * body.InertiaTensorDiag.X,
                body.AngularVelocity.Y * body.InertiaTensorDiag.Y,
                body.AngularVelocity.Z * body.InertiaTensorDiag.Z
            );
            Vector3 gyroTorque = Vector3.Cross(body.AngularVelocity, iOmega);
            Vector3 netTorque = appliedTorqueNm - gyroTorque;

            Vector3 alpha = new(
                netTorque.X / body.InertiaTensorDiag.X,
                netTorque.Y / body.InertiaTensorDiag.Y,
                netTorque.Z / body.InertiaTensorDiag.Z
            );

            Vector3 nextAngVel = body.AngularVelocity + (alpha * dtSeconds);

            // 3. 쿼터니언 회전 갱신 (dq/dt = 0.5 * q * omega)
            Quaternion angVelQuat = new(nextAngVel.X, nextAngVel.Y, nextAngVel.Z, 0);
            Quaternion qDot = Quaternion.Multiply(body.Orientation, angVelQuat);
            Quaternion unnormNextQ = new(
                body.Orientation.X + qDot.X * 0.5f * dtSeconds,
                body.Orientation.Y + qDot.Y * 0.5f * dtSeconds,
                body.Orientation.Z + qDot.Z * 0.5f * dtSeconds,
                body.Orientation.W + qDot.W * 0.5f * dtSeconds
            );
            Quaternion nextOrient = Quaternion.Normalize(unnormNextQ);

            // 4. 출력 기록
            output.NextPosition = nextPos;
            output.NextVelocity = nextVel;
            output.NextOrientation = nextOrient;
            output.NextAngularVelocity = nextAngVel;

            // 5. 역학적 운동 에너지 연산 (E_k = 0.5 * m * v^2 + 0.5 * omega^T * I * omega)
            float linearEk = 0.5f * body.MassKg * nextVel.LengthSquared();
            float rotEk = 0.5f * (nextAngVel.X * nextAngVel.X * body.InertiaTensorDiag.X +
                                 nextAngVel.Y * nextAngVel.Y * body.InertiaTensorDiag.Y +
                                 nextAngVel.Z * nextAngVel.Z * body.InertiaTensorDiag.Z);
            output.KineticEnergyJoules = linearEk + rotEk;
        }
    }
}