uas-ugv/main/lat_long.cc

#include "lat_long.hh"

float LatLong::distance_to(const LatLong &target) const {
  float lat1 = latitude * RAD_PER_DEG;
  float lat2 = target.latitude * RAD_PER_DEG;
  float long1 = longitude * RAD_PER_DEG;
  float long2 = target.longitude * RAD_PER_DEG;
  float clat1 = cosf(lat1);
  float clat2 = cosf(lat2);
  float a = powf(sinf((long2 - long1) / 2.f), 2.f) * clat1 * clat2 +
            powf(sinf((lat2 - lat1) / 2.f), 2.f);
  float d_over_r = 2 * atan2f(sqrtf(a), sqrtf(1 - a));
  return d_over_r * EARTH_RAD;
}

float LatLong::bearing_toward(const LatLong &target) const {
  float dlong = (target.longitude - longitude) * RAD_PER_DEG;
  float sdlong = sinf(dlong);
  float cdlong = cosf(dlong);
  float lat1 = latitude * RAD_PER_DEG;
  float lat2 = target.latitude * RAD_PER_DEG;
  float slat1 = sinf(lat1);
  float clat1 = cosf(lat1);
  float slat2 = sinf(lat2);
  float clat2 = cosf(lat2);
  float num = sdlong * clat2;
  float denom = (clat1 * slat2) - (slat1 * clat2 * cdlong);
  float course = atan2f(num, denom);
  if (course < 0.0) {
    course += 2 * PI;
  }
  return course / RAD_PER_DEG;
}