Updated GlobalTrackData struct to store precomputed circle radius

src/custom_types.cpp

@@ -4,14 +4,13 @@
 #include "custom_types.h"
 #include "math.h"
 
-Vec2 eqRectProjection(const LatLng target, const LatLng ref) {
+Vec2 eqRectProjection(const LatLng& target, const LatLng& ref) {
   const float R = 6371000.0;
   float lat0_r = ref.lat_ * M_PI / 180;
   float lng0_r = ref.lng_ * M_PI / 180;
   float lat1_r = target.lat_ * M_PI / 180;
   float lng1_r = target.lng_ * M_PI / 180;
-  float lat_avg = (lat0_r + lat1_r) * 0.5f;
-  float lat0_cos = cos(lat_avg);
+  float lat0_cos = cos((lat0_r + lat1_r) * 0.5f);
 
 
   Vec2 res;
@@ -21,14 +20,46 @@ Vec2 eqRectProjection(const LatLng target, const LatLng ref) {
   return res;
 }
 
-Vec2 abMidpoint(const Vec2 A, const Vec2 B) {
+LatLng eqRectInverse(const Vec2& point, const LatLng& ref) {
+    const float R = 6371000.0f;
+
+    float lat0 = ref.lat_ * M_PI / 180.0f;
+    float lng0 = ref.lng_ * M_PI / 180.0f;
+
+    float cos_lat0 = cos(lat0);
+
+    // Recover latitude
+    float lat = lat0 + (point.y_ / R);
+
+    // Recover longitude
+    float lng = lng0 + (point.x_ / (R * cos_lat0));
+
+    LatLng res;
+    res.lat_ = lat * 180.0f / M_PI;
+    res.lng_ = lng * 180.0f / M_PI;
+
+    return res;
+}
+
+Vec2 abMidpoint(const Vec2& A, const Vec2& B) {
   Vec2 res;
   res.x_ = (A.x_ + B.x_) * 0.5f;
   res.y_ = (A.y_ + B.y_) * 0.5f;
   return res;
 }
 
-float abSqDist(const Vec2 A, const Vec2 B) {
+Vec2 abSum(const Vec2& A, const Vec2& B) {
+  Vec2 res;
+  res.x_ = A.x_ + B.x_;
+  res.y_ = A.y_ + B.y_;
+  return res;
+}
+
+float vecMod(const Vec2& in) {
+  return sqrtf(in.x_*in.x_ + in.y_*in.y_);
+}
+
+float abSqDist(const Vec2& A, const Vec2& B) {
   float deltaX = B.x_ - A.x_;
   float deltaY = B.y_ - A.y_;
   return deltaX * deltaX + deltaY * deltaY;