Add Metalstorm-style cursor aim

Virtual stick cursor steered through the gun reticle, server-side virtual pilot
using measured control responses, client cursor/camera handling with look-ahead,
free look and scroll zoom.

Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01YVREx3uo7nz1FEhzveWTUh

src/main/java/dev/lumareth/improvedAerodynamics/flight/AeroState.java

@@ -27,6 +27,7 @@ public final class AeroState implements PhysicsBridge.Publishable {
     private static final double INPUT_RATE_PER_TICK = 0.2;
     private static final double VELOCITY_EPSILON = 0.05;
     private static final double ANGULAR_EPSILON = 0.005;
+    private static final ControlAxis[] MEASURED_AXES = {ControlAxis.PITCH, ControlAxis.YAW, ControlAxis.ROLL};
 
     private final SubstepClock clock;
 
@@ -57,9 +58,22 @@ public final class AeroState implements PhysicsBridge.Publishable {
         // Torque (ship axes) one unit of pitch input on this node's pitch surfaces makes, measured every substep
         final Vector3d pitchResponse = new Vector3d();
         final Vector3d pitchResponseBuilding = new Vector3d();
+        // The same for yaw and roll, measured while cursor aim flies the node
+        final Vector3d yawResponse = new Vector3d();
+        final Vector3d yawResponseBuilding = new Vector3d();
+        final Vector3d rollResponse = new Vector3d();
+        final Vector3d rollResponseBuilding = new Vector3d();
         final AutoTrim.Targets trimTargets = new AutoTrim.Targets();
         volatile double savedTrim;
 
+        Vector3d responseBuilding(ControlAxis axis) {
+            return switch (axis) {
+                case YAW -> yawResponseBuilding;
+                case ROLL -> rollResponseBuilding;
+                default -> pitchResponseBuilding;
+            };
+        }
+
         NodeState(NodeData data) {
             pitch = data.pitch();
             yaw = data.yaw();
@@ -211,6 +225,8 @@ public final class AeroState implements PhysicsBridge.Publishable {
             wingStalled = false;
             for (NodeState state : nodeStates.values()) {
                 state.pitchResponseBuilding.zero();
+                state.yawResponseBuilding.zero();
+                state.rollResponseBuilding.zero();
             }
         }
 
@@ -221,7 +237,7 @@ public final class AeroState implements PhysicsBridge.Publishable {
             r[2] = s.z() - centerOfMass.z();
             NodeData node = s.node() != AeroSurface.NO_NODE ? frame.nodes.get(s.node()) : null;
             NodeState state = node != null ? nodeStates.get(node.key()) : null;
-            double input = s.type().controllable ? input(s, node, state, 0.0) : 0.0;
+            double input = s.type().controllable ? input(s, node, state, ControlAxis.NONE, 0.0) : 0.0;
 
             if (improved) {
                 boolean nowStalled = ImprovedAero.evaluate(s, v, w, r, input, weight, tuning, stalled[i], out);
@@ -233,53 +249,62 @@ public final class AeroState implements PhysicsBridge.Publishable {
                 WariumAero.evaluate(s, v, w, r, input, weight, out);
             }
 
-            // How much a little more pitch input on this node would change the torque, for auto trim
-            if (commit && node != null && node.autoTrim() && s.type().controllable && s.listensToPitch()) {
-                double delta = 0.05;
-                probe.clear();
-                double base = 0.0;
-                if (improved) {
-                    ImprovedAero.evaluate(s, v, w, r, input, weight, tuning, stalled[i], probe);
-                } else {
-                    WariumAero.evaluate(s, v, w, r, input, weight, probe);
+            // How much a little more input on an axis would change the torque: pitch for auto trim, all three for
+            // cursor aim
+            if (commit && node != null && s.type().controllable) {
+                boolean aimed = CursorAimRegistry.isActive(node.key());
+                for (ControlAxis axis : MEASURED_AXES) {
+                    if (!s.listensTo(axis) || !(aimed || (axis == ControlAxis.PITCH && node.autoTrim()))) {
+                        continue;
+                    }
+                    double delta = 0.05;
+                    probe.clear();
+                    if (improved) {
+                        ImprovedAero.evaluate(s, v, w, r, input, weight, tuning, stalled[i], probe);
+                    } else {
+                        WariumAero.evaluate(s, v, w, r, input, weight, probe);
+                    }
+                    double[] before = {probe.totalTorque(0), probe.totalTorque(1), probe.totalTorque(2)};
+                    probe.clear();
+                    double nudged = input(s, node, state, axis, delta);
+                    if (improved) {
+                        ImprovedAero.evaluate(s, v, w, r, nudged, weight, tuning, stalled[i], probe);
+                    } else {
+                        WariumAero.evaluate(s, v, w, r, nudged, weight, probe);
+                    }
+                    state.responseBuilding(axis).add(
+                            (probe.totalTorque(0) - before[0]) / delta,
+                            (probe.totalTorque(1) - before[1]) / delta,
+                            (probe.totalTorque(2) - before[2]) / delta);
                 }
-                double[] before = {probe.totalTorque(0), probe.totalTorque(1), probe.totalTorque(2)};
-                probe.clear();
-                double nudged = input(s, node, state, delta);
-                if (improved) {
-                    ImprovedAero.evaluate(s, v, w, r, nudged, weight, tuning, stalled[i], probe);
-                } else {
-                    WariumAero.evaluate(s, v, w, r, nudged, weight, probe);
-                }
-                state.pitchResponseBuilding.add(
-                        (probe.totalTorque(0) - before[0]) / delta,
-                        (probe.totalTorque(1) - before[1]) / delta,
-                        (probe.totalTorque(2) - before[2]) / delta);
             }
         }
 
         if (commit) {
             for (NodeState state : nodeStates.values()) {
                 state.pitchResponse.set(state.pitchResponseBuilding);
+                state.yawResponse.set(state.yawResponseBuilding);
+                state.rollResponse.set(state.rollResponseBuilding);
             }
         }
     }
 
     /** A control surface's input, combined from its two keys and clamped like Warium does. */
-    private static double input(AeroSurface s, NodeData node, NodeState state, double extraPitch) {
+    private static double input(AeroSurface s, NodeData node, NodeState state, ControlAxis nudged, double extra) {
         if (node == null || state == null) {
             return 0.0;
         }
-        double value = axisValue(s.axis1(), node, state, extraPitch) * s.multiplier1()
-                + axisValue(s.axis2(), node, state, extraPitch) * s.multiplier2();
+        double value = axisValue(s.axis1(), node, state, nudged, extra) * s.multiplier1()
+                + axisValue(s.axis2(), node, state, nudged, extra) * s.multiplier2();
         return Math.max(-1.0, Math.min(1.0, value));
     }
 
-    private static double axisValue(ControlAxis axis, NodeData node, NodeState state, double extraPitch) {
+    private static double axisValue(ControlAxis axis, NodeData node, NodeState state, ControlAxis nudged, double extra) {
+        double nudge = axis == nudged ? extra : 0.0;
         return switch (axis) {
-            case PITCH -> state.pitch + state.trimOutput + extraPitch;
-            case YAW -> state.yaw;
-            case ROLL -> state.roll;
+            case PITCH -> state.pitch + state.trimOutput + nudge;
+            case YAW -> state.yaw + nudge;
+            case ROLL -> state.roll + nudge;
             case THROTTLE -> node.throttle();
             case NONE -> 0.0;
         };
@@ -293,10 +318,12 @@ public final class AeroState implements PhysicsBridge.Publishable {
             if (state == null) {
                 continue;
             }
-            state.pitch = moveTowards(state.pitch, node.pitch(), step);
-            state.yaw = moveTowards(state.yaw, node.yaw(), step);
-            state.roll = moveTowards(state.roll, node.roll(), step);
-            AutoTrim.update(state, node, worldVelocity, altitude, rotation, inertia, w, dt, improved && wingStalled);
+            // Cursor aim, if the pilot has it on, supplies the inputs the pilot is not giving with the keys
+            NodeData effective = CursorPilot.apply(state, node, rotation, inertia, w, worldVelocity.length(), wingStalled);
+            state.pitch = moveTowards(state.pitch, effective.pitch(), step);
+            state.yaw = moveTowards(state.yaw, effective.yaw(), step);
+            state.roll = moveTowards(state.roll, effective.roll(), step);
+            AutoTrim.update(state, effective, worldVelocity, altitude, rotation, inertia, w, dt, improved && wingStalled);
         }
     }
 

src/main/java/dev/lumareth/improvedAerodynamics/flight/AeroSurface.java

@@ -36,6 +36,10 @@ public record AeroSurface(long key, SurfaceType type, boolean slab, double x, do
     }
 
     boolean listensToPitch() {
-        return node != NO_NODE && (axis1 == ControlAxis.PITCH || axis2 == ControlAxis.PITCH);
+        return listensTo(ControlAxis.PITCH);
+    }
+
+    boolean listensTo(ControlAxis axis) {
+        return node != NO_NODE && (axis1 == axis || axis2 == axis);
     }
 }

src/main/java/dev/lumareth/improvedAerodynamics/flight/AimBasis.java

@@ -0,0 +1,101 @@
+package dev.lumareth.improvedAerodynamics.flight;
+
+import net.minecraft.core.BlockPos;
+import net.minecraft.core.Direction;
+import net.minecraft.nbt.CompoundTag;
+import net.minecraft.world.level.Level;
+import net.minecraft.world.level.block.entity.BlockEntity;
+import net.minecraft.world.level.block.state.BlockState;
+import net.minecraft.world.level.block.state.properties.DirectionProperty;
+import net.minecraft.world.level.block.state.properties.Property;
+
+/**
+ * The frame cursor aim steers in, in ship coordinates: forward is the boresight (what Warium's gun reticle points
+ * along, from the HUD block in front of the seat, else the way the seat faces), up is the ship's up axis and right
+ * completes it. Server and client both build it from the same block data, so a cursor sent as components in this
+ * frame means the same on both.
+ *
+ * @param nodePos the Vehicle Control Node the seat is linked to
+ */
+public record AimBasis(BlockPos seatPos, BlockPos nodePos, double[] forward, double[] right, double[] up) {
+
+    /**
+     * @param entityPos block position of the seat entity. Warium's control seat block is at that position or just
+     *                  below it.
+     * @return null if there is no seat block linked to a control node there
+     */
+    public static AimBasis of(Level level, BlockPos entityPos) {
+        for (BlockPos pos : new BlockPos[]{entityPos, entityPos.below()}) {
+            BlockEntity seat = level.getBlockEntity(pos);
+            if (seat == null || !seat.getPersistentData().contains("ControlX")) {
+                continue;
+            }
+            CompoundTag tag = seat.getPersistentData();
+            BlockPos nodePos = BlockPos.containing(tag.getDouble("ControlX"), tag.getDouble("ControlY"), tag.getDouble("ControlZ"));
+            return new AimBasis(pos, nodePos, null, null, null).withAxes(level, pos, nodePos);
+        }
+        return null;
+    }
+
+    private AimBasis withAxes(Level level, BlockPos seatPos, BlockPos nodePos) {
+        BlockState state = level.getBlockState(seatPos);
+        Direction facing = Direction.NORTH;
+        Property<?> property = state.getBlock().getStateDefinition().getProperty("facing");
+        if (property instanceof DirectionProperty directionProperty) {
+            facing = state.getValue(directionProperty);
+        }
+        double fx = facing.getStepX();
+        double fy = facing.getStepY();
+        double fz = facing.getStepZ();
+
+        // Warium's gun reticle: the HUD block in front of the seat holds the aim vector of its guns
+        BlockEntity hud = level.getBlockEntity(seatPos.relative(facing));
+        if (hud != null && hud.getPersistentData().contains("Pitch")) {
+            CompoundTag hudTag = hud.getPersistentData();
+            double bx = hudTag.getDouble("X") + facing.getStepX();
+            double by = hudTag.getDouble("Pitch");
+            double bz = hudTag.getDouble("Z") + facing.getStepZ();
+            if (bx * bx + by * by + bz * bz > 1.0E-6) {
+                fx = bx;
+                fy = by;
+                fz = bz;
+            }
+        }
+        double length = Math.sqrt(fx * fx + fy * fy + fz * fz);
+        if (length < 1.0E-6) {
+            fx = 0.0;
+            fy = 0.0;
+            fz = -1.0;
+            length = 1.0;
+        }
+        fx /= length;
+        fy /= length;
+        fz /= length;
+
+        // right = forward x up(0,1,0)
+        double rx = -fz;
+        double ry = 0.0;
+        double rz = fx;
+        double rl = Math.sqrt(rx * rx + rz * rz);
+        if (rl < 1.0E-6) {
+            rx = 1.0;
+            rz = 0.0;
+            rl = 1.0;
+        }
+        rx /= rl;
+        rz /= rl;
+        // up = right x forward
+        double ux = ry * fz - rz * fy;
+        double uy = rz * fx - rx * fz;
+        double uz = rx * fy - ry * fx;
+        return new AimBasis(seatPos, nodePos, new double[]{fx, fy, fz}, new double[]{rx, ry, rz}, new double[]{ux, uy, uz});
+    }
+
+    /** Ship-space direction of a cursor given by its components along forward, right and up. */
+    public double[] toShip(double cf, double cr, double cu) {
+        return new double[]{
+                forward[0] * cf + right[0] * cr + up[0] * cu,
+                forward[1] * cf + right[1] * cr + up[1] * cu,
+                forward[2] * cf + right[2] * cr + up[2] * cu};
+    }
+}

src/main/java/dev/lumareth/improvedAerodynamics/flight/CursorAimConfig.java

@@ -0,0 +1,52 @@
+package dev.lumareth.improvedAerodynamics.flight;
+
+import net.minecraftforge.common.ForgeConfigSpec;
+
+/** Server side settings of the cursor aim virtual pilot. Lives in improved_aerodynamics-cursor-aim.toml. */
+public final class CursorAimConfig {
+    public static final ForgeConfigSpec SPEC;
+
+    public static final ForgeConfigSpec.BooleanValue ENABLED;
+    public static final ForgeConfigSpec.DoubleValue FULL_DEFLECTION_DEGREES;
+    public static final ForgeConfigSpec.DoubleValue MAX_TURN_RATE_DEGREES;
+    public static final ForgeConfigSpec.DoubleValue MAX_ROLL_RATE_DEGREES;
+    public static final ForgeConfigSpec.DoubleValue MAX_BANK_DEGREES;
+    public static final ForgeConfigSpec.DoubleValue BANK_GAIN;
+    public static final ForgeConfigSpec.DoubleValue RATE_GAIN;
+    public static final ForgeConfigSpec.DoubleValue PUSH_FACTOR;
+    public static final ForgeConfigSpec.DoubleValue STALLED_PITCH_FACTOR;
+    public static final ForgeConfigSpec.DoubleValue MIN_SPEED;
+
+    static {
+        ForgeConfigSpec.Builder builder = new ForgeConfigSpec.Builder();
+        builder.comment("Cursor aim: the mouse moves a cursor on screen, a virtual pilot flies the control node's craft by it").push("cursor_aim");
+        ENABLED = builder.comment("Whether pilots may use cursor aim on this server")
+                .define("enabled", true);
+        FULL_DEFLECTION_DEGREES = builder.comment("Angle of the cursor from the gun reticle at which the virtual pilot asks for full deflection: the maximum bank",
+                        "sideways, the full pitch rate up or down")
+                .defineInRange("fullDeflectionDegrees", 30.0, 5.0, 90.0);
+        MAX_TURN_RATE_DEGREES = builder.comment("Turn rate in degrees per second the pilot asks for at full deflection, both for the turn at maximum bank and for pitching")
+                .defineInRange("maxTurnRateDegrees", 40.0, 5.0, 360.0);
+        MAX_ROLL_RATE_DEGREES = builder.comment("Fastest roll in degrees per second the pilot asks for when it rolls to a bank angle")
+                .defineInRange("maxRollRateDegrees", 150.0, 10.0, 720.0);
+        MAX_BANK_DEGREES = builder.comment("Bank angle the pilot rolls to when the cursor is at full sideways deflection")
+                .defineInRange("maxBankDegrees", 70.0, 10.0, 90.0);
+        BANK_GAIN = builder.comment("Roll rate in rad/s per radian of bank angle error, how eagerly the pilot rolls to the wanted bank and levels the wings")
+                .defineInRange("bankGain", 3.0, 0.2, 20.0);
+        RATE_GAIN = builder.comment("How hard the pilot corrects its turn rate, in angular acceleration per rad/s of rate error. Lower is smoother,",
+                        "higher follows the cursor tighter and can start to oscillate")
+                .defineInRange("rateGain", 5.0, 0.5, 40.0);
+        PUSH_FACTOR = builder.comment("A cursor below the gun reticle pushes the nose down. This is how much of the pull rate that may be, since",
+                        "negative g is harder on the craft and the pilot")
+                .defineInRange("pushFactor", 0.4, 0.0, 1.0);
+        STALLED_PITCH_FACTOR = builder.comment("Multiplier for the pitch rate the pilot asks for while the wings are stalled, so it does not keep pulling")
+                .defineInRange("stalledPitchFactor", 0.3, 0.0, 1.0);
+        MIN_SPEED = builder.comment("Below this speed in blocks per second the pilot does nothing, the surfaces have no effect anyway")
+                .defineInRange("minSpeed", 15.0, 0.0, 200.0);
+        builder.pop();
+        SPEC = builder.build();
+    }
+
+    private CursorAimConfig() {
+    }
+}

src/main/java/dev/lumareth/improvedAerodynamics/flight/CursorAimRegistry.java

@@ -0,0 +1,44 @@
+package dev.lumareth.improvedAerodynamics.flight;
+
+import java.util.concurrent.ConcurrentHashMap;
+
+/**
+ * Where each control node's pilot currently holds the cursor of cursor aim. Written by the server thread when the
+ * client's packet arrives, read by the physics thread. An entry that is not refreshed for a moment (the player left
+ * the seat, lost the connection) counts as gone.
+ */
+public final class CursorAimRegistry {
+    private static final long EXPIRE_NANOS = 500_000_000L;
+    private static final ConcurrentHashMap<Long, Aim> AIMS = new ConcurrentHashMap<>();
+
+    /** Cursor as components along the basis axes (unit length), and the basis itself, in ship space. */
+    public record Aim(double cf, double cr, double cu, double[] forward, double[] right, double[] up, long nanos) {
+    }
+
+    private CursorAimRegistry() {
+    }
+
+    public static void set(long nodeKey, Aim aim) {
+        AIMS.put(nodeKey, aim);
+    }
+
+    public static void clear(long nodeKey) {
+        AIMS.remove(nodeKey);
+    }
+
+    public static Aim get(long nodeKey) {
+        Aim aim = AIMS.get(nodeKey);
+        if (aim == null) {
+            return null;
+        }
+        if (System.nanoTime() - aim.nanos() > EXPIRE_NANOS) {
+            AIMS.remove(nodeKey, aim);
+            return null;
+        }
+        return aim;
+    }
+
+    public static boolean isActive(long nodeKey) {
+        return get(nodeKey) != null;
+    }
+}

src/main/java/dev/lumareth/improvedAerodynamics/flight/CursorAimServer.java

@@ -0,0 +1,30 @@
+package dev.lumareth.improvedAerodynamics.flight;
+
+import net.mcreator.valkyrienwarium.entity.SeatEntityEntity;
+import net.minecraft.server.level.ServerPlayer;
+import net.minecraft.world.entity.Entity;
+
+/** Server side of cursor aim: takes the cursor a pilot sends and hands it to the control node's virtual pilot. */
+public final class CursorAimServer {
+    private CursorAimServer() {
+    }
+
+    public static void accept(ServerPlayer player, boolean active, double cf, double cr, double cu) {
+        Entity vehicle = player.getVehicle();
+        if (!(vehicle instanceof SeatEntityEntity)) {
+            return;
+        }
+        AimBasis basis = AimBasis.of(vehicle.level(), vehicle.blockPosition());
+        if (basis == null) {
+            return;
+        }
+        long nodeKey = basis.nodePos().asLong();
+        double length = Math.sqrt(cf * cf + cr * cr + cu * cu);
+        if (!active || !CursorAimConfig.ENABLED.get() || !(length > 1.0E-6) || !Double.isFinite(length)) {
+            CursorAimRegistry.clear(nodeKey);
+            return;
+        }
+        CursorAimRegistry.set(nodeKey, new CursorAimRegistry.Aim(cf / length, cr / length, cu / length,
+                basis.forward(), basis.right(), basis.up(), System.nanoTime()));
+    }
+}

src/main/java/dev/lumareth/improvedAerodynamics/flight/CursorPilot.java

@@ -0,0 +1,136 @@
+package dev.lumareth.improvedAerodynamics.flight;
+
+import org.joml.Matrix3d;
+import org.joml.Matrix3dc;
+import org.joml.Quaterniondc;
+import org.joml.Vector3d;
+
+/**
+ * The virtual pilot of cursor aim. The cursor is a stick that springs back to the gun reticle:
+ * <ul>
+ *   <li>sideways it commands a bank angle and turn: the further out, the steeper the bank and the harder the pull
+ *   that comes with it. Centered it levels the wings, so a centered cursor is straight flight;</li>
+ *   <li>up and down it commands a pitch rate;</li>
+ *   <li>close to the reticle it steers with yaw and pitch directly instead of banking, so aiming does not rock the
+ *   wings.</li>
+ * </ul>
+ * The wanted rotation is turned into stick inputs by the measured effect of each input on the craft (torque per unit
+ * of input, see {@link AeroState}), so it works for any craft however its surfaces are mixed or oriented: angular
+ * acceleration wanted times inertia, solved for the three inputs with a little regularization for axes that have no
+ * surfaces. Keys the pilot holds win over it, per axis.
+ */
+final class CursorPilot {
+    private static final double FINE_START = Math.toRadians(4.0);
+    private static final double FINE_END = Math.toRadians(16.0);
+    private static final double MANUAL_OVERRIDE = 0.05;
+    private static final double MAX_PULL_BANK = Math.toRadians(90.0);
+
+    private CursorPilot() {
+    }
+
+    /**
+     * @return the node with the pilot's wanted inputs in place of the keys, or the node itself if cursor aim is off
+     */
+    static NodeData apply(AeroState.NodeState state, NodeData node, Quaterniondc rotation, Matrix3dc inertia, double[] w,
+                          double speed, boolean wingStalled) {
+        if (!CursorAimConfig.ENABLED.get()) {
+            return node;
+        }
+        CursorAimRegistry.Aim aim = CursorAimRegistry.get(node.key());
+        if (aim == null || speed < CursorAimConfig.MIN_SPEED.get()) {
+            return node;
+        }
+
+        double fullAngle = Math.toRadians(CursorAimConfig.FULL_DEFLECTION_DEGREES.get());
+        double maxTurn = Math.toRadians(CursorAimConfig.MAX_TURN_RATE_DEGREES.get());
+        double maxRoll = Math.toRadians(CursorAimConfig.MAX_ROLL_RATE_DEGREES.get());
+        double maxBank = Math.toRadians(CursorAimConfig.MAX_BANK_DEGREES.get());
+
+        double[] f = aim.forward();
+        double[] r = aim.right();
+        double[] u = aim.up();
+
+        // Cursor as angles from the reticle: sideways (positive right) and up, both through any 270 degree cone
+        double sideways = Math.atan2(aim.cr(), aim.cf());
+        double vertical = Math.atan2(aim.cu(), Math.hypot(aim.cf(), aim.cr()));
+        double lateral = saturate(sideways / fullAngle);
+        double blend = smoothstep(Math.abs(sideways), FINE_START, FINE_END);
+
+        // Bank of the craft: positive when the right wing is down, through upside down
+        Vector3d rightWorld = rotation.transform(new Vector3d(r[0], r[1], r[2]));
+        Vector3d upWorld = rotation.transform(new Vector3d(u[0], u[1], u[2]));
+        double bank = Math.atan2(-rightWorld.y, upWorld.y);
+
+        double bankCommand = maxBank * lateral * blend;
+        double rollRate = clamp(CursorAimConfig.BANK_GAIN.get() * (bankCommand - bank), -maxRoll, maxRoll);
+
+        // Pitch: the stick, plus the pull that carries the turn while banked
+        double pitchStick = saturate(vertical / fullAngle);
+        double pitchRate = maxTurn * (pitchStick >= 0.0 ? pitchStick : pitchStick * CursorAimConfig.PUSH_FACTOR.get());
+        if (Math.abs(bank) < MAX_PULL_BANK) {
+            pitchRate += maxTurn * Math.abs(lateral) * blend * Math.sin(Math.abs(bank));
+        }
+        if (wingStalled && pitchRate > 0.0) {
+            pitchRate *= CursorAimConfig.STALLED_PITCH_FACTOR.get();
+        }
+        // Close to the reticle the nose follows the cursor with the rudder
+        double yawRate = maxTurn * lateral * (1.0 - blend);
+
+        // Wanted angular velocity in ship axes: roll about forward, pitch about right, yaw (nose right) about -up
+        Vector3d wanted = new Vector3d(
+                rollRate * f[0] + pitchRate * r[0] - yawRate * u[0],
+                rollRate * f[1] + pitchRate * r[1] - yawRate * u[1],
+                rollRate * f[2] + pitchRate * r[2] - yawRate * u[2]);
+        Vector3d error = wanted.sub(w[0], w[1], w[2]);
+        Vector3d acceleration = error.mul(CursorAimConfig.RATE_GAIN.get());
+        Vector3d torque = inertia.transform(acceleration, new Vector3d());
+
+        double[] inputs = solve(state.pitchResponse, state.yawResponse, state.rollResponse, torque);
+        if (inputs == null) {
+            return node;
+        }
+
+        double pitch = clamp(inputs[0] - state.trimOutput, -1.0, 1.0);
+        double yaw = clamp(inputs[1], -1.0, 1.0);
+        double roll = clamp(inputs[2], -1.0, 1.0);
+        return new NodeData(node.key(),
+                Math.abs(node.pitch()) > MANUAL_OVERRIDE ? node.pitch() : pitch,
+                Math.abs(node.yaw()) > MANUAL_OVERRIDE ? node.yaw() : yaw,
+                Math.abs(node.roll()) > MANUAL_OVERRIDE ? node.roll() : roll,
+                node.throttle(), node.autoTrim(), node.storedTrim());
+    }
+
+    /** Least squares for the three inputs whose responses add up to the torque, ridge-regularized so unused axes stay at zero. */
+    private static double[] solve(Vector3d pitch, Vector3d yaw, Vector3d roll, Vector3d torque) {
+        Matrix3d responses = new Matrix3d(pitch, yaw, roll);
+        Matrix3d transposed = responses.transpose(new Matrix3d());
+        Matrix3d normal = transposed.mul(responses, new Matrix3d());
+        double trace = normal.m00() + normal.m11() + normal.m22();
+        if (!(trace > 1.0E-9) || !Double.isFinite(trace)) {
+            return null;
+        }
+        double ridge = trace / 3.0 * 1.0E-3;
+        normal.m00(normal.m00() + ridge);
+        normal.m11(normal.m11() + ridge);
+        normal.m22(normal.m22() + ridge);
+        Vector3d right = transposed.transform(torque, new Vector3d());
+        Vector3d x = normal.invert().transform(right);
+        if (!x.isFinite()) {
+            return null;
+        }
+        return new double[]{x.x, x.y, x.z};
+    }
+
+    private static double saturate(double value) {
+        return clamp(value, -1.0, 1.0);
+    }
+
+    private static double smoothstep(double value, double from, double to) {
+        double t = clamp((value - from) / (to - from), 0.0, 1.0);
+        return t * t * (3.0 - 2.0 * t);
+    }
+
+    private static double clamp(double value, double min, double max) {
+        return Math.max(min, Math.min(max, value));
+    }
+}