/*---
  WGS84の緯度,経度,高さと直交座標(ECEF)の変換
  2002.4.25: S.Fukushima(ENRI)
  ---*/

#include "str01.h"
#include "MathUtils.h"
#include <cstdio>
#include <cstdlib>

using namespace qrk;


#define A 6378137.0             /* Semi-major axis */
#define E2 ((1.0/ONE_F)*(2-(1.0/ONE_F)))
#define NN(p) (A/sqrt(1.0 - (E2)*SQR(sin(p*PI/180.0))))
#define ONE_F 298.257223563     /* 1/F */
#define        PI 3.1415926535898
#define SQR(x) ((x)*(x))
#define B (A*(1.0 - 1.0/ONE_F))
#define ED2 (E2*A*A/(B*B))
#define CUB(y) ((y)*(y)*(y))


/* 緯度,経度,高さからECEF座標に変換 */
vector blh2ecef(double phi, double ramda, double height)
{
  vector ecef;

  ecef.n = 3;
  ecef.a[0] = (NN(phi)+height)*cos(phi*PI/180)*cos(ramda*PI/180);
  ecef.a[1] = (NN(phi)+height)*cos(phi*PI/180)*sin(ramda*PI/180);
  ecef.a[2] = (NN(phi)*(1-E2)+height)*sin(phi*PI/180);

  return ecef;
}


/* ECEF座標からWGS84の{緯度,経度,楕円体高}へ変換 */
vector ecef2blh(vector ec)
{
  vector blh;
  double phi, ramda, height, p;
  double x, y, z;
  double sita;

  ec.n = 3; blh.n = 3;
  x = ec.a[0], y = ec.a[1], z = ec.a[2];

  p = sqrt(x*x + y*y);
  sita = (180/PI) * atan2(z*A, p*B);

  /*--- 緯度 */
  phi = (180/PI) * atan2(z+ED2*B*(CUB(sin(sita*PI/180))),(p-E2*A*(CUB(cos(sita*PI/180)))));

  /*--- 経度 */
  ramda = (180/PI) * atan2(y,x);

  /*--- 高さ */
  height = (p / cos(phi*PI/180)) - NN(phi);

  blh.a[0] = phi; blh.a[1] = ramda; blh.a[2] = height;
  return blh;
}


#if 0
/*--- チェック用 */
int main()
{
  double lat, lon, hig;
  vector ecef, blh;

  lat =  38.13579617;
  lon = 140.91581617;
  hig =  41.940;

  ecef = blh2ecef(lat, lon, hig);
  printf("%.3f %.3f %.3f\n", ecef.a[0], ecef.a[1], ecef.a[2]);

  blh = ecef2blh(ecef);
  printf("%.8f %.8f %.3f\n", blh.a[0], blh.a[1], blh.a[2]);
}
/*---
  結果: -3899086.094 3166914.545 3917336.601
  38.13579617 140.91581617 41.940
  ---*/
#endif
