Zakladni verze planovace

src/external/gps_utils.py

@@ -0,0 +1,34 @@
+import math
+
+EARTH_RADIUS = 6378.1  # Radius of the Earth
+
+
+def get_bearing(center_point: tuple[float, float], end_point: tuple[float, float]):
+    lat3 = math.radians(end_point[0])
+    long3 = math.radians(end_point[1])
+    lat1 = math.radians(center_point[0])
+    long1 = math.radians(center_point[1])
+
+    dLon = long3 - long1
+
+    X = math.cos(lat3) * math.sin(dLon)
+    Y = math.cos(lat1) * math.sin(lat3) - math.sin(lat1) * math.cos(lat3) * math.cos(dLon)
+
+    end_brng = math.atan2(X, Y)
+
+    return end_brng
+
+
+def get_distance(a: tuple[float, float], b: tuple[float, float]):
+    lat1 = math.radians(a[0])
+    lon1 = math.radians(a[1])
+    lat2 = math.radians(b[0])
+    lon2 = math.radians(b[1])
+
+    dlon = lon2 - lon1
+    dlat = lat2 - lat1
+
+    a = math.sin(dlat / 2) ** 2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2) ** 2
+    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
+
+    return EARTH_RADIUS * c