经纬高转XYZ

1.入行时候所写，留作纪念

#if 1
#include <stdio.h>
#include <math.h>
#define MAXLEN 150
#define MAXSOLLEN 8191
#define PI          3.1415926535897932  /* pi */
#define D2R         (PI/180.0)          /* deg to rad */
#define RE_WGS84    6378137.0            //长半轴
#define FE_WGS84    (1.0/298.257223563) /* 地球扁率 (WGS84) */
 
extern double str2num(const char *s, int i, int n)
{
	double value;
	char str[256], *p = str;

	if (i<0 || (int)strlen(s)<i || (int)sizeof(str)-1<n) return 0.0;
	for (s += i; *s&&--n >= 0; s++) *p++ = *s == 'd' || *s == 'D' ? 'E' : *s;
	*p = '\0';
	return sscanf(str, "%lf", &value) == 1 ? value : 0.0;
}
int blh2xyz(double *XYZ, double *BLH)
{
	double t = RE_WGS84*(1.0 - FE_WGS84);
	double e = sqrt((RE_WGS84*RE_WGS84 - t*t) / (RE_WGS84*RE_WGS84));
	double Br = BLH[0] * D2R;
	double Lr = BLH[1] * D2R;
	double N = RE_WGS84 / sqrt(1.0 - e*e*sin(Br)*sin(Br));

	XYZ[0] = (N + BLH[2]) * cos(Br)*cos(Lr);
	XYZ[1] = (N + BLH[2]) * cos(Br)*sin(Lr);
	XYZ[2] = (N*(1.0 - e*e) + BLH[2]) * sin(Br);

	return 1;
}
int outXYZ(double GPST,double *BLH, FILE *fp)
{
	double XYZ[3];
	int i,j;
	if (blh2xyz(&XYZ, BLH)>0)
	{
		fprintf(fp, "%.3lf  ", GPST);
		for (i = 0; i < 3; i++)
		{
			fprintf(fp, "%.5lf  ", XYZ[i]);
		}
		fprintf(fp, "\n");
		return 1;
	}
	return 0;
}
int exsess(char *infile, char *outfile)
{
	int i,flag=0;
	char buff[MAXLEN];
	double GPST;
	double BLH[3];
	FILE *in, *out;
	if (!(in=fopen(infile, "rb")))
	{
		printf("打开输入文件错误 ");
	}
	if (!(out = fopen(outfile, "w")))
	{
		printf("打开输出文件错误 ");
	}
	while (fgets(buff, MAXLEN, in))
	{
		if (flag == 1){
			GPST = str2num(buff, 0, 10);
			for (i = 0; i < 3; i++)
			{
				BLH[i] = str2num(buff, 61 + 15 * i, 15);
			}
			outXYZ(GPST,BLH, out);
		}
		if (strstr(buff, "(sec)")){ flag = 1; }
	}
	fclose(in); fclose(out);
	return 1;
}
int main(int argc,char **argv)
{
	char *tfile;
	char *p,name[20];
	char *outfile = {0};
	char *infile = argv[1];
	if (*argv[1])
	{
		tfile = name;
		strcpy(tfile, argv[1]);
		outfile = tfile;
		if ((p = strrchr(outfile, '\0'))) strcpy(p, ".txt");
	}
	if (exsess(infile, outfile)){ printf("转换成功！！"); return 1; }
	else { printf("转换失败！！"); return 0; }
	free(tfile);
	system("pause");
}
#endif