/////////////////////////////////////////////////////////////////////
//	ユーザ位置（緯度、経度、高度）をWGS84系（地球中心のｘ、ｙ、ｚ）の座標に変換する
//　ユーザ位置とはDOP等を計算するための仮想観測地点
//　ここでは複数のユーザ位置を決定できるようにする
/////////////////////////////////////////////////////////////////////

#include <math.h>

#define DIV1     1//何個(縦横)にグリッドを区切るか
#define DIVMAX  4//グリッドを格納する配列の最大値を設定
const double PI       = 3.1415926535898;//円周率

//解析グリッドについて
extern double Start_latitude;//解析領域の左下
extern double Start_longitude;//左下
extern double End_latitude;//右上
extern double End_longitude;//右上
extern double Height;//高度は全ての地点で一定

extern double Lat_pos[DIVMAX],Lon_pos[DIVMAX],Hei_pos[DIVMAX][DIVMAX];//ユーザ位置の座標
extern double POSx[DIVMAX][DIVMAX],POSy[DIVMAX][DIVMAX],POSz[DIVMAX][DIVMAX];//解析位置


void trans_user_point()
{
	int i,j;
	double pp[5],a,f,b,n;

	pp[0] = Start_latitude;
	pp[1] = Start_longitude;
	pp[2] = Height;
	pp[3] = End_latitude;
	pp[4] = End_longitude;

	a = 6378137.0;//赤道半径
	f = 1/298.257223563;//逆扁平率
	b = a*(1-f);

	//DOP等を計算するためのユーザ位置を決定
	//複数のユーザ位置におけるDOP等を計算するために
	//四角形の四隅を決定し、その中を等分する
	for(i=0;i<=DIV1;i++){
		for(j=0;j<=DIV1;j++){
			Lat_pos[i]=pp[0]+i*(pp[3]-pp[0])/DIV1;
			Lon_pos[j]=pp[1]+j*(pp[4]-pp[1])/DIV1;
			Hei_pos[i][j]=pp[2];

		n = a*a/sqrt(a*a*cos(Lat_pos[i]*PI/180)*cos(Lat_pos[i]*PI/180)
		    	+b*b*sin(Lat_pos[i]*PI/180)*sin(Lat_pos[i]*PI/180));
		POSx[i][j] = (n+Hei_pos[i][j])*cos(Lat_pos[i]*PI/180)*cos(Lon_pos[j]*PI/180);
		POSy[i][j] = (n+Hei_pos[i][j])*cos(Lat_pos[i]*PI/180)*sin(Lon_pos[j]*PI/180);
		POSz[i][j] = (n*b*b/a/a+Hei_pos[i][j])*sin(Lat_pos[i]*PI/180);
		}
	}
}