Refactor projection

mavsdk_wrapper.cpp

@@ -38,6 +38,9 @@ static std::future<std::shared_ptr<System>> fut;
 
 static const float DEFAULT_RADIUS = 100;
 static const float EARTH_RADIUS = 6371000;
+static const float ADDED_DISTANCE = (2 * DEFAULT_RADIUS) / EARTH_RADIUS;
+static const double COS_ADDED_DISTANCE = cos(ADDED_DISTANCE);
+static const double SIN_ADDED_DISTANCE = sin(ADDED_DISTANCE);
 
 static int mavsdk_started = 0;
 static Telemetry::Position origin;
@@ -117,15 +120,16 @@ static double bearing(double lat1, double lon1, double lat2, double lon2) {
 
 static Coordinates project(Coordinates destination, double bearing) {
     double laRad = toRad(destination.latitude);
+    double sinLa = sin(laRad);
+    double cosLa = cos(laRad);
     double loRad = toRad(destination.longitude);
-    float addedDistance = (2 * DEFAULT_RADIUS) / EARTH_RADIUS;
-    double newLa = asin(sin(laRad) * cos(addedDistance)
-                        + cos(laRad) * sin(addedDistance) * cos(bearing));
+    double newLa = asin(sinLa * COS_ADDED_DISTANCE
+                        + cosLa * SIN_ADDED_DISTANCE * cos(bearing));
     Coordinates projected{
         toDeg(newLa),
         toDeg(loRad + atan2(
-            sin(bearing) * sin(addedDistance) * cos(laRad),
-            cos(addedDistance) - sin(laRad) * sin(newLa)
+            sin(bearing) * SIN_ADDED_DISTANCE * cosLa,
+            COS_ADDED_DISTANCE - sinLa * sin(newLa)
         )),
     };
     return projected;