생체모사 가오리(Batoid) 유연 가슴지느러미 기복 파동 & 1,000Hz 0-Alloc C# 유체역학 추력 서보 엔진

using System;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;

namespace ZeliDesk.Stem.BiomimeticPropulsion
{
    /// <summary>
    /// 수중 유영 속도 및 방향타 센서 데이터
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Pack = 8)]
    public readonly struct BatoidKinematicsData
    {
        public readonly double ForwardSpeedM_S; // 전진 속도 (m/s)
        public readonly double DesiredYawRateRadS; // 선회 요구 각속도 (rad/s)
        public readonly double DesiredPitchAngleRad; // 상승/하강 피치각

        public BatoidKinematicsData(double u, double yaw, double pitch)
        {
            ForwardSpeedM_S = u;
            DesiredYawRateRadS = yaw;
            DesiredPitchAngleRad = pitch;
        }
    }

    /// <summary>
    /// 6개 관절 분절 모터 각도 명령 (스택 할당 0-Alloc)
    /// </summary>
    public ref struct BatoidFinJointAngles
    {
        public double Joint1Deg; // 전단
        public double Joint2Deg;
        public double Joint3Deg;
        public double Joint4Deg;
        public double Joint5Deg;
        public double Joint6Deg; // 후단

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public BatoidFinJointAngles(double j1, double j2, double j3, double j4, double j5, double j6)
        {
            Joint1Deg = j1; Joint2Deg = j2; Joint3Deg = j3;
            Joint4Deg = j4; Joint5Deg = j5; Joint6Deg = j6;
        }
    }

    /// <summary>
    /// 1,000Hz 무할당 생체모사 가오리 지느러미 추진 서보 엔진
    /// </summary>
    public ref struct BiomimeticBatoidFinEngine
    {
        private const double OptimalStrouhal = 0.30; // 최적 스트루할 수
        private const double FinLengthM = 0.65;      // 지느러미 길이 (m)
        private const double MaxAmplitudeDeg = 28.0; // 최대 진동각 (도)

        private double _globalPhaseTime;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ExecuteControlCycle(
            in BatoidKinematicsData kinematics,
            double timeStepSec,
            bool isRightFin,
            out BatoidFinJointAngles jointAngles,
            out double estimatedThrustN,
            out double currentStrouhal)
        {
            _globalPhaseTime += timeStepSec;

            // 1. 유영 속도에 기초한 최적 진동 주파수 f 산출 (St = f * A / U = 0.3)
            double u = Math.Max(kinematics.ForwardSpeedM_S, 0.2);
            double amplitudeM = FinLengthM * Math.Sin(MaxAmplitudeDeg * Math.PI / 180.0);
            double targetFrequency = (OptimalStrouhal * u) / amplitudeM;
            targetFrequency = Math.Clamp(targetFrequency, 0.5, 3.5); // 0.5Hz ~ 3.5Hz

            currentStrouhal = (targetFrequency * amplitudeM) / u;

            // 2. 선회(Yaw) 요구에 따른 좌/우 차동 진폭 계산
            double yawDifferential = isRightFin ? -kinematics.DesiredYawRateRadS * 8.0 : kinematics.DesiredYawRateRadS * 8.0;
            double actualAmpDeg = Math.Clamp(MaxAmplitudeDeg + yawDifferential, 5.0, 35.0);

            // 3. 피치(Pitch) 요구에 따른 바이어스 오프셋 각도
            double pitchBiasDeg = kinematics.DesiredPitchAngleRad * (180.0 / Math.PI) * 0.5;

            // 4. 길이 방향 분산 위상 지연(진행파 형성) 각도 계산
            double omega = 2.0 * Math.PI * targetFrequency;
            double waveNumberK = 2.0 * Math.PI / FinLengthM; // 1파장 = 지느러미 전체 길이

            double p1 = omega * _globalPhaseTime - waveNumberK * (FinLengthM * 0.1);
            double p2 = omega * _globalPhaseTime - waveNumberK * (FinLengthM * 0.28);
            double p3 = omega * _globalPhaseTime - waveNumberK * (FinLengthM * 0.46);
            double p4 = omega * _globalPhaseTime - waveNumberK * (FinLengthM * 0.64);
            double p5 = omega * _globalPhaseTime - waveNumberK * (FinLengthM * 0.82);
            double p6 = omega * _globalPhaseTime - waveNumberK * (FinLengthM * 1.0);

            // 후단으로 갈수록 진폭 증가 (선형 엔벨로프)
            double j1 = (0.35 * actualAmpDeg) * Math.Sin(p1) + pitchBiasDeg;
            double j2 = (0.48 * actualAmpDeg) * Math.Sin(p2) + pitchBiasDeg;
            double j3 = (0.61 * actualAmpDeg) * Math.Sin(p3) + pitchBiasDeg;
            double j4 = (0.74 * actualAmpDeg) * Math.Sin(p4) + pitchBiasDeg;
            double j5 = (0.87 * actualAmpDeg) * Math.Sin(p5) + pitchBiasDeg;
            double j6 = (1.00 * actualAmpDeg) * Math.Sin(p6) + pitchBiasDeg;

            jointAngles = new BatoidFinJointAngles(j1, j2, j3, j4, j5, j6);

            // 5. Lighthill 연장체 이론에 따른 예상 추력 (N)
            estimatedThrustN = 0.5 * 1025.0 * Math.PI * Math.Pow(FinLengthM * 0.4, 2.0) * Math.Pow(amplitudeM * targetFrequency, 2.0);
        }
    }
}