getgpsdotspace.c

得到GPS两点之间的距离,对于初步的应用是足够的

/******************************************************************************
得到GPS两点之间的距离,对于初步的应用是足够的，直接用圆的周长知识.
******************************************************************************/


/******************************************************************************
1.原理 
因为排版的原因，用pow(X,Y)表示X的Y次幂，用B[0]，B[1]，...，B[m-1]表示一个序列， 
其中[x]为下标。 

假设： 
B[x],b[x]都是二进制序列,取值0或1。 
M = B[m-1]*pow(2,m-1) + B[m-2]*pow(2,m-2) + ... + B[1]*pow(2,1) + B[0]*pow(2,0) 
N = b[n-1]*pow(2,n-1) + b[n-2]*pow(2,n-2) + ... + b[1]*pow(2,1) + n[0]*pow(2,0) 
pow(N,2) = M 

(1) N的最高位b[n-1]可以根据M的最高位B[m-1]直接求得。 
设 m 已知,因为 pow(2, m-1) <= M <= pow(2, m)，所以 pow(2, (m-1)/2) <= N <= pow(2, m/2) 
如果 m 是奇数，设m=2*k+1, 
那么 pow(2,k) <= N < pow(2, 1/2+k) < pow(2, k+1), n-1=k, n=k+1=(m+1)/2 
如果 m 是偶数，设m=2k, 
那么 pow(2,k) > N >= pow(2, k-1/2) > pow(2, k-1), n-1=k-1,n=k=m/2 
所以b[n-1]完全由B[m-1]决定。 
余数 M[1] = M - b[n-1]*pow(2, 2*n-2) 

(2) N的次高位b[n-2]可以采用试探法来确定。 
因为b[n-1]=1，假设b[n-2]=1，则 pow(b[n-1]*pow(2,n-1) + b[n-1]*pow(2,n-2), 
2) = b[n-1]*pow(2,2*n-2) + (b[n-1]*pow(2,2*n-2) + b[n-2]*pow(2,2*n-4)), 
然后比较余数M[1]是否大于等于 (pow(2,2)*b[n-1] + b[n-2]) * pow(2,2*n-4)。这种 
比较只须根据B[m-1]、B[m-2]、...、B[2*n-4]便可做出判断，其余低位不做比较。 
若 M[1] >= (pow(2,2)*b[n-1] + b[n-2]) * pow(2,2*n-4), 则假设有效，b[n-2] = 1； 
余数 M[2] = M[1] - pow(pow(2,n-1)*b[n-1] + pow(2,n-2)*b[n-2], 2) = M[1] - (pow(2,2)+1)*pow(2,2*n-4)； 
若 M[1] < (pow(2,2)*b[n-1] + b[n-2]) * pow(2,2*n-4), 则假设无效，b[n-2] = 0；余数 M[2] = M[1]。 

(3) 同理，可以从高位到低位逐位求出M的平方根N的各位。 

使用这种算法计算32位数的平方根时最多只须比较16次，而且每次比较时不必把M的各位逐 
一比较，尤其是开始时比较的位数很少，所以消耗的时间远低于牛顿迭代法。 

2. 流程图 
(制作中，稍候再上) 

3. 实现代码 
这里给出实现32位无符号整数开方得到16位无符号整数的C语言代码。 
******************************************************************************/

/****************************************/ 
/*Function: 开根号处理 */ 
/*入口参数：被开方数，长整型 */ 
/*出口参数：开方结果，整型 */ 
/****************************************/ 
UINT16 sqrt_16(UINT32 M)
{
	UINT16 N,  i;
	UINT32 tmp, ttp; // 结果、循环计数
	
	if (M == 0) // 被开方数，开方结果也为0
		return 0;
	
	N = 0;
	
	tmp = (M >> 30); // 获取最高位：B[m-1]
	M <<= 2;
	if (tmp > 1) // 最高位为1
	{
		N ++; // 结果当前位为1，否则为默认的0
		tmp -= N;
	}
	
	for (i=15; i>0; i--) // 求剩余的15位
	{
		N <<= 1; // 左移一位
		
		tmp <<= 2;
		tmp += (M >> 30); // 假设
		
		ttp = N;
		ttp = (ttp<<1)+1;
		
		M <<= 2;
		if (tmp >= ttp) // 假设成立
		{
			tmp -= ttp;
			N ++;
		}
	}
	return N;
} 


#ifndef PI
#define PI 3.14159265;
#endif
static UINT32 Rc = 6378137;  // 赤道半径
static UINT32 Rj = 6356725;  // 极半径 

// #define SPACE_SEC	310
const float_type SPACE_SEC = {31, 0, 0, {0, 0}};

UINT32 GetGPSDotSpace(char *pLat1, char *Long1, char *pLat2, char *Long2)
{
	float_type PDot1, LDot1, PDot2, LDot2;
	float_type DeltaPDot, DeltaLDot;
	float_type float_op, float_result;
	
	UINT32 dd, mm, ss, ss2;
	
	INT8 *pNumber;
	UINT32 Number;
	INT8 StrNumber1[16];
	INT8 StrNumber2[16];
	
	// ddmm.mmmm
	dd = (UINT32)(pLat1[0]-'0')*10 + (UINT32)(pLat1[1]-'0');
	mm = (UINT32)(pLat1[2]-'0')*10 + (UINT32)(pLat1[3]-'0');
	ss2 = (UINT32)(pLat1[5]-'0')*1000 + (UINT32)(pLat1[6]-'0')*100 + (UINT32)(pLat1[7]-'0')*10 + (UINT32)(pLat1[8]-'0');
	ss2 = ss2 * 60;
	ss = dd*3600+mm*60+ss2/10000;
	ss2 = ss2 %10000;
	memset(StrNumber1, 0, sizeof(StrNumber1));
	memset(StrNumber2, 0, sizeof(StrNumber2));
	sprintf(StrNumber1, "%d", ss);
	sprintf(StrNumber2, "%d", ss2);
	if(ss2>=1000)
		strcat(StrNumber1, ".");
	else if(ss2>=100)
		strcat(StrNumber1, ".0");
	else if(ss2>=10)
		strcat(StrNumber1, ".00");
	else if(ss2>=1)
		strcat(StrNumber1, ".000");
	else
		StrNumber2[0] = 0;
	strcat(StrNumber1, StrNumber2);
	PDot1 = str2fp(StrNumber1);
	
	dd = (UINT32)(pLat2[0]-'0')*10 + (UINT32)(pLat2[1]-'0');
	mm = (UINT32)(pLat2[2]-'0')*10 + (UINT32)(pLat2[3]-'0');
	ss2 = (UINT32)(pLat2[5]-'0')*1000 + (UINT32)(pLat2[6]-'0')*100 + (UINT32)(pLat2[7]-'0')*10 + (UINT32)(pLat2[8]-'0');
	ss2 = ss2 * 60;
	ss = dd*3600+mm*60+ss2/10000;
	ss2 = ss2 %10000;
	memset(StrNumber1, 0, sizeof(StrNumber1));
	memset(StrNumber2, 0, sizeof(StrNumber2));
	sprintf(StrNumber1, "%d", ss);
	sprintf(StrNumber2, "%d", ss2);
	if(ss2>=1000)
		strcat(StrNumber1, ".");
	else if(ss2>=100)
		strcat(StrNumber1, ".0");
	else if(ss2>=10)
		strcat(StrNumber1, ".00");
	else if(ss2>=1)
		strcat(StrNumber1, ".000");
	else
		StrNumber2[0] = 0;
	strcat(StrNumber1, StrNumber2);
	PDot2 = str2fp(StrNumber1);
	
	// dddmm.mmmm
	dd = (UINT32)(Long1[0]-'0')*100 + (UINT32)(Long1[1]-'0')*10 + (UINT32)(Long1[2]-'0');
	mm = (UINT32)(Long1[3]-'0')*10 + (UINT32)(Long1[4]-'0');
	ss2 = (UINT32)(Long1[6]-'0')*1000 + (UINT32)(Long1[7]-'0')*100 + (UINT32)(Long1[8]-'0')*10 + (UINT32)(Long1[9]-'0');
	ss2 = ss2 * 60;
	ss = dd*3600+mm*60+ss2/10000;
	ss2 = ss2 %10000;
	memset(StrNumber1, 0, sizeof(StrNumber1));
	memset(StrNumber2, 0, sizeof(StrNumber2));
	sprintf(StrNumber1, "%d", ss);
	sprintf(StrNumber2, "%d", ss2);
	if(ss2>=1000)
		strcat(StrNumber1, ".");
	else if(ss2>=100)
		strcat(StrNumber1, ".0");
	else if(ss2>=10)
		strcat(StrNumber1, ".00");
	else if(ss2>=1)
		strcat(StrNumber1, ".000");
	else
		StrNumber2[0] = 0;
	strcat(StrNumber1, StrNumber2);
	LDot1 = str2fp(StrNumber1);
	
	dd = (UINT32)(Long2[0]-'0')*100 + (UINT32)(Long2[1]-'0')*10 + (UINT32)(Long2[2]-'0');
	mm = (UINT32)(Long2[3]-'0')*10 + (UINT32)(Long2[4]-'0');
	ss2 = (UINT32)(Long2[6]-'0')*1000 + (UINT32)(Long2[7]-'0')*100 + (UINT32)(Long2[8]-'0')*10 + (UINT32)(Long2[9]-'0');
	ss2 = ss2 * 60;
	ss = dd*3600+mm*60+ss2/10000;
	ss2 = ss2 %10000;
	memset(StrNumber1, 0, sizeof(StrNumber1));
	memset(StrNumber2, 0, sizeof(StrNumber2));
	sprintf(StrNumber1, "%d", ss);
	sprintf(StrNumber2, "%d", ss2);
	if(ss2>=1000)
		strcat(StrNumber1, ".");
	else if(ss2>=100)
		strcat(StrNumber1, ".0");
	else if(ss2>=10)
		strcat(StrNumber1, ".00");
	else if(ss2>=1)
		strcat(StrNumber1, ".000");
	else
		StrNumber2[0] = 0;
	strcat(StrNumber1, StrNumber2);
	LDot2 = str2fp(StrNumber1);
	
	DeltaPDot = fp_sub(PDot1, PDot2);
	if(DeltaPDot.sign)
		DeltaPDot = fp_sub(PDot2, PDot1);
	
	DeltaLDot = fp_sub(LDot1, LDot2);
	if(DeltaLDot.sign)
		DeltaLDot = fp_sub(LDot2, LDot1);
	
	DeltaPDot = fp_mul(DeltaPDot, SPACE_SEC);
	DeltaLDot = fp_mul(DeltaLDot, SPACE_SEC);
	float_op = fp_add( fp_mul(DeltaPDot, DeltaPDot), fp_mul(DeltaLDot, DeltaLDot) );
	float_result = fp_sqrt(float_op);
	
	pNumber = fp2str(float_result, FALSE);
	Number = atoi(pNumber);
	
	return Number;
}