geo_utm.h

可以接入GPS等三个传感器,并且封装在一个组件里.上层只需拷贝几个文件,就可直接调用,组件的更新不影响上层使用

#include <math.h>

#define  AXIS        6378137
#define  K0          0.9996
#define  ECC         0.00669437999013
#define  EP2         0.006739496742227

#define  M_PI        (3.14159265358979323846) 
#define  RADDEG      (M_PI/180.0)
#define  FN	         (0.0)
#define  FS	         (10000000.0)
#define  FE	         (500000.0)


double compute(double ecc,double rphi, double sphi, double cphi)
{
	double A0, A2, A4, A6;
	double e1, e2, e3;
	double result;
	e1 = ecc;
	e2 = ecc*ecc;
	e3 = e2*e1;
	A0 = 1.0 - e1/4.0 - 3.0*e2/64.0 - 5.0*e3/256.0;
	A2 = 3.0*e1/8.0 + 3.0*e2/32.0 + 45.0*e3/1024.0;
	A4 = 15.0*e2/256.0 + 45.0*e3/1024.0;
	A6 = 35.0*e3/3072.0;
	double sphi2 = 2 * sphi * cphi;
	double sphi3 = 3 * sphi - 4 * sphi * sphi * sphi;
	double cphi2 = cphi*cphi - sphi*sphi;
	double cphi3 = 4 * cphi * cphi * cphi - 3 * cphi;
	double sphi4 = 2 * sphi2 * cphi2;
	double sphi6 = 2 * sphi3 * cphi3;
	result = A0*rphi - A2*sphi2 + A4*sphi4 - A6*sphi6;
	return result;
}


int geo_to_transverse_mercator(float lat,float lon, double *x, double *y )
{
	    double T1, T2, T3;
		double rphi = lat*RADDEG, rlam = lon*RADDEG;// dlam = rlam;
		//double dlam2 = dlam*dlam;
		double sphi = sin(rphi), cphi = cos(rphi), tphi = tan(rphi);
		double sp2 = sphi*sphi, cp2 = cphi*cphi;
		double A = rlam * cphi;
		double A2 = A*A;
		double A4 = A2*A2;
		double T = tphi*tphi;
		double C = EP2*cp2;
		double RN = AXIS / sqrt(1 - sp2 * ECC);
		double N = K0*RN;
		//double Q = 0.5*sphi*cphi*N;
		//double T1 = meridian_distance( rphi,sphi, MDCFS )*K0*AXIS
		T1 = (1.0 - T + C) * A2 * A / 6.0;
		T2 = (5.0 - 18.0*T + T*T + 72.0*C - 58.0*EP2)*A4*A/120.0;
		*x = N * (A + T1 + T2);
		double M = AXIS * compute(ECC, rphi, sphi, cphi);
		T1 = (5.0 - T + 9.0*C + 4.0*C*C)*A4/24.0;
		T2 = (61.0 - 58.0*T + T + 600.0*C - 330.0*EP2)*A4*A2/720.0;
		T3 = A2/2.0 + T1 + T2;
		*y = K0 * (M + RN * tphi * T3);
		return 1;
}



short geo_to_utm(float lat, float lon, long *izone, double *east,double *north )
{
	    //const proj_dfn* cnsts = (proj_dfn*)tcnsts;	
		short status;
		float lam0;
		long zone = *izone;
		/*if(zone < 31)
			lam0 = 6 * zone - 183.0;
		else if(zone > 30)
			lam0 = 6 * zone + 177;*/
		
		if( zone <= 0 || zone > 60 )	// if a zone is provided use it, otherwise compute it. 
		{	zone = (long)(31.0 + lon / 6.0);
			if(zone >= 61) zone = 60;	
    		if(zone <= 0) zone = 1;
    		*izone = zone;
		}
		lam0 = zone * 6 - 183.0;
		lon = lon - lam0;
    	//lon -= (zone*6-183);		   // Change the longitude to offset 
		status = geo_to_transverse_mercator(lat,lon, east, north );
		*east += FE;
		if( *north < 0.0 ) *north = -(*north + FS);
		return status;
}