VS--Improved-Aerodynamics/src/main/java/dev/lumareth/improvedAerodynamics/flight/ImprovedAero.java

package dev.lumareth.improvedAerodynamics.flight;

import dev.lumareth.improvedAerodynamics.Config;

/**
 * Flight model based on each surface's angle of attack, used instead of {@link WariumAero} when enabled.
 * <p>
 * Every block is treated as one unit of wing area that sees the airflow at its own position, including the part
 * caused by the ship's rotation. Lift is {@code K * pressure * airspeed² * CL(angle of attack)}: slow, heavy or
 * high-g flight needs a higher angle of attack for the same lift until a surface reaches its critical angle and
 * stalls. Stalls are per surface, soft (lift fades over a few degrees into flat-plate behaviour) and sticky (a
 * stalled surface only recovers a few degrees below its critical angle). Rotational damping is not added
 * separately, it comes from the surfaces' changing angle of attack while rotating, so it vanishes for stalled
 * surfaces and thrust vectoring can hold the nose past the stall.
 * <p>
 * K is chosen so that a level wing at the configured design speed makes exactly Warium's lift.
 */
final class ImprovedAero {
    // Lift slope of a finite wing per radian, about 80% of thin airfoil theory's 2 pi
    private static final double LIFT_SLOPE = 2.0 * Math.PI * 0.8;
    private static final double INDUCED_DRAG = 0.05;
    // Fully separated flow behaves like a flat plate
    private static final double SEPARATED_LIFT = 1.1;
    private static final double SEPARATED_DRAG = 1.8;
    private static final double STALL_BLEND = Math.toRadians(2.0);
    private static final double STALL_HYSTERESIS = Math.toRadians(3.0);
    private static final double MAX_DEFLECTION = Math.toRadians(20.0);
    // Share of the deflection that acts as angle of attack, gives Warium's control authority at the design speed
    private static final double FLAP_EFFECTIVENESS = 0.75;
    // How much of the lift a flap adds raises the stall angle, and the parasitic drag coefficient per radian² of flap
    // Share of the flap's deflection that acts as angle of attack: 35 degrees of landing flap adds about 1.1 to the
    // lift coefficient, like a real plain or slotted flap
    private static final double FLAP_ALPHA_FACTOR = 0.4;
    private static final double FLAP_STALL_GAIN = 0.7;
    private static final double FLAP_DRAG = 0.35;
    // Angle of attack of a level cambered wing, where K is calibrated
    private static final double REFERENCE_ANGLE = Math.toRadians(4.0);
    private static final double REFERENCE_SPEED = 40.0;
    // Oswald efficiency of a finite wing
    private static final double EFFICIENCY = 0.85;

    private ImprovedAero() {
    }

    /**
     * Lift constant K, fixed for every craft: a level wing of average proportions makes Warium's lift at 40 blocks per
     * second. Faster flight needs less angle of attack, slower flight more, and how fast a craft has to fly depends
     * only on its weight, wing area, wing shape and the air pressure at its altitude.
     */
    static final double LIFT_CONSTANT = 160.0 / (REFERENCE_SPEED * LIFT_SLOPE * REFERENCE_ANGLE);

    /** The config values the model reads, taken once per evaluation. */
    record Tuning(double liftScale, double stallAngleScale, double thinDrag,
                  boolean transonic, double speedOfSound, double transonicPeakDrag, double supersonicDrag,
                  boolean vortex, double vortexStallBonus, double vortexLiftStrength) {
        static Tuning read() {
            return new Tuning(Config.LIFT_SCALE.get(), Config.STALL_ANGLE_SCALE.get(), Config.STABILIZER_DRAG.get(),
                    Config.TRANSONIC_DRAG.get(), Config.SPEED_OF_SOUND.get(), Config.TRANSONIC_PEAK_DRAG.get(),
                    Config.SUPERSONIC_DRAG.get(),
                    Config.VORTEX_LIFT.get(), Config.VORTEX_STALL_BONUS.get(), Config.VORTEX_LIFT_STRENGTH.get());
        }
    }

    /**
     * Adds the surface's forces to out.
     *
     * @param v       ship velocity, ship axes
     * @param w       ship angular velocity, ship axes
     * @param r       block center relative to the center of mass, ship axes
     * @param input   control input of the surface, -1 to 1
     * @param flap    flap deflection in radians, 0 for surfaces that are not flaps
     * @param stalled whether the surface was stalled in the previous substep
     * @return whether the surface is stalled now
     */
    static boolean evaluate(AeroSurface s, double[] v, double[] w, double[] r, double input, double flap, double weight,
                            Tuning tuning, boolean stalled, Wrench out) {
        // Airflow at the block: ship velocity plus w x r
        double vx = v[0] + w[1] * r[2] - w[2] * r[1];
        double vy = v[1] + w[2] * r[0] - w[0] * r[2];
        double vz = v[2] + w[0] * r[1] - w[1] * r[0];
        double localSpeed = Math.sqrt(vx * vx + vy * vy + vz * vz);
        double p = s.pressure() * weight;
        double m = s.speedMultiplier();
        double fd = s.fluidFactor();

        if (s.type() == SurfaceType.AIR_BRAKE) {
            double ds = -(s.facingX() * vx + s.facingZ() * vz);
            double brake = -Math.abs(ds * 100.0 * s.brakeInput() * p * fd);
            out.addForce(r, vx * brake, vy * brake, vz * brake);
            return false;
        }

        // Warium's own per-block drag, lower for thin surfaces since they stabilize with lift, not drag. Shock waves
        // multiply it near the speed of sound; this is the only zero-lift drag a craft has, induced and stall drag
        // below are not affected
        double shockDrag = tuning.transonic()
                ? TransonicDrag.multiplier(localSpeed / tuning.speedOfSound(), tuning.transonicPeakDrag(), tuning.supersonicDrag())
                : 1.0;
        double parasitic = -localSpeed * (s.type().thin ? tuning.thinDrag() : s.type().parasiticDrag) * shockDrag * p / m;
        out.addForce(r, vx * parasitic, vy * parasitic, vz * parasitic);

        // Chord c along the facing, normal n up for horizontal surfaces and to the side for vertical ones
        double cx = s.facingX();
        double cz = s.facingZ();
        double nx = s.type().vertical ? s.rightX() : 0.0;
        double ny = s.type().vertical ? 0.0 : 1.0;
        double nz = s.type().vertical ? s.rightZ() : 0.0;
        double u = vx * cx + vz * cz;
        double vn = vx * nx + vy * ny + vz * nz;
        double planeSpeedSq = u * u + vn * vn;
        if (planeSpeedSq < 1.0E-4) {
            return stalled;
        }
        double planeSpeed = Math.sqrt(planeSpeedSq);

        // Moving down through the air (vn < 0) means air hits the underside: positive angle of attack.
        // Surfaces work the same with air coming from either end of the chord, like in Warium
        double alpha = Math.atan2(-vn, Math.abs(u));
        alpha += Math.toRadians(s.type().camber);
        if (s.type().controllable) {
            // Same sign convention as Warium: positive input pushes along n while moving against the facing
            alpha += FLAP_EFFECTIVENESS * MAX_DEFLECTION * input * (u <= 0.0 ? 1.0 : -1.0);
        }
        // A flap bends the airfoil's camber whichever way the air comes, so it always adds lift, and it moves the stall
        // up with it: a flap raises the highest lift a wing can make
        double flapShift = FLAP_ALPHA_FACTOR * flap;
        alpha += flapShift;
        alpha = Math.max(-Math.PI / 2.0, Math.min(Math.PI / 2.0, alpha));

        // Low aspect ratio wings keep their flow attached longer thanks to leading-edge vortices. Control surfaces
        // in a wing's group stall with the wing instead of before it
        double vortexFactor = tuning.vortex() && (s.type() == SurfaceType.WING || s.wingGroup() && s.type().controllable)
                ? VortexLift.factor(s.aspectRatio()) : 0.0;
        double baseStall = s.slab() ? SurfaceType.SLAB_STALL_ANGLE
                : s.wingGroup() && s.type().controllable ? SurfaceType.WING.stallAngle : s.type().stallAngle;
        double critical = Math.toRadians(baseStall) * (1.0 + tuning.vortexStallBonus() * vortexFactor) * tuning.stallAngleScale();
        double absAlpha = Math.abs(alpha);
        if (alpha >= 0.0) {
            critical += FLAP_STALL_GAIN * flapShift;
        }
        // Flips only where both blends agree, so the hysteresis never makes the force jump
        if (!stalled && absAlpha > critical + STALL_BLEND) {
            stalled = true;
        } else if (stalled && absAlpha < critical - STALL_HYSTERESIS - STALL_BLEND) {
            stalled = false;
        }
        double center = stalled ? critical - STALL_HYSTERESIS : critical;
        double attached = 1.0 - smoothstep(center - STALL_BLEND, center + STALL_BLEND, absAlpha);

        // Finite wing theory: lift slope and induced drag follow from the wing's aspect ratio
        double aspectRatio = s.aspectRatio();
        double slope = aspectRatio > 0.0 ? 2.0 * Math.PI / (1.0 + 2.0 / aspectRatio) : LIFT_SLOPE;
        double inducedDrag = aspectRatio > 0.0 ? 1.0 / (Math.PI * EFFICIENCY * aspectRatio) : INDUCED_DRAG;
        double attachedLift = slope * alpha;
        if (s.type() == SurfaceType.WING) {
            attachedLift += VortexLift.liftCoefficient(alpha, vortexFactor, tuning.vortexLiftStrength());
        }
        double lift = attached * attachedLift + (1.0 - attached) * SEPARATED_LIFT * Math.sin(2.0 * alpha);
        double drag = attached * inducedDrag * attachedLift * attachedLift
                + (1.0 - attached) * SEPARATED_DRAG * Math.sin(alpha) * Math.sin(alpha);

        double q = LIFT_CONSTANT * p * fd * planeSpeedSq;
        // Lift is perpendicular to the airflow in the chord/normal plane, on the side of n
        double side = u < 0.0 ? -1.0 : 1.0;
        double lx = (Math.abs(u) * nx - side * vn * cx) / planeSpeed;
        double ly = Math.abs(u) * ny / planeSpeed;
        double lz = (Math.abs(u) * nz - side * vn * cz) / planeSpeed;
        // Drag opposes the airflow in that plane
        double dx = -(u * cx + vn * nx) / planeSpeed;
        double dy = -(vn * ny) / planeSpeed;
        double dz = -(u * cz + vn * nz) / planeSpeed;
        double liftForce = q * lift * tuning.liftScale();
        double dragForce = q * (drag + FLAP_DRAG * flap * flap) / m;
        out.addForce(r, lx * liftForce + dx * dragForce, ly * liftForce + dy * dragForce, lz * liftForce + dz * dragForce);
        return stalled;
    }

    static double smoothstep(double edge0, double edge1, double x) {
        double t = Math.max(0.0, Math.min(1.0, (x - edge0) / (edge1 - edge0)));
        return t * t * (3.0 - 2.0 * t);
    }
}