demo.py

#!/usr/bin/env python3

import numpy as np
import matplotlib.pyplot as plt
from math import radians, degrees, pi, sin, asin, cos, atan2, sqrt, acos

EARTH_RADIUS = 6371


def wingman_coord(latitude, longitude, altitude, heading, wingman_angle, wingman_distance):
    a = wingman_distance / (EARTH_RADIUS + altitude)
    gamma = radians(heading + wingman_angle)
    c = asin(sin(a) * sin(gamma))
    b = asin(cos(gamma) / cos(c) * sin(a))
    return latitude + degrees(b), longitude + degrees(c), altitude


coords = []
coords1 = []
coords2 = []


latitude, longitude = 48.93972405501592, 2.308811640560172
altitude = 10
heading = 0


for i in range(0, 1360):
    if i > 1000:
        heading += 1  # tourne en rond
    else:
        heading = 45 + sin(i / 100) * 100  # zig-zag

    # leader
    coords.append((longitude, latitude))

    # wingman 1 (à 50m sur l'arrière droite)
    latitude_w, longitude_w, _ = wingman_coord(latitude, longitude, altitude, heading, 120, 0.05)
    coords1.append((longitude_w, latitude_w))

    # wingman 2 (à 50m sur l'arrière gauche)
    latitude_w, longitude_w, _ = wingman_coord(latitude, longitude, altitude, heading, -120, 0.05)
    coords2.append((longitude_w, latitude_w))

    # avance de 2m
    latitude, longitude, _ = wingman_coord(latitude, longitude, altitude, heading, 0, 0.002)


plt.plot(*np.array(coords).T, label="leader")
plt.plot(*np.array(coords1).T, label="right")
plt.plot(*np.array(coords2).T, label="left")
plt.legend(loc="upper left")
plt.show()

squadron.py

#!/usr/bin/env python3

import simplekml
import numpy as np
import matplotlib.pyplot as plt
from math import radians, degrees, pi, sin, asin, cos, atan2, sqrt, acos
from geographiclib.geodesic import Geodesic


EARTH_RADIUS = 6371

geod = Geodesic.WGS84
# geod = Geodesic(EARTH_RADIUS * 1000, 0)


def wingman_coord(latitude, longitude, altitude, heading, wingman_angle, wingman_distance):
    """
    Calcul simple avec formules de trigo sphérique
    https://fr.wikipedia.org/wiki/Trigonométrie_sphérique
    """
    a = wingman_distance / (EARTH_RADIUS + altitude)
    gamma = radians(heading + wingman_angle)
    c = asin(sin(a) * sin(gamma))
    sin_beta = cos(gamma) / cos(c)
    b = asin(sin_beta * sin(a))
    return latitude + degrees(b), longitude + degrees(c), altitude


def wingman_geodesic(latitude, longitude, altitude, heading, wingman_angle, wingman_distance):
    """
    https://geographiclib.sourceforge.io/html/python/code.html#geographiclib.geodesic.Geodesic.Direct
    """
    g = geod.Direct(latitude, longitude, heading + wingman_angle, wingman_distance * 1000)
    return g["lat2"], g["lon2"], altitude


def great_circle(lat1, lon1, lat2, lon2):
    lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
    return EARTH_RADIUS * acos(sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(lon1 - lon2))


def haversine(lat1, lon1, lat2, lon2):
    lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])
    dlon = lon2 - lon1
    dlat = lat2 - lat1
    a = sin(dlat / 2) ** 2 + cos(lat1) * cos(lat2) * sin(dlon / 2) ** 2
    return 2 * 6371 * asin(sqrt(a))


coords = []
coords1 = []
coords2 = []


latitude, longitude = 48.93972405501592, 2.308811640560172
altitude = 10
heading = 0


for i in range(0, 1360):
    if i > 1000:
        heading += 1  # tourne en rond
    else:
        heading = 45 + sin(i / 100) * 100  # zig-zag

    # leader
    coords.append((longitude, latitude))

    # wingman 1 (à 50m sur l'arrière droite)
    latitude_w, longitude_w, _ = wingman_geodesic(latitude, longitude, altitude, heading, 90, 0.05)
    coords1.append((longitude_w, latitude_w))

    # wingman 2 (à 50m sur l'arrière gauche)
    # latitude_w, longitude_w, altitude_w = wingman(latitude, longitude, altitude, heading, 240, 0.05)
    latitude_w, longitude_w, _ = wingman_geodesic(latitude, longitude, altitude, heading, -90, 0.05)
    coords2.append((longitude_w, latitude_w))

    # avance de 2m
    latitude, longitude, _ = wingman_geodesic(latitude, longitude, altitude, heading, 0, 0.002)

# exporte en KML
#
kml = simplekml.Kml(open=1)

linestring = kml.newlinestring(name="leader")
linestring.coords = coords
linestring.linestyle.width = 4
linestring.linestyle.color = simplekml.Color.blue
linestring.altitudemode = simplekml.AltitudeMode.clamptoground

linestring = kml.newlinestring(name=f"right")
linestring.coords = coords1
linestring.linestyle.width = 4
linestring.linestyle.color = simplekml.Color.red
linestring.altitudemode = simplekml.AltitudeMode.clamptoground

linestring = kml.newlinestring(name=f"left")
linestring.coords = coords2
linestring.linestyle.width = 4
linestring.linestyle.color = simplekml.Color.yellowgreen
linestring.altitudemode = simplekml.AltitudeMode.clamptoground

kml.save("squadron.kml")


# montre en 2D les trajectoires
#
plt.plot(*np.array(coords).T, label="leader")
plt.plot(*np.array(coords1).T, label="right")
plt.plot(*np.array(coords2).T, label="left")
plt.legend(loc="upper left")
plt.show()


# def ginv(lat1, lon1, lat2, lon2):
#     g = geod.Inverse(lat1, lon1, lat2, lon2)
#     return g["s12"], g["azi1"]

# latitude, longitude = 20, 50

# for heading in range(0, 360, 45):
#     print(f"{heading:3}", end=" ")
#     lat1, lon1 = latitude, longitude
#     lat2, lon2, _ = wingman_coord(lat1, lon1, 0, heading, 0, 0.002)
#     # print(haversine(lat1, lon1, lat2, lon2) * 1000, great_circle(lat1, lon1, lat2, lon2) * 1000, end=" ")
#     # lat2, lon2, _ = wingman_geodesic(lat1, lon1, 0, heading, 0, 0.002)
#     hs = haversine(lat1, lon1, lat2, lon2) * 1000
#     gc = great_circle(lat1, lon1, lat2, lon2) * 1000
#     print(f"{hs:8.6f} {gc:8.6f}", end=" ")
#     s, az = ginv(lat1, lon1, lat2, lon2)
#     print(f"{s:8.6f} {az%360:9.4f}", end=" ")
#     print()