Hypersonic Boost-Glide 6-DOF (dim=13)

def _atmosphere_density(altitude):
    """Exponential atmosphere model."""
    h = max(altitude, 0.0)
    return _RHO0 * np.exp(-h / _H_SCALE)

def _gravity_accel(pos):
    """Spherical gravity: returns acceleration vector (toward center)."""
    r = np.linalg.norm(pos)
    r = max(r, _R_EARTH * 0.5)
    g_mag = _MU_EARTH / (r * r)
    return -g_mag * pos / r

def _mach_number(speed, altitude):
    """Approximate Mach from speed; sound speed decreases with altitude."""
    # Simplified: assume isothermal atmosphere for sound speed
    h = max(altitude, 0.0)
    a = _SPEED_OF_SOUND_SL * np.exp(-h / (2.0 * 42000.0))
    return speed / max(a, 1.0)

def hypersonic_boost_glide_6dof_rhs(t, y):
    pos = y[0:3]
    vel = y[3:6]
    angles = y[6:9]       # roll, pitch, yaw
    omega = y[9:12]       # angular rates
    mass = max(y[12], 100.0)

    speed = np.linalg.norm(vel)
    speed = max(speed, 1e-6)
    v_hat = vel / speed

    altitude = np.linalg.norm(pos) - _R_EARTH
    rho = _atmosphere_density(altitude)
    mach = _mach_number(speed, altitude)

    cd = np.interp(mach, _MACH_TABLE, _CD_TABLE)
    cl = np.interp(mach, _MACH_TABLE, _CL_TABLE)

    q_dyn = 0.5 * rho * speed * speed
    drag = q_dyn * cd * _S_REF
    lift = q_dyn * cl * _S_REF

    # Drag opposes velocity; lift perpendicular (simplified: radially outward)
    f_drag = -drag * v_hat

    r_hat = pos / max(np.linalg.norm(pos), 1.0)
    # Lift direction: component of radial direction perpendicular to velocity
    lift_dir = r_hat - np.dot(r_hat, v_hat) * v_hat
    lift_norm = np.linalg.norm(lift_dir)
    if lift_norm > 1e-10:
        lift_dir = lift_dir / lift_norm
    else:
        lift_dir = np.zeros(3)
    f_lift = lift * lift_dir

    g_accel = _gravity_accel(pos)

    # Thrust along velocity direction during boost phase
    if t < _T_BURN and mass > _M0 - _MDOT * _T_BURN:
        f_thrust = _THRUST * v_hat
        dm_dt = -_MDOT
    else:
        f_thrust = np.zeros(3)
        dm_dt = 0.0

    accel = (f_drag + f_lift + f_thrust) / mass + g_accel

    # Simplified Euler rotational dynamics with restoring moments
    # toward zero angles and linear damping
    torque = np.array([
        (-_K_RESTORE * angles[0] - _C_DAMP * omega[0]) / _I_XX,
        (-_K_RESTORE * angles[1] - _C_DAMP * omega[1]) / _I_YY,
        (-_K_RESTORE * angles[2] - _C_DAMP * omega[2]) / _I_ZZ,
    ])

    dy = np.empty(13)
    dy[0:3] = vel
    dy[3:6] = accel
    dy[6:9] = omega
    dy[9:12] = torque
    dy[12] = dm_dt
    return dy