carto-revive/src/Craft/Krakensbane.cs

using System.Diagnostics;
using Godot;
using ImGuiNET;
using Quadratic.Carto.MathExt;
using Quadratic.Carto.Orbital;
using Vim.Math3d;

namespace Quadratic.Carto.Craft;

/// <summary>
/// Krakensbane manages the position of its direct descendants, to ensure the
/// focused vessel is not too far from the origin.
/// </summary>
/// <remarks>
/// The name Krakensbane is a legacy of the original KSP module which does the 
/// same thing, i.e. preventing floating point errors from causing the vessel
/// to be thrown into deep space.
/// </remarks>
public sealed partial class Krakensbane : Node3D
{
    [Export]
    public Node3D? FocusedVessel { get; set; }

    const float maxDistance = 500.0f;
    const float planetRadius = 6372.0f;

    CelestialBody celestialBody = CelestialBody.FromSurfaceGravity(9.81, planetRadius, 86164.1);

    DVector3 originPosition = DVector3.UnitY * planetRadius;

    /// <summary>
    /// Something to manage the non-inertial frame of reference.
    /// </summary>
    /// <para>
    /// It's good to have an inertial frame of reference when you're close to
    /// a lot of static stuff, like on the ground. However, a non-inertial frame
    /// of reference might be more appropriate when you're in deep space.
    /// </para>
    /// <para>
    /// This provider field is null if we're using an inertial frame of reference.
    /// When it's not null, it should be responsible for managing the acceleration,
    /// angular acceleration, and even positions and/or rotations of children
    /// objects.
    INonInertialFrameProvider? nonInertialFrameProvider;

    public override void _PhysicsProcess(double delta)
    {
        Debug.Assert(
            FocusedVessel == null || FocusedVessel.GetParent() == this,
            "Focused vessel must be a direct descendant of Krakensbane"
        );

        if (FocusedVessel == null)
        {
            return;
        }

        var vesselPosition = FocusedVessel.Transform.Origin;
        var vesselDistance = vesselPosition.Length();
        if (vesselDistance > maxDistance)
        {
            // Reset the vessel's position to the origin
            // TODO: save the total offset; we don't yet have a double-precision vector3
            var applyTransform = Transform3D.Identity.Translated(-vesselPosition);

            int n_children = GetChildCount();
            for (int i = 0; i < n_children; i++)
            {
                var child = GetChild<Node3D>(i);
                child.Transform = applyTransform * child.Transform;
            }

            originPosition += vesselPosition.AsVim().AsDouble();
        }

        // Apply gravity
        foreach (var child in GetChildren())
        {
            if (child is RigidBody3D rb)
            {
                var localPosition = rb.Transform.Origin.AsVim().AsDouble();
                var globalPosition = originPosition + localPosition;
                var gravityVector = -globalPosition.Normalize();
                var floatGravityVector = gravityVector.AsSingle().AsGodot();
                var distance = globalPosition.Length();
                var gravity = celestialBody.Mu / (distance * distance);
                rb.ApplyCentralForce(floatGravityVector * (float)gravity);
            }
        }
    }

    public DVector3 GetPositionOf(Node3D node)
    {
        var localPosition = node.Transform.Origin.AsVim().AsDouble();
        return originPosition + localPosition;
    }

    public override void _Process(double delta)
    {

        ImGui.Begin("Krakensbane");
        ImGui.Text($"Origin: {originPosition}");
        ImGui.Text($"Focused vessel: {FocusedVessel?.Name}");

        // Debug the orbit of the focused vessel
        if (FocusedVessel != null && FocusedVessel is RigidBody3D rigidBody3D)
        {
            var position = GetPositionOf(rigidBody3D);
            var velocity = rigidBody3D.LinearVelocity.AsVim().AsDouble();
            var state = new DStateVector(position, velocity);
            var (elements, epoch) = KeplerianElements.FromMotionStateFull(state, celestialBody.Mu);
            ImGui.Text("Orbit");
            ImGui.Indent();
            ImGui.Text($"Ap: {elements.Apoapsis}");
            ImGui.Text($"Pe: {elements.Periapsis}");
            ImGui.Text($"Period: {elements.Period(celestialBody.Mu)}");
            ImGui.Text($"Inclination: {elements.IncDegree}deg");
            ImGui.Text($"Epoch: {epoch}");
            ImGui.Unindent();

            // Roundtrip test
            var newState = elements.MotionStateAt(celestialBody.Mu, epoch);
            ImGui.Text("Roundtrip test");
            ImGui.Indent();
            ImGui.Text($"Original position: {position:F6}");
            ImGui.Text($"New position: {newState.Position:F6}");
            ImGui.Text($"Original velocity: {velocity:F6}");
            ImGui.Text($"New velocity: {newState.Velocity:F6}");
            ImGui.Unindent();
        }
        ImGui.End();
    }
}

public interface INonInertialFrameProvider
{
    public DVector3 GetCurrentAcceleration();
    public DVector3 GetCurrentAngularAcceleration();
}