crdtran.cpp

卫星单点定位程序

#include <math.h>
#include "crdtran.h"

//1. 由笛卡尔坐标转换为大地坐标
void CartesianToGeodetic (PCRDGEODETIC pcg,		//指向所转换出的大地坐标的指针；
						  PCRDCARTESIAN pcc,	//指向待转换的笛卡尔坐标的指针；
						  double dSemiMajorAxis,//参考椭球的长半轴							
						  double dFlattening)	//参考椭球的扁率。
{
	double	ee;		//椭球的第一偏心率的平方
	double	ee1;	//椭球的第二偏心率的平方
	double	eb;		//短半轴
	double	B0;		//纬度的初始值
	double	N;		//卯酉圈半径
	double	sita;
	double	xxyy;

	//判断是否为地心
	/*if ((fabs(pcc->x) < 10e-7) && (fabs(pcc->y) < 10e-7) && (fabs(pcc->z) < 10e-7))	{
		pcg->height = 0;
		pcg->latitude = 0;
		pcg->longitude = 0;
		return;
	}*/
	ee		= dFlattening * (2 - dFlattening);
	eb		= dSemiMajorAxis * (1 - dFlattening);
	ee1		= ee / (1 - ee);
	xxyy	= pcc->x * pcc->x + pcc->y * pcc->y;
	sita	= atan2 (pcc->z * dSemiMajorAxis, sqrt(xxyy) * eb);
	pcg->longitude	= atan2 (pcc->y, pcc->x);
	if (pcg->longitude < 0) pcg->longitude += 3.141592653589793238462*2;
	B0=pcg->latitude=atan2(pcc->z+ee1*eb*pow(sin(sita),3) ,
		sqrt (xxyy) - ee * dSemiMajorAxis * pow (cos (sita), 3));
	do{
		B0				= pcg->latitude;
		N				= dSemiMajorAxis
			/ sqrt (1 - ee * sin (pcg->latitude) * sin (pcg->latitude));
		pcg->height		= sqrt (xxyy + pow (pcc->z + N * ee * sin (B0), 2)) - N;
		pcg->latitude	= atan2 (pcc->z,
			sqrt (xxyy) * (1 - ee * N / ( N + pcg->height)));
	} while (fabs(B0-pcg->latitude) > 0.0000000000000001);
}


//2. 由大地坐标转换为笛卡尔坐标
void GeodeticToCartesian (PCRDCARTESIAN pcc,	//指向所转换出的笛卡尔坐标的指针；
						  PCRDGEODETIC pcg,		//指向待转换的大地坐标的指针；
						  double dSemiMajorAxis,//参考椭球的长半轴；
						  double dFlattening)	//参考椭球的扁率。
{
	double	ee;			//椭球的第一偏心率的平方
	double	N;			//卯酉圈半径
	/*
	if ((fabs(pcg->height) < 10e-7) && (fabs(pcg->latitude) < 10e-7) 
		&& (fabs(pcg->longitude) < 10e-7))	{
		pcc->x = 0;
		pcc->y = 0;
		pcc->z = 0;
		return;
	}*/
	ee		= dFlattening * (2 - dFlattening);
	N		= dSemiMajorAxis / sqrt (1 - ee * pow (sin (pcg->latitude), 2));
	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 - ee) + pcg->height) * sin (pcg->latitude);
}

//3. 由笛卡尔坐标转换为站心地平坐标
void CartesianToTopocentric (PCRDTOPOCENTRIC pct,		//指向所转换出的站心地平坐标的指针；
							 PCRDCARTESIAN pcc,			//指向待转换的笛卡尔坐标的指针；
							 PCRDCARTESIAN pccCenter,	//指向站心的笛卡尔坐标的指针；
							 double dSemiMajorAxis,		//参考椭球的长半轴；
							 double dFlattening)		//参考椭球的扁率。
{
	CRDGEODETIC pcg,pcgCenter;
	double	dx	=pcc->x - pccCenter->x;
	double	dy	=pcc->y - pccCenter->y;
	double	dz	=pcc->z - pccCenter->z;
	CartesianToGeodetic (&pcg, pcc, dSemiMajorAxis, dFlattening);
	CartesianToGeodetic (&pcgCenter, pccCenter, dSemiMajorAxis, dFlattening);
	pct->northing	= -sin (pcg.latitude) * cos (pcg.longitude) * dx
					- sin (pcg.latitude) * sin (pcg.longitude) * dy
					+ cos (pcg.latitude) * dz;
	pct->easting	= -sin (pcg.longitude) * dx + cos (pcg.longitude) * dy;
	pct->upping		= cos (pcg.latitude) * cos (pcg.longitude) * dx
					- cos (pcg.latitude) * sin (pcg.longitude) * dy
					+ sin (pcg.latitude) * dz; 

}

//4. 由站心地平直角坐标转换为站心地平极坐标
void TopocentricToTopocentricPolar (PCRDTOPOCENTRICPOLAR pctp,	//指向所转换出的站心地平极坐标的指针；
									PCRDTOPOCENTRIC pct)		//指向待转换的站心地平坐标的指针；
{
	pctp->range		= sqrt (pow (pct->easting, 2) + pow (pct->northing, 2)
		+ pow(pct->upping, 2));
	if	(pct->northing != 0)	{
		pctp->azimuth	= radnormal (atan2 (pct->easting, pct->northing));
	}
	else	{
		pctp->azimuth	= 3.14159265358979323846;
	}
	pctp->elevation	= radnormal (asin(fabs(pct->upping) / pctp->range));
}

//将 DDD.MMSS 表示的角度化成弧度
double ang2rad (double ang)
{
	if	(ang == 0)
		return 0;
	else if	(ang > 0)	{
		int		d, m;
		double	s;
		d	= (int) (ang + 0.0000000000001);			  //加一个很小的不影响计算精度的小数，防止浮点数表示引起的舍尾问题
		m	= (int) ((ang - d) * 100+ 0.0000000000001);
		s	= (ang - d - m * 0.01) * 10000 + 0.0000000000001;
		return (d * 3600 + m * 60 + s) / 206264.80624709635515647; 
	}
	else return -ang2rad (-ang);
}

////////////////////////////////////////////////////////////////////////////////////////////
//将弧度化成 DDD.MMSS 表示的角度
double rad2ang (double rad)
{
	if	(rad == 0)
		return 0;
	int		d, m;
	double	t, s;
	t	= rad * 206264.80624709635515647; 
	d	= (long) (t + 0.0000000000001) / 3600;
	m	= (long) (t - d * 3600 + 0.0000000000001) / 60;
	s	= t - d * 3600 - m * 60;
	return d + m * 0.01 + s *0.0001;
}

////////////////////////////////////////////////////////////////////////////////////////////
//将任意一个弧度 original 标准化成 0 ~ 6.28318530717958647693 之间的数字 [0 , 2π)
double radnormal(double original)
{
	if(original==0) return 0;
	double tmp=original;
	for ( ; (tmp - 6.28318530717958647693) > 0.00000000000001;
			tmp -= 6.28318530717958647693) ;
	for ( ; tmp<0; tmp += 6.28318530717958647693) ;
	return tmp;
}