#include "CoordCovert.h"
#include "math.h"

void CartesianToGeodetic (PCRDGEODETIC pcg, PCRDCARTESIAN pcc,
double dSemiMajorAxis, double dFlattening)
{
	double e2;//第一偏心率的平方
	e2=2*dFlattening-dFlattening*dFlattening;

	pcg->longitude=atan(pcc->y/pcc->x);
	double W,N,N1=0,B,B1;
	B1=atan(pcc->z/sqrt(pcc->x*pcc->x+pcc->y*pcc->y));
	while(1)
	{   
		W=sqrt(1-e2*sin(B1)*sin(B1));
		N1=dSemiMajorAxis/W;
		B=atan((pcc->z+N1*e2*sin(B1))/sqrt(pcc->x*pcc->x+pcc->y*pcc->y));

			if(fabs(B-B1)<delta)
				break;
			else
				B1=B;
	}

	pcg->latitude=B;
	N=dSemiMajorAxis/sqrt(1-e2*sin(pcg->latitude)*sin(pcg->latitude));
	pcg->height=sqrt(pcc->x*pcc->x+pcc->y*pcc->y)/cos(B)-N;
    

}

//由大地坐標轉換為笛卡爾坐標
void GeodeticToCartesian (PCRDCARTESIAN pcc, PCRDGEODETIC pcg,
double dSemiMajorAxis, double dFlattening)
{   
	double e2;//第一偏心率的平方
	double N;//卯酉圈半徑
	e2=2*dFlattening-dFlattening*dFlattening;
	N=dSemiMajorAxis/sqrt(1-e2*sin(pcg->latitude)*sin(pcg->latitude));

	pcc->x=(N+pcg->height)*cos(pcg->latitude)*cos(pcg->longitude);
	pcc->y=(N+pcg->height)*cos(pcg->latitude)*sin(pcg->longitude);
	pcc->z=(N*(1-e2)+pcg->height)*sin(pcg->latitude);

}

//由笛卡爾坐標轉換為站心地平坐標
void CartesianToTopocentric (PCRDTOPOCENTRIC pct,
PCRDCARTESIAN pcc,
PCRDCARTESIAN pccCenter,
double dSemiMajorAxis,
double dFlattening)
{
	double dx,dy,dz;
	dx=pcc->x-pccCenter->x;
	dy=pcc->y-pccCenter->y;
	dz=pcc->z-pccCenter->z;

	PCRDGEODETIC pd;
	pd=(PCRDGEODETIC)malloc(sizeof(CRDGEODETIC));

    CartesianToGeodetic (pd,pccCenter,dSemiMajorAxis,dFlattening);

	pct->northing=-sin(pd->latitude)*cos(pd->longitude)*dx
		-sin(pd->latitude)*sin(pd->longitude)*dy
		+cos(pd->latitude)*dz;
	pct->easting=-sin(pd->longitude)*dx
		+cos(pd->longitude)*dy;
	pct->upping=cos(pd->latitude)*cos(pd->longitude)*dx
		+cos(pd->latitude)*sin(pd->longitude)*dy
		+sin(pd->latitude)*dz;
	free(pd);

}

//由站心地平直角坐標轉換為站心地平極坐標
void TopocentricToTopocentricPolar (PCRDTOPOCENTRICPOLAR pctp,
PCRDTOPOCENTRIC pct)
{   

	pctp->range=sqrt(pct->northing*pct->northing+pct->easting*pct->easting+pct->upping*pct->upping);
	pctp->azimuth=atan(pct->easting/pct->northing);
	pctp->elevation=asin(pct->upping/pctp->range);


}

//由站心地平極坐標轉換為站心地平直角坐標
void TopocentricPolarToTopocentric (PCRDTOPOCENTRIC pct,
PCRDTOPOCENTRICPOLAR pctp)
{
	pct->northing=pctp->range*cos(pctp->elevation)*cos(pctp->azimuth);
	pct->easting=pctp->range*cos(pctp->elevation)*sin(pctp->azimuth);
	pct->upping=pctp->range*sin(pctp->elevation);

}