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

package dev.lumareth.improvedAerodynamics.flight;

/**
 * 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;
    // Angle of attack of a level cambered wing, where K is calibrated
    private static final double REFERENCE_ANGLE = Math.toRadians(4.0);

    private ImprovedAero() {
    }

    /** Lift constant K for the given design speed. */
    static double liftConstant(double designSpeed) {
        return 160.0 / (designSpeed * LIFT_SLOPE * REFERENCE_ANGLE);
    }

    /**
     * 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 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 weight,
                            double liftConstant, double stallAngleScale, 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
        double parasitic = -localSpeed * s.type().parasiticDrag * 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);
        }
        alpha = Math.max(-Math.PI / 2.0, Math.min(Math.PI / 2.0, alpha));

        double critical = Math.toRadians(s.slab() ? SurfaceType.SLAB_STALL_ANGLE : s.type().stallAngle) * stallAngleScale;
        double absAlpha = Math.abs(alpha);
        // 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);

        double attachedLift = LIFT_SLOPE * alpha;
        double lift = attached * attachedLift + (1.0 - attached) * SEPARATED_LIFT * Math.sin(2.0 * alpha);
        double drag = attached * INDUCED_DRAG * attachedLift * attachedLift
                + (1.0 - attached) * SEPARATED_DRAG * Math.sin(alpha) * Math.sin(alpha);

        double q = liftConstant * 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;
        double dragForce = q * drag / m;
        out.addForce(r, lx * liftForce + dx * dragForce, ly * liftForce + dy * dragForce, lz * liftForce + dz * dragForce);
        return stalled;
    }

    private 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);
    }
}