Untitled

import numpy as np
from numpy.linalg import inv
# //////////////////////////////////////////////ИСПОЛЬЗУЕМЫЕ ФУНКЦИИ////////////////////////////////////////////////////
# Начальные данные для атмосферы
R_Z = 6356767.0
R = 287.05287
gc = 9.8065
k = 1.4
# Сама атмосфера
def atmos(Height):
    h = Height
    def H(h):  # геопотенциальная высота
        return ((R_Z * h) / (R_Z + h))
    def g(h):  # ускорение свободного падения
        return (gc * pow(R_Z / (R_Z + h), 2.0))
    def T(h):  # абсолютная температура
        h_border = [-2000.0, 0.0, 11000.0, 20000.0, 32000.0, 47000.0,
                    51000.0, 71000.0, 85000.0, 94000.0, 102450.0, 117777.0]
        T_border = [301.15, 288.15, 216.65, 216.65, 228.65,
                    270.65, 270.65, 214.65, 186.65, 186.65, 212.0]
        betta = [-0.0065, -0.0065, 0.0, 0.001, 0.0028,
                 0.0, -0.0028, -0.002, 0.0, 0.003, 0.011]
        H = R_Z * h / (R_Z + h)
        for i in range(11):
            if (H > h_border[i] - 1e-6) and (H < h_border[i + 1]):
                return (T_border[i] + betta[i] * (H - h_border[i]))
            elif (abs(H - h_border[11]) < 1e-6):
                return (T_border[10] + betta[10] * (H - h_border[11]))
    def p(h):  # давление
        h_border = [-2000.0, 0.0, 11000.0, 20000.0, 32000.0, 47000.0,
                    51000.0, 71000.0, 85000.0, 94000.0, 102450.0, 117777.0]
        T_border = [301.15, 288.15, 216.65, 216.65, 228.65,
                    270.65, 270.65, 214.65, 186.65, 186.65, 212.0]
        betta = [-0.0065, -0.0065, 0.0, 0.001, 0.0028,
                 0.0, -0.0028, -0.002, 0.0, 0.003, 0.011]
        p_border = [127773.0, 101325.0, 22632.0401, 5474.886, 868.017,
                    110.906, 66.939, 3.959, 0.36341194, 0.07025889, 0.016438325]
        H = R_Z * h / (R_Z + h)
        for i in range(11):
            if ((H > h_border[i] - 1e-6) and (H < h_border[i + 1])):
                if (abs(betta[i]) < 1e-6):
                    return (p_border[i] * pow(10.0, -gc * 0.434294 * (H - h_border[i]) / (R * T(h))))
                else:
                    return (p_border[i] * pow(T(h) / T_border[i], -gc / (R * betta[i])))
            elif (abs(H - h_border[11]) < 1e-6):
                return (p_border[10] * pow(T(h) / T_border[10], -gc / (R * betta[10])))        
    def pl(h): # плостность
        R = 8314.32
        Mol = 28.964420
        return (p(h) * Mol / (R * T(h))) 
    def a(h):  # скорость звука
        return (20.046796 * np.sqrt(T(h)))
    parametr = {'T': T(h), 'p': p(h), 'pl': pl(h), 'a': a(h), 'H': H(h)}
    return parametr

# //////////////////////////////////////////////НАЧАЛЬНЫЕ ДАННЫЕ////////////////////////////////////////////////////////
N = 11                                              # номер варианта ДЗ
m = 400                                             # масса ЛА, кг
dm = 1                                              # диаметр миделева сечения, м
L = 1                                               # характерная длина, м
S = np.pi * dm**2 / 4                               # площадь миделева сечения
t = 0
dt = 0.0001

METOD = int(input("Введите какой метод: "))
SHEMA = int(input("Введите какая схема: "))

vx0 = 100 + 20 * N     # начальная скорость по оси x
vy0 = 0      # начальная скорость по оси y
vz0 = 0                # начальная скорость по оси z

wx0 = 0                # начальная угловая скорость по оси x
wy0 = 0                # начальная угловая скорость по оси y
wz0 = 0                # начальная угловая скорость по оси z
                       
x0 = 0                 # координата x0
y0 = 5000 + 100 * N    # координата y0
z0 = 0                 # координата z0

kren0 = 0              # начальный угол крена, град
rysk0 = 45 - N         # начальный угол рысканья, град
tang0 = - 80 + N       # начальный угол тангажа, град
# print ('\nкрен =', np.round(kren0,5), '\nрыскание =', np.round(rysk0,5),'\nтангаж =', np.round(tang0,5))
# Перевод из градусов в радианы
kr = np.radians(kren0) #рад
ry = np.radians(rysk0) #рад
tn = np.radians(tang0) #рад

w = np.array([0, 0, 0])                             # вектор угловой скорост
I = np.array([[10, 0, 0], [0, 10, 0], [0, 0, 10]])  #тензор инерции

atm = atmos(y0)
ro = atm['pl']

x = x0
y = y0
z = z0
Vx = vx0
Vy = vy0
Vz = vz0
Wx = wx0
Wy = wy0
Wz = wz0
ϑ = tn
ψ = ry
γ = kr

def findA(ϑ, ψ, γ):
    A11 = np.cos(ϑ) * np.cos(ψ)
    A12 = np.sin(ϑ)
    A13 = -np.cos(ϑ) * np.sin(ψ)
    A21 = -np.sin(ϑ) * np.cos(ψ) * np.cos(γ) + np.sin(ψ) * np.sin(γ)
    A22 = np.cos(ϑ) * np.cos(γ)
    A23 = np.cos(ψ) * np.sin(γ) + np.sin(ϑ) * np.sin(ψ) * np.cos(γ)
    A31 = np.sin(ϑ) * np.cos(ψ) * np.sin(γ) + np.sin(ψ) * np.cos(γ)
    A32 = -np.cos(ϑ) * np.sin(γ)
    A33 = np.cos(ψ) * np.cos(γ) - np.sin(ϑ) * np.sin(ψ) * np.sin(γ)
    A = np.array([[A11, A12, A13], [A21, A22, A23], [A31, A32, A33]])
    return A

def dvdt (Vx, Vy, Vz, Wx, Wy, Wz, ϑ, ψ, γ):

    V = np.sqrt(Vx**2 + Vy**2 + Vz**2)
    
    ap = np.arccos(Vx/V)
    phi = np.pi/2
    if Vy!=0:
        phi = np.arctan(-Vz/Vy)

    Cxp = 2.62375 * np.cos(ap) - 1.95067460 * 10**(-3) * ap**2 - 5.47690476 * 10**(-4)*ap #коэффициент лобового сопротивления 
    Cyp = (-5 * ap**2) / np.pi**2 + (5 * ap) / np.pi  #коэффициент подъемной силы
    
    Cx = Cxp                  #в ССК
    Cy = Cyp * np.cos(phi)    #в ССК
    Cz = - Cyp * np.sin(phi)

    A = findA(ϑ, ψ, γ)

    dVx = - Wy * Vz + Wz * Vy - gc * A[0][1] - Cx * (ro * (Vx**2) / 2) * S / m
    dVy = - Wz * Vx + Wx * Vz - gc * A[1][1] - Cy * (ro * (Vy**2) / 2) * S / m
    dVz = - Wx * Vy + Wy * Vx - gc * A[2][1] - Cz * (ro * (Vz**2) / 2) * S / m

    dV = np.array([dVx, dVy, dVz])

    return dV

def drdt (Vx, Vy, Vz, ϑ, ψ, γ):

    A = findA(ϑ, ψ, γ)

    dX = Vx * A[0][0] + Vy * A[1][0] + Vz * A[2][0]
    dY = Vx * A[0][1] + Vy * A[1][1] + Vz * A[2][1]
    dZ = Vx * A[0][2] + Vy * A[1][2] + Vz * A[2][2]

    dr = np.array([dX, dY, dZ])

    return dr

def dwdt (Vx, Vy, Vz):

    V = np.sqrt(Vx ** 2 + Vy ** 2 + Vz ** 2)

    ap = np.arccos(Vx / V)
    phi = np.pi / 2
    if Vy != 0:
        phi = np.arctan(-Vz / Vy)

    Cxp = 2.62375 * np.cos(ap) - 1.95067460 * 10 ** (-3) * ap ** 2 - 5.47690476 * 10 ** (-4) * ap  # коэффициент лобового сопротивления
    Cyp = (-5 * ap ** 2) / np.pi ** 2 + (5 * ap) / np.pi  # коэффициент подъемной силы

    Cx = Cxp  # в ССК
    Cy = Cyp * np.cos(phi)  # в ССК
    Cz = - Cyp * np.sin(phi)

    dWx = Cx * (ro * (Vx ** 2) / 2) * S * L / I[0][0]
    dWy = Cy * (ro * (Vy ** 2) / 2) * S * L / I[1][1]
    dWz = Cz * (ro * (Vz ** 2) / 2) * S * L / I[2][2]

    dW = np.array([dWx, dWy, dWz])

    return dW

def Euler(Wx, Wy, Wz, ϑ, ψ, γ):

    if SHEMA==0:
        dϑ = (Wy * np.sin(γ) + Wz * np.cos(γ)) / np.cos(ψ)
        dψ = Wy * np.cos(γ) - Wz * np.sin(γ)
        dγ = Wx + (Wy * np.sin(γ) + Wz * np.cos(γ)) * np.tan(ψ)

    if SHEMA==1:
        dϑ = Wy * np.sin(γ) + Wz * np.cos(γ)
        dψ = (Wy * np.cos(γ) - Wz * np.sin(γ)) / np.cos(ϑ)
        dγ = Wx - (Wy * np.cos(γ) - Wz * np.sin(γ)) * np.tan(ϑ)

    return dϑ, dψ, dγ

def RK (Vx, Vy, Vz, x, y, z, Wx, Wy, Wz, ϑ, ψ, γ):

    # расчет коэффициентов: k1, k2, k3, k4 через \n\n соответственно
    V_k1 = dvdt(Vx, Vy, Vz, Wx, Wy, Wz, ϑ, ψ, γ)
    Vx_k1 = dt * V_k1[0]
    Vy_k1 = dt * V_k1[1]
    Vz_k1 = dt * V_k1[2]
    R_k1 = drdt(Vx, Vy, Vz, ϑ, ψ, γ)
    x_k1 = dt * R_k1[0]
    y_k1 = dt * R_k1[1]
    z_k1 = dt * R_k1[2]
    W_k1 = dwdt(Vx, Vy, Vz)
    Wx_k1 = dt * W_k1[0]
    Wy_k1 = dt * W_k1[1]
    Wz_k1 = dt * W_k1[2]
    if METOD==0:
        YGOL_k1 = Euler(Wx, Wy, Wz, ϑ, ψ, γ)
    ϑ_k1 = dt * YGOL_k1[0]
    ψ_k1 = dt * YGOL_k1[1]
    γ_k1 = dt * YGOL_k1[2]

    V_k2 = dvdt(Vx + Vx_k1 / 2, Vy + Vy_k1 / 2, Vz + Vz_k1 / 2, Wx + Wx_k1 / 2, Wy + Wy_k1 / 2, Wz + Wz_k1 / 2, ϑ + ϑ_k1 / 2, ψ + ψ_k1 / 2, γ + γ_k1 / 2)
    Vx_k2 = dt * V_k2[0]
    Vy_k2 = dt * V_k2[1]
    Vz_k2 = dt * V_k2[2]
    R_k2 = drdt(Vx + Vx_k1 / 2, Vy + Vy_k1 / 2, Vz + Vz_k1 / 2, ϑ + ϑ_k1 / 2, ψ + ψ_k1 / 2, γ + γ_k1 / 2)
    x_k2 = dt * R_k2[0]
    y_k2 = dt * R_k2[1]
    z_k2 = dt * R_k2[2]
    W_k2 = dwdt(Vx + Vx_k1 / 2, Vy + Vy_k1 / 2, Vz + Vz_k1 / 2)
    Wx_k2 = dt * W_k2[0]
    Wy_k2 = dt * W_k2[1]
    Wz_k2 = dt * W_k2[2]
    if METOD==0:
        YGOL_k2 = Euler(Wx + Wx_k1 / 2, Wy + Wy_k1 / 2, Wz + Wz_k1 / 2, ϑ + ϑ_k1 / 2, ψ + ψ_k1 / 2, γ + γ_k1 / 2)
    ϑ_k2 = dt * YGOL_k2[0]
    ψ_k2 = dt * YGOL_k2[1]
    γ_k2 = dt * YGOL_k2[2]

    V_k3 = dvdt(Vx + Vx_k2 / 2, Vy + Vy_k2 / 2, Vz + Vz_k2 / 2, Wx + Wx_k2 / 2, Wy + Wy_k2 / 2, Wz + Wz_k2 / 2, ϑ + ϑ_k2 / 2, ψ + ψ_k2 / 2, γ + γ_k2 / 2)
    Vx_k3 = dt * V_k3[0]
    Vy_k3 = dt * V_k3[1]
    Vz_k3 = dt * V_k3[2]
    R_k3 = drdt(Vx + Vx_k2 / 2, Vy + Vy_k2 / 2, Vz + Vz_k2 / 2, ϑ + ϑ_k2 / 2, ψ + ψ_k2 / 2, γ + γ_k2 / 2)
    x_k3 = dt * R_k3[0]
    y_k3 = dt * R_k3[1]
    z_k3 = dt * R_k3[2]
    W_k3 = dwdt(Vx + Vx_k2 / 2, Vy + Vy_k2 / 2, Vz + Vz_k2 / 2)
    Wx_k3= dt * W_k3[0]
    Wy_k3 = dt * W_k3[1]
    Wz_k3 = dt * W_k3[2]
    if METOD==0:
        YGOL_k3 = Euler(Wx + Wx_k2 / 2, Wy + Wy_k2 / 2, Wz + Wz_k2 / 2, ϑ + ϑ_k2 / 2, ψ + ψ_k2 / 2, γ + γ_k2 / 2)
    ϑ_k3 = dt * YGOL_k3[0]
    ψ_k3 = dt * YGOL_k3[1]
    γ_k3 = dt * YGOL_k3[2]

    V_k4 = dvdt(Vx + Vx_k3, Vy + Vy_k3, Vz + Vz_k3, Wx + Wx_k3, Wy + Wy_k3, Wz + Wz_k3, ϑ + ϑ_k3, ψ + ψ_k3, γ + γ_k3)
    Vx_k4 = dt * V_k4[0]
    Vy_k4 = dt * V_k4[1]
    Vz_k4 = dt * V_k4[2]
    R_k4 = drdt(Vx + Vx_k3, Vy + Vy_k3, Vz + Vz_k3, ϑ + ϑ_k3, ψ + ψ_k3, γ + γ_k3)
    x_k4 = dt * R_k4[0]
    y_k4 = dt * R_k4[1]
    z_k4 = dt * R_k4[2]
    W_k4 = dwdt(Vx + Vx_k3, Vy + Vy_k3, Vz + Vz_k3)
    Wx_k4 = dt * W_k4[0]
    Wy_k4 = dt * W_k4[1]
    Wz_k4 = dt * W_k4[2]
    if METOD==0:
        YGOL_k4 = Euler(Wx + Wx_k3, Wy + Wy_k3, Wz + Wz_k3, ϑ + ϑ_k3, ψ + ψ_k3, γ + γ_k3)
    ϑ_k4 = dt * YGOL_k4[0]
    ψ_k4 = dt * YGOL_k4[1]
    γ_k4 = dt * YGOL_k4[2]

    # расчет новых величин в конце участка, соответствующего времени t + dt
    Vx += (Vx_k1 + Vx_k2 * 2 + Vx_k3 * 2 + Vx_k4) / 6
    Vy += (Vy_k1 + Vy_k2 * 2 + Vy_k3 * 2 + Vy_k4) / 6
    Vz += (Vz_k1 + Vz_k2 * 2 + Vz_k3 * 2 + Vz_k4) / 6
    x += (x_k1 + x_k2 * 2 + x_k3 * 2 + x_k4) / 6
    y += (y_k1 + y_k2 * 2 + y_k3 * 2 + y_k4) / 6
    z += (z_k1 + z_k2 * 2 + z_k3 * 2 + z_k4) / 6
    Wx += (Wx_k1 + Wx_k2 * 2 + Wx_k3 * 2 + Wx_k4) / 6
    Wy += (Wy_k1 + Wy_k2 * 2 + Wy_k3 * 2 + Wy_k4) / 6
    Wz += (Wz_k1 + Wz_k2 * 2 + Wz_k3 * 2 + Wz_k4) / 6
    ϑ += (ϑ_k1 + ϑ_k2 * 2 + ϑ_k3 * 2 + ϑ_k4) / 6
    ψ += (ψ_k1 + ψ_k2 * 2 + ψ_k3 * 2 + ψ_k4) / 6
    γ += (γ_k1 + γ_k2 * 2 + γ_k3 * 2 + γ_k4) / 6

    return Vx, Vy, Vz, x, y, z, Wx, Wy, Wz, ϑ, ψ, γ


while y>0:
    V = np.sqrt(Vx ** 2 + Vy ** 2 + Vz ** 2)
    print(V, x, y, z, t)
    Vx, Vy, Vz, x, y, z, Wx, Wy, Wz, ϑ, ψ, γ = RK (Vx, Vy, Vz, x, y, z, Wx, Wy, Wz, ϑ, ψ, γ)
    t += dt