popufare/busunit/common/gpsmath.c

/*
 * Copyright (c) 2019 Clementine Computing LLC.
 *
 * This file is part of PopuFare.
 * 
 * PopuFare is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * PopuFare is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Affero General Public License for more details.
 * 
 * You should have received a copy of the GNU Affero General Public License
 * along with PopuFare.  If not, see <https://www.gnu.org/licenses/>.
 *
 */

#include <stdio.h>
#include <math.h>
#include "gpsmath.h"

#define EARTH_RAD (6372795.0)
#define DEG2RAD(deg) (deg * 0.0174532925)
#define DECDEG(gps) (trunc(gps / 100.0) + ((gps - (trunc(gps / 100.0) * 100.0)) / 60.0))

double trunc(double x);

// calculated GPS distance in meters between two points
//
float GPS_Dist(float lat1, float lon1, float lat2, float lon2) {
  float dr_lat1,dr_lat2,delta_lon;
  float sinlat1,coslat1,sinlat2,coslat2,sindl,cosdl;
  float y,x;

  dr_lat1 = DEG2RAD(DECDEG(lat1));  
  dr_lat2 = DEG2RAD(DECDEG(lat2));

  delta_lon = DEG2RAD(DECDEG(lon1)) - DEG2RAD(DECDEG(lon2));

  sinlat1=sin(dr_lat1);
  sinlat2=sin(dr_lat2);
  coslat1=cos(dr_lat1);
  coslat2=cos(dr_lat2);
  sindl=sin(delta_lon);
  cosdl=cos(delta_lon);

  y=sqrt( pow(coslat2 * sindl , 2) + pow((coslat1 * sinlat2) - (sinlat1 * coslat2 * cosdl), 2));
  x=(sinlat1 * sinlat2) + (coslat2 * coslat2 * cosdl);
  return EARTH_RAD * atan2(y,x);
}

/* int main() */
/* { */
/*   printf("Distance in meters = %f\n",GPS_Dist(-3607.2,-8640.2, -3356.4,-11824.0)/1000); */
/*   return 0; */
/* } */