Расчет траектории манёвра летательного аппарата

using Plots

# ========================
# Структура точки — ПЕРВОЙ
# ========================

struct Waypoint
    x::Float64
    y::Float64
end

# ========================
# Входные параметры
# ========================

A = Waypoint(0.0, 0.0)        # Начальная точка маршрута
B = Waypoint(1500.0, 0.0)     # Точка начала разворота
C = Waypoint(2000.0, -1800.0) # Целевая точка
R = 300.0                      # Радиус разворота, м
V = 35.0                       # Скорость, м/с

# ========================
# Утилиты
# ========================

distance(p1::Waypoint, p2::Waypoint) =
    sqrt((p1.x - p2.x)^2 + (p1.y - p2.y)^2)

# ========================
# Основной алгоритм
# ========================

function compute_turn(A::Waypoint, B::Waypoint, C::Waypoint, R::Float64)

    # Шаг 1. Коэффициенты нормали к прямой AB
    A_ab = B.y - A.y
    B_ab = A.x - B.x
    t    = R / sqrt(A_ab^2 + B_ab^2)

    # Шаг 2. Два кандидата на центр окружности O
    O1 = Waypoint(B.x + A_ab * t, B.y + B_ab * t)
    O2 = Waypoint(B.x - A_ab * t, B.y - B_ab * t)
    O  = distance(C, O1) < distance(C, O2) ? O1 : O2

    # Шаг 3. Находим точку H
    CO = distance(C, O)
    t1 = R^2 / CO^2
    H  = Waypoint(O.x + t1 * (C.x - O.x), O.y + t1 * (C.y - O.y))

    # Шаг 4. Длина DH и нормаль к OC
    DH   = R * sqrt(max(0.0, 1 - t1))
    A_oc = O.y - C.y
    B_oc = C.x - O.x
    t2   = DH / sqrt(A_oc^2 + B_oc^2)

    # Шаг 5. Два кандидата на точку D
    D1 = Waypoint(H.x + A_oc * t2, H.y + B_oc * t2)
    D2 = Waypoint(H.x - A_oc * t2, H.y - B_oc * t2)

    # Шаг 6. Выбираем D по углу ABC
    AC² = distance(A, C)^2
    AB² = distance(A, B)^2
    BC² = distance(B, C)^2
    BD1 = distance(B, D1)
    BD2 = distance(B, D2)

    if AC² ≈ AB² + BC²
        D = distance(A, D1) > distance(A, D2) ? D1 : D2
    elseif AC² > AB² + BC²
        D = BD1 < BD2 ? D1 : D2
    else
        D = BD1 > BD2 ? D1 : D2
    end

    # Шаг 7. Центральный угол φ
    BD    = distance(B, D)
    cos_φ = clamp(1 - BD^2 / (2 * R^2), -1.0, 1.0)
    φ     = acos(cos_φ)

    # Шаг 8. Расстояние от C до прямой AB
    L1 = abs(A_ab * C.x + B_ab * C.y - A_ab * A.x - B_ab * A.y) /
         sqrt(A_ab^2 + B_ab^2)

    # Шаг 9. Длина дуги BD
    arc_BD = if L1 < 2R && AC² < AB² + BC²
        (2π - φ) * R
    else
        φ * R
    end

    return D, O, arc_BD, φ
end

# ========================
# Запуск
# ========================

D, O, arc_BD, φ = compute_turn(A, B, C, R)

# Метрики
dist_AB    = distance(A, B)
dist_arc   = arc_BD
dist_DC    = distance(D, C)
total_dist = dist_AB + dist_arc + dist_DC

ω          = V / R
time_AB    = dist_AB  / V
time_arc   = φ / ω
time_DC    = dist_DC  / V
total_time = time_AB + time_arc + time_DC

# ========================
# Вывод результатов
# ========================

println("=" ^ 45)
println("Входные параметры:")
println("  A = ($(A.x), $(A.y))")
println("  B = ($(B.x), $(B.y))")
println("  C = ($(C.x), $(C.y))")
println("  R = $R м")
println("  V = $V м/с")
println("=" ^ 45)
println("Результаты:")
println("  Центр окружности O = ($(round(O.x, digits=1)), $(round(O.y, digits=1)))")
println("  Точка касания    D = ($(round(D.x, digits=1)), $(round(D.y, digits=1)))")
println("  Центральный угол   = $(round(rad2deg(φ), digits=1))°")
println("  Угловая скорость   = $(round(ω, digits=3)) рад/с")
println("-" ^ 45)
println("Участок A → B:  $(round(dist_AB,   digits=1)) м  |  $(round(time_AB,   digits=1)) с")
println("Дуга  B → D:    $(round(dist_arc,  digits=1)) м  |  $(round(time_arc,  digits=1)) с")
println("Участок D → C:  $(round(dist_DC,   digits=1)) м  |  $(round(time_DC,   digits=1)) с")
println("-" ^ 45)
println("Итого расстояние: $(round(total_dist, digits=1)) м")
println("Итого время:      $(round(total_time, digits=1)) с")
println("=" ^ 45)

# ========================
# Визуализация
# ========================

angle_B  = atan(B.y - O.y, B.x - O.x)
angle_D  = atan(D.y - O.y, D.x - O.x)
angles   = range(angle_B, angle_D, length=200)
arc_x    = O.x .+ R .* cos.(angles)
arc_y    = O.y .+ R .* sin.(angles)

θ        = range(0, 2π, length=300)
circle_x = O.x .+ R .* cos.(θ)
circle_y = O.y .+ R .* sin.(θ)

plt = plot(circle_x, circle_y,
    linestyle=:dash, color=:lightgray,
    label="Окружность разворота", aspect_ratio=:equal)

plot!(plt, [A.x, B.x], [A.y, B.y],
    color=:blue, linewidth=2, label="A → B")

plot!(plt, arc_x, arc_y,
    color=:red, linewidth=2, label="Дуга B → D")

plot!(plt, [D.x, C.x], [D.y, C.y],
    color=:green, linewidth=2, label="D → C")

scatter!(plt,
    [A.x, B.x, C.x, D.x, O.x],
    [A.y, B.y, C.y, D.y, O.y],
    color=:black, markersize=5, label="")

off = R * 0.08
annotate!(plt, A.x, A.y + off, text("A", 10, :blue))
annotate!(plt, B.x, B.y + off, text("B", 10, :blue))
annotate!(plt, C.x, C.y + off, text("C", 10, :green))
annotate!(plt, D.x, D.y + off, text("D", 10, :red))
annotate!(plt, O.x, O.y + off, text("O", 10, :gray))

title!(plt, "Траектория A → B → (дуга) → D → C")
xlabel!(plt, "x, м")
ylabel!(plt, "y, м")
xlims!(plt, -100, 2500)
ylims!(plt, -2500, 100)
display(plt)