/*
  google Mapの座標値から距離を求めるルーチン
  2018-03-21(水) 14:41:31  
 
  gcc -c location_calc.cpp
*/
 
#include <math.h>
 
 
typedef struct location{
  // ちなみに X,Y 軸座標は、→に＋  ↑に＋
  double longitude; // 経度 東経 139.691 X軸  
  double latitude;  // 緯度 北緯 35.698  Y軸 
} LOCATION;
 
 
#define rad2deg(a) ((a)/M_PI * 180.0) /* rad を deg に換算するマクロ関数 */
#define deg2rad(a) ((a)/180.0 * M_PI) /* deg を rad に換算するマクロ関数 */
 
double distance_km(LOCATION* A, LOCATION* B, double *rad_up)
{
  double earth_r = 6378.137;
 
  double loRe = deg2rad(B->longitude - A->longitude); // 東西
  double laRe = deg2rad(B->latitude - A->latitude); // 南北
 
  double EWD = cos(deg2rad(A->latitude))*earth_r*loRe; // 東西距離
  double NSD = earth_r*laRe; //南北距離
 
  double distance = sqrt(pow(NSD,2)+pow(EWD,2));  
  *rad_up = atan2(NSD, EWD);
 
  return distance;
}
 
LOCATION* new_location(LOCATION* D, double diff_p_x, double diff_p_y) 
{
  double earth_r = 6378.137;
 
  double loRe = diff_p_x / earth_r / cos(deg2rad(D->latitude)); // 東西
  double laRe = diff_p_y / earth_r;  // 南北
  
  double diff_lo = rad2deg(loRe); // 東西
  double diff_la = rad2deg(laRe); // 南北
 
  static LOCATION C;
 
  C.longitude = D->longitude + diff_lo; // 東西
  C.latitude = D->latitude + diff_la; // 南北
  
  return &C;
}
  
double diff_longitude(double diff_p_x, double latitude) 
{
  double earth_r = 6378.137;
  // ↓ これが正解だけど、
  double loRe = diff_p_x / earth_r / cos(deg2rad(latitude)); // 東西
  // 面倒なので、これで統一しよう(あまり差が出ないしね)
  //double loRe = diff_p_x / earth_r / cos(deg2rad(35.700759)); // 東西
  double diff_lo = rad2deg(loRe); // 東西
 
  return diff_lo; // 東西
}
  
double diff_latitude(double diff_p_y) 
{
  double earth_r = 6378.137;
  double laRe = diff_p_y / earth_r;  // 南北
  double diff_la = rad2deg(laRe); // 南北
  
  return diff_la; // 南北
}