동해 울릉분지 수심 2,000m 심해 해양 탐사 무인잠수정(ROV) 중성부력 밸러스트 탱크 압력 밸런싱 및 1,000Hz 0-Alloc C# 6축 벡터 추진기 자세 제어 엔진

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

namespace ZeliDesk.Education.StemOcean.DeepSeaRov
{
    public readonly record struct RovSensoryTelemetry(
        float DepthMeters,
        float CurrentWaterTempCelsius,
        Vector3 PositionErrorMeters,
        Vector3 VelocityMps,
        Quaternion Orientation,
        Vector3 AngularRatesRadPerSec
    );

    public ref struct ThrusterDistributionOutput
    {
        public float Thruster1Newton;
        public float Thruster2Newton;
        public float Thruster3Newton;
        public float Thruster4Newton;
        public float Vertical1Newton;
        public float Vertical2Newton;
        public float Vertical3Newton;
        public float Vertical4Newton;
        public float BallastPumpVolumeCc;
        public bool HoveringStationKept;
    }

    public static class DeepSeaRovPropulsionCore
    {
        private const float MaxThrustPerMotor = 180.0f; // 최대 180N

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ComputeVectorThrust(
            ref readonly RovSensoryTelemetry tel,
            ref ThrusterDistributionOutput output)
        {
            // 1. 수심에 따른 해수 밀도 및 체적 압축 손실 부력 보상
            float hydrostaticPressureMpa = tel.DepthMeters * 0.01005f;
            float buoyancyLossNewton = hydrostaticPressureMpa * 1.85f; // 부력재 압축 손실

            // 2. 수직 축 PID 요구 추력 (중성부력 상쇄 포함)
            float heaveError = tel.PositionErrorMeters.Z;
            float heaveDemand = (heaveError * 450.0f) - (tel.VelocityMps.Z * 220.0f) + buoyancyLossNewton;
            float vertThrust = Math.Clamp(heaveDemand / 4.0f, -MaxThrustPerMotor, MaxThrustPerMotor);

            output.Vertical1Newton = vertThrust;
            output.Vertical2Newton = vertThrust;
            output.Vertical3Newton = vertThrust;
            output.Vertical4Newton = vertThrust;

            // 3. 수평 평면 4축 벡터 추진기 유사역행렬 분배 (45도 대칭 배치)
            float surgeDemand = (tel.PositionErrorMeters.X * 350.0f) - (tel.VelocityMps.X * 180.0f);
            float swayDemand = (tel.PositionErrorMeters.Y * 350.0f) - (tel.VelocityMps.Y * 180.0f);
            float yawDemand = -tel.AngularRatesRadPerSec.Z * 85.0f;

            float cos45 = 0.7071f;
            output.Thruster1Newton = Math.Clamp((surgeDemand + swayDemand) * cos45 + yawDemand, -MaxThrustPerMotor, MaxThrustPerMotor);
            output.Thruster2Newton = Math.Clamp((surgeDemand - swayDemand) * cos45 - yawDemand, -MaxThrustPerMotor, MaxThrustPerMotor);
            output.Thruster3Newton = Math.Clamp((surgeDemand + swayDemand) * cos45 - yawDemand, -MaxThrustPerMotor, MaxThrustPerMotor);
            output.Thruster4Newton = Math.Clamp((surgeDemand - swayDemand) * cos45 + yawDemand, -MaxThrustPerMotor, MaxThrustPerMotor);

            // 4. 장기 정밀 호버링 판정
            output.BallastPumpVolumeCc = buoyancyLossNewton * 0.98f;
            output.HoveringStationKept = tel.PositionErrorMeters.Length() <= 0.05f;
        }
    }
}