//Transform.cpp
//#include "stdafx.h"
#include "math.h"
#include "Transform.h"
#include "stdio.h"
#define a 6378137 //半长轴
#define e2 0.00669437999
#define pi 3.1415926536
#define limit_err1 5e-10
#define limit_err2 5e-4
#define Limit_Zero 5e-8
#define D2R(angle) (angle/180.0*pi)
#define R2D(angle) (angle/pi*180.0)
void MatrixMultiply(double A[3][3], double B[3][3], double C[3][3]);
void GeocentricToGroundMatrix(double lat,double lon, double AA[3][3]);
void GroundToBodyMatrix( double theta, double gamma, double psi,double BB[3][3]) ;
void GeocentricToBodyMatrix(double psi,double theta,double phi, double BB[3][3]);
void XYZtoLonLatH(double*pXYZ,double*pLLH);
void LonLatHtoXYZ(double*pLLH,double*pXYZ);
void Transpose(double AA[3][3], double BB[3][3]);
void MatrixMultiplyVecor(double AA[3][3],double BB[3],double CC[3]);
void GetEulerAngle(double AA[3][3],double BB[3]);
void GroundToGeocentric(double lat, double lon,double Ground[3], double Geocentric[3]);
void GeocentricToGround(double lat, double lon,double Geocentric[3], double Ground[3]);
void RelativePosition(double lat1,double lon1,double alt1,double theta,double gamma,double psi,double lat2,double lon2,double alt2,double RAE[3]);
/************************************************************************************
*函数名称: XYZtoLonLatH
*功 能: 将本地系统中的空间直角坐标系 X、Y、Z转换成基于WGS-84标准椭圆面的经度、纬度、高
* 度。其中高度单位为“m”,经度与纬度的单位为deg。
*输 入：*pXYZ
*输 出：*pLLH
************************************************************************************/
void XYZtoLonLatH(double*pXYZ,double*pLLH)
{
double lat,lat0,err1,err2;
double lon;
double v,x,y,z,h,h0;
x=pXYZ[0];
y=pXYZ[1];
z=pXYZ[2];
lat0=D2R(45); //经度初值，用于迭代
h0=2000;
lat=lat0;
h=h0;
for(int i=0;i<10000;i++)
{
v=a/pow(1-e2*pow(sin(lat),2),0.5);
lat=atan(z/sqrt(pow(x,2)+pow(y,2))/(1-e2*v/(v+h)));
h=sqrt(pow(x,2)+pow(y,2))/cos(lat)-v;
err1=fabs(lat-lat0);
err2=fabs(h-h0);
lat0=lat;
h0=h;
if(err1<limit_err1 &&err2<limit_err2)
break;
}
lon=atan(y/x);
if(y>0 && x<0)
lon=lon+pi;
else if(y < 0 && x < 0)
lon=lon-pi;
pLLH[0]=R2D(lon);
pLLH[1]=R2D(lat);
pLLH[2]=h;
}
/************************************************************************************
*函数名称: XYZtoLonLatH
*功 能: 将基于WGS-84标准椭圆面的经度、纬度、高度转换成本地系统中的空间直角坐标系 X、Y、Z。
* 其中高度单位为“m”,经度与纬度的单位为“ °”
*输 入：*pLLH
*输 出：*pXYZ
************************************************************************************/
void LonLatHtoXYZ(double*pLLH,double*pXYZ)
{
double lat,lon,h;
double v,x,y,z;
lon=pLLH[0];
lat=pLLH[1];
h=pLLH[2];
lat=D2R(lat);
lon=D2R(lon);
v=a/pow(1-e2*pow(sin(lat),2),0.5);
x=(v+h)*cos(lat)*cos(lon);
y=(v+h)*cos(lat)*sin(lon);
z=(v*(1-e2)+h)*sin(lat);
pXYZ[0]=x;
pXYZ[1]=y;
pXYZ[2]=z;
}
/************************************************************************************
*函数名称: MatrixMultiply()
*功 能: 三维矩阵乘法 C = A x B，结果存储在C
*输 入： A ,B
*输 出： C
************************************************************************************/
void MatrixMultiply(double A[3][3], double B[3][3], double C[3][3])
{
for(int i=0;i<3;i++)
{
for(int j=0;j<3;j++)
{
C[i][j]=(A[i][0])*(B[0][j])+(A[i][1])*(B[1][j])+(A[i][2])*(B[2][j]);
}
}
}
/************************************************************************************
*函数名称: GeocentricToGroundMatrix()
*功 能: 计算地心坐标系的坐标值转换成地面坐标系的坐标值所需要的转换矩阵，并存储在矩阵AA中。
* 第一个参数为纬度，第二个参数为经度，角度单位为“ °”，第三个参数是转换矩阵，
*输 入： lat,lon
*输 出： AA
************************************************************************************/
void GeocentricToGroundMatrix(double lat,double lon, double AA[3][3])
{
double coslat;
double sinlat;
double coslon;
double sinlon;
coslat=cos(D2R(lat));
sinlat=sin(D2R(lat));
coslon=cos(D2R(lon));
sinlon=sin(D2R(lon));
AA[0][0]=-coslon*sinlat;
AA[0][1]=-sinlon*sinlat;
AA[0][2]=coslat;
AA[1][0]=-sinlon;
AA[1][1]=coslon;
AA[1][2]=0;
AA[2][0]=-coslat*coslon;
AA[2][1]=-coslat*sinlon;
AA[2][2]=-sinlat;
}
/************************************************************************************
*函数名称: GroundToBodyMatrix()
*功 能: 计算地面坐标系下的坐标值转换成机体坐标系下的坐标值所需的转换矩阵，并存储在矩阵BB中。
* 参数1是俯仰角（Y轴旋转角），参数2是横滚角（X轴旋转角），参数3是偏航角（Z轴旋转角），
* 角度单位为deg，参数4是目标矩阵。当沿着机体轴正方向观察时，旋转轴的正方向为顺时针指向。
*输 入： theta,gamma,psi,
*输 出： BB
************************************************************************************/
void GroundToBodyMatrix( double theta, double gamma, double psi,double BB[3][3])
{
double costheta;
double sintheta;
double cosgamma;
double singamma;
double cospsi;
double sinpsi;
costheta=cos(D2R(theta));
sintheta=sin(D2R(theta));
cosgamma=cos(D2R(gamma));
singamma=sin(D2R(gamma));
cospsi=cos(D2R(psi));
sinpsi=sin(D2R(psi));
BB[0][0]=costheta*cospsi;
BB[0][1]=costheta*sinpsi;
BB[0][2]=-sintheta;
BB[1][0]=sintheta*singamma*cospsi-cosgamma*sinpsi;
BB[1][1]=sintheta*singamma*sinpsi+cosgamma*cospsi;
BB[1][2]=costheta*singamma;
BB[2][0]=sintheta*cosgamma*cospsi+singamma*sinpsi;
BB[2][1]=sintheta*cosgamma*sinpsi-singamma*cospsi;
BB[2][2]=costheta*cosgamma;
}
/************************************************************************************
*函数名称: GeocentricToBodyMatrix()
*功 能: 计算地心坐标系下的坐标值转换成机体坐标系下的坐标值所需的转换矩阵,并存储在矩阵BB中。
* 参数1是偏航角（z轴旋转角），参数2是俯仰角（y轴旋转角），参数3是横滚角（x轴旋转角），
* 角度单位为deg，参数4是目标矩阵。当沿着机体轴正方向观察时，旋转轴的
* 正方向为顺时针指向。
*输 入： psi,theta,phi
*输 出： BB
************************************************************************************/
void GeocentricToBodyMatrix(double psi,double theta,double phi, double BB[3][3])
{
double costheta;
double sintheta;
double cosgamma;
double singamma;
double cospsi;
double sinpsi;
costheta=cos(theta);
sintheta=sin(theta);
cosgamma=cos(phi);
singamma=sin(phi);
cospsi=cos(psi);
sinpsi=sin(psi);
BB[0][0]=costheta*cospsi;
BB[0][1]=costheta*sinpsi;
BB[0][2]=-sintheta;
BB[1][0]=sintheta*singamma*cospsi-cosgamma*sinpsi;
BB[1][1]=sintheta*singamma*sinpsi+cosgamma*cospsi;
BB[1][2]=costheta*singamma;
BB[2][0]=sintheta*cosgamma*cospsi+singamma*sinpsi;
BB[2][1]=sintheta*cosgamma*sinpsi-singamma*cospsi;
BB[2][2]=costheta*cosgamma;
}
/************************************************************************************
*函数名称: Transpose()
*功 能: 转置矩阵AA，结果存储在矩阵BB
*输 入： AA
*输 出： BB
************************************************************************************/
void Transpose(double AA[3][3], double BB[3][3])
{
for(int i=0;i<3;i++)
{
for(int j=0;j<3;j++)
{
BB[i][j]=AA[j][i];
}
}
}
/************************************************************************************
*函数名称: matrixMultiVector()
*功 能: 矩阵AA乘向量BB，结果存储在矩阵CC中
*输 入： AA, BB
*输 出： CC
************************************************************************************/
void MatrixMultiplyVector(double AA[3][3],double BB[3], double CC[3])
{
for(int i=0;i<3;i++)
{
CC[i]=AA[i][0]*BB[0]+AA[i][1]*BB[1]+AA[i][2]*BB[2];
}
}
/************************************************************************************
*函数名称: GetEulerAngle()
*功 能: 从变化矩阵AA中获取欧拉角，并将其存储在矩阵BB中，角�
