server/locate_algorithm.cpp

#include "stdafx.h"
#include "locate_algorithm.h"
#include <math.h>
#include <algorithm>
#include "../common/matrix/_Matrix.h"
#include "../common/Functions/Functions.h"
using namespace std;

// ant 天线， tar定位目标， d 测距
void AOAformula( struct _coordinate* ant, double angle, struct _coordinate* tar, bool is_line /*= false*/, double z_offset /*= 0*/ )
{
	double aa = sin(angle);
	double bb = cos(angle);
	double h = ant->z;
	if(is_line){
		h = z_offset;
	}
	tar->x = ant->x + sqrt(ant->d * ant->d - h * h) * cos(angle); 
	tar->y = ant->y + sqrt(ant->d * ant->d - h * h) * sin(angle);
	tar->z = ant->z; // 高度
	tar->a = ant->a; // 角度
	tar->t = ant->t; // 时间戳
	tar->d_offset = ant->d_offset;
	tar->reader_id = ant->reader_id;
	tar->d = ant->d;
}

// ant 天线， last 上次坐标
double Distanceformula( struct _coordinate* ant, struct _coordinate* last )
{
	double x0,y0;
	double tan_diff = tan(last->a) - tan(ant->a);
	double ret = 0;
	if(tan_diff == 0){
		ret = sqrt((ant->x - last->x) * (ant->x - last->x) +  (ant->y - last->y) * (ant->y - last->y));
	}else{
		x0 = (last->x * tan(last->a) - ant->x * tan(ant->a) + ant->y - last->y) / (tan(last->a) - tan(ant->a));
		y0 = (((last->x - ant->x) * tan(last->a) * tan(ant->a)) + ant->y * tan(last->a) - last->y * tan(ant->a)) / (tan(last->a) - tan(ant->a));
		ret = sqrt((x0 - ant->x) * (x0 - ant->x) + (y0 - ant->y) * (y0 - ant->y)) 
			+ sqrt((x0 - last->x) * (x0 - last->x) + (y0 - last->y) * (y0 - last->y));
	}
	// n = Nodeformula( n1, n2);
	return ret;
}

// 接收到的新数据
void algorithm_locate( struct _coordinate** coor_list, int coor_size, struct _coordinate* hist, struct _coordinate* dest, double dist_limit, double z_offset)
{
	double d0;  //历史定位坐标与实时定位坐标之间的距离；
	double dN;  //利用速率V和时间戳理论计算出行进距离；
	double Angle1,Angle2,Angle3;  //三个坐标时，任何2坐标的角度
	double Angle;   //两个坐标之间的角度
	double dLong;
	double dLenth;
	int position;  //定位点位置

	switch(coor_size){		
	case 1: // 1条定位数据，5.3.1
		{
			if(coor_list[0]->d > dist_limit){ // 如果测距的距离大于某个固定值,此时判断此位置与天线角度一致
				//coor_list[0]->d -= coor_list[0]->d_offset;
				AOAformula(coor_list[0], coor_list[0]->a, dest, true, z_offset);
			}else { // 测距距离小于某值
				//利用速率V和时间戳理论计算出行进距离
				dN = coor_list[0]->v * (coor_list[0]->t - hist->t);
				//计算实时定位分站（天线）坐标和历史定位坐标之间的距离
				d0 = Distanceformula(coor_list[0], hist);
				if(hist->x - coor_list[0]->x == 0){
					Angle = (coor_list[0]->y >= hist->y) ? 90 : 270;
					Angle = Angle * M_PI / 180;
				}else{
					//计算历史坐标与实时分站坐标的角度，用来表明历史坐标位于定位坐标的方向
					Angle = atan((hist->y - coor_list[0]->y)/(hist->x - coor_list[0]->x));
				}

				// 与历史坐标同向		
				//如果定位距离小于两个坐标之间的距离，说明定位位置在两个坐标之内
				if(d0 >= coor_list[0]->d){
					if(dN >= coor_list[0]->d){
						Angle += M_PI;  //与历史坐标反方向
					}
				}else{  
					//如果定位距离大于两个坐标之间的距离，说明定位位置在两个坐标之外,接下来只要判断在哪个yu坐标的位置即可
					//如果理论进行距离大于两者坐标之间的距离，则说明与历史坐标在同一方向，反之在反方向
					if(dN < coor_list[0]->d){ 
						Angle += M_PI;  //与历史坐标反方向
					}
				}
				//coor_list[0]->d -= coor_list[0]->d_offset;
				AOAformula(coor_list[0], Angle, dest, true, z_offset);  //最终定位坐标计算
			}
			break;
		}
	case 2: //2条定位数据，5.3.2
		{
			//2条定位数据中分站(天线)坐标与历史定位坐标之间的角度，以此推出这三个坐标是否在一条直线上或者呈三角形
			if(coor_list[0]->x == hist->x ){
				Angle1 = (coor_list[0]->y == hist->y) ? 0 : ((coor_list[0]->y > hist->y) ? 90 : 270);
				Angle1 = Angle1 * M_PI / 180;
			}else{
				Angle1 = atan((coor_list[0]->y - hist->y) / (coor_list[0]->x - hist->x)); 
			}
			if(coor_list[1]->x == hist->x ){
				Angle2 = (coor_list[1]->y == hist->y) ? 0 : ((coor_list[1]->y > hist->y)? 90 : 270);
				Angle2 = Angle2 * M_PI / 180;
			}else{
				Angle2 = atan((coor_list[1]->y - hist->y) / (coor_list[1]->x - hist->x)); 
			}
			if(coor_list[1]->x == coor_list[0]->x ){
				Angle3 = (coor_list[1]->y >= coor_list[0]->y) ? 90 : 270; // 必须在不同点
				Angle3 = Angle3 * M_PI / 180;
			}else{
				Angle3 = atan((coor_list[1]->y - coor_list[0]->y) / (coor_list[1]->x - coor_list[0]->x)); 
				if(Angle3 > M_PI){
					Angle3 -= M_PI;
				}
				/*if(coor_list[0]->x > coor_list[1]->x)
				Angle3 += M_PI;*/
			}
			//2个角度相同或者相差180度说明在一条直线上
			//if(coor_list[0]->a == coor_list[1]->a || coor_list[0]->a == coor_list[1]->a + M_PI || coor_list[0]->a == coor_list[1]->a - M_PI){
			if(Angle1 == Angle2 || Angle1 == Angle2 + M_PI || Angle1 == Angle2 - M_PI){
				//取两个距离之间的最大值,保存最短距离的数据作为定位点
				dLong = max(coor_list[0]->d, coor_list[1]->d);
				position = (coor_list[0]->d > coor_list[1]->d) ? 1 : 0;
				dLenth = Distanceformula(coor_list[0], coor_list[1]);
				if(dLong > dLenth){
					Angle = Angle3 + ((position + 1)%2) * M_PI; 
				}else{
					Angle = Angle3 +  position * M_PI;
				}
				//2个坐标的距离小于其2个距离中的最大值，则说明定位点在2个坐标的外侧，
				//且在距离短的那条作为定位坐标，定位角度与此相同
				//2个坐标的距离大于这两条距离中的任何一个，则说明定位点在两个坐标之间，取任意一条作为定位点,定位角度与其角度相反（+180）
				// AOAformula(coor_list[position], Angle, dest);  //最终定位坐标计算

				// 根据测量的结果，调整距离d 
				// x2 - (d2 - ((d1+d2) - (x2-x1))/2) 靠近x2  x2 > x1,d1 > d2
				// x1 + (d1 - ((d1+d2) - (x2-x1))/2) 靠近x1  x2 > x1,d1 < d2
				// 化简得到 (x1 + x2 + d1 - d2)/2
				// 显示的d = ((d1 + d2) - abs(x2-x1))/2
				double d_offset;
				if(fabs(coor_list[0]->x - coor_list[1]->x) > fabs(coor_list[0]->y - coor_list[1]->y)){ // 水平方向
					d_offset = (coor_list[0]->d + coor_list[1]->d - fabs(coor_list[0]->x - coor_list[1]->x))/2;
				}else{ // 垂直方向
					d_offset = (coor_list[0]->d + coor_list[1]->d - fabs(coor_list[0]->y - coor_list[1]->y))/2;
				}
				dest->d_offset = d_offset;
				coor_list[0]->d_offset = d_offset;
				coor_list[1]->d_offset = d_offset;
				coor_list[position]->d -= d_offset;
				AOAformula(coor_list[position], Angle, dest, true, z_offset);
			}
			//岔路口形式
			else {
				dLenth = Distanceformula(coor_list[0], coor_list[1]); //2个天线坐标之间的距离
				if(dLenth > (coor_list[0]->d  + coor_list[1]->d)) { //取一条作为定位参考数据，采用1条定位数据算法,参考5.3.1
					//如果测距的距离大于某个固定值,此时判断此位置与天线角度一致，
					if(coor_list[0]->d > dist_limit){
						AOAformula(coor_list[0], coor_list[0]->a, dest);  //最终定位坐标计算
					}
					//测距距离小于某值
					else{
						dN = coor_list[0]->v * ( hist->t - coor_list[0]->t);  //利用速率V和时间戳理论计算出行进距离；
						d0 = Distanceformula(coor_list[0], hist);  //计算实时定位分站（天线）坐标和历史定位坐标之间的距离
						if(hist->x == coor_list[0]->x){
							Angle = (hist->y >= coor_list[0]->y) ? 90 : 270; //计算历史坐标与实时分站坐标的角度，用来表明历史坐标位于定位坐标的方向
							Angle = Angle * M_PI / 180;
						}else{
							Angle = atan((hist->y - coor_list[0]->y) / (hist->x - coor_list[0]->x));
						}

						//如果定位距离小于两个坐标之间的距离，说明定位位置在两个坐标之内
						if(d0 > coor_list[0]->d){
							if(dN >= coor_list[0]->d){
								Angle += M_PI; //与历史坐标反方向
							}
						}
						else{
							//如果定位距离大于两个坐标之间的距离，说明定位位置在两个坐标之外,接下来只要判断在哪个坐标的位置即可
							//如果理论进行距离大于两者坐标之间的距离，则说明与历史坐标在同一方向，反之在反方向
							if(dN < coor_list[0]->d){
								Angle += M_PI;
							}
						}
						AOAformula(coor_list[0], Angle, dest, true, z_offset);  //最终定位坐标计算
					}
				}
				else { //采用2条定位数据算法，采用5.3.2				
					//取两个距离之间的最大值,保存最短距离的数据作为定位点
					dLong = max(coor_list[0]->d, coor_list[1]->d);
					position = (coor_list[0]->d > coor_list[1]->d) ? 1 : 0;
					dLenth = Distanceformula(coor_list[0], coor_list[1]);
					//2个坐标的距离小于其2个距离中的最大值，则说明定位点在2个坐标的外侧，
					//且在距离短的那条作为定位坐标，定位角度与此相同
					//2个坐标的距离大于这两条距离中的任何一个，则说明定位点在两个坐标之间，取任意一条作为定位点,定位角度与其角度相反
					if(dLong > dLenth){
						Angle = Angle3 + ((position + 1)%2) * M_PI; 
					}else{
						Angle = Angle3 +  position * M_PI;
					}
					AOAformula(coor_list[position], Angle, dest, true, z_offset);  //最终定位坐标计算
				}
			}
			break;
		}
	case 3: // 3条定位数据，三点定位法 
		{
			// 在同一直线上不能使用三点定位
			locatebycordinate(coor_list, dest);

			break;
		}
	case 4: // 4条数据，取前3条
		{
			locatebycordinate(coor_list, dest);
			break;
		}
	default:
		{
			dest->x = -1;
			dest->y = -1;
			dest->a = 0;
			break;
		}
	}
}

void locatebycordinate( struct _coordinate** coor_list, struct _coordinate* dest )
{
	double h = 0;
	int i = 0;
	int j = 0;
	double temp = 0;

	double x[3]; // 坐标
	double y[3]; // 坐标
	double z[3];
	double d[3];
	double p[2]; 
	_Matrix pi;

	matrix_set(&pi,3,2);
	matrix_init(&pi);

	for(i=0;i<3;i++){
		x[i] = coor_list[i]->x;
		y[i] = coor_list[i]->y;
		z[i] = coor_list[i]->z;
		d[i] = coor_list[i]->d; // d * CM_PIX * 100;
	}
	//获得坐标
	matrix_write(&pi,0,0,x[0]);
	matrix_write(&pi,0,1,y[0]);
	h = z[0];
	d[0] = sqrt(d[0] * d[0] - h * h);

	matrix_write(&pi,1,0,x[1]);
	matrix_write(&pi,1,1,y[1]);
	h = z[1];
	d[1] = sqrt(d[1] * d[1]- h * h);

	matrix_write(&pi,2,0,x[2]);
	matrix_write(&pi,2,1,y[2]);
	h = z[2];
	d[2] = sqrt(d[2] * d[2] - h * h);

	if (locate(&pi,d,p) > 0)
	{
		dest->x = p[0];
		dest->y = p[1];
	}else
	{
		dest->x = -1;
		dest->y = -1;
	}
}

void algorithm_locate_ex( _coordinate** coor_list, int coor_size, _coordinate* hist, _coordinate* dest, double dist_limit )
{
	switch (coor_size)
	{
	case 1:
		{
			if(!hist){ // 初次计算，只有一个分站数据的时候跳出，不计算坐标
				return;
			}
			// 获取历史节点与当前分站的角度，判断是否仍在同一直线
			double angle = getAngleByAngle(coor_list[0], hist);
			if(0 == tan(angle)){ // 在同一直线
				double dist_temp = getDistance(hist, coor_list[0]); // 获得上一次的距离
				// 获得移动的距离
				double dist_mv = fabs(dist_temp - coor_list[0]->d);
				// 沿着原移动方向运动
				calcCoordinate(hist, dist_mv, hist->a, dest); //
			}else{ // 有角度，找交点
				_coordinate* pt_cross = getCross(hist, coor_list[0]);
				double dist_temp = getDistance(pt_cross, coor_list[0]);
				if(dist_temp >= coor_list[0]->d){
					// 避免动画穿墙，先将该点定位到交点上
					dest->x = pt_cross->x;
					dest->y = pt_cross->y;
					dest->z = coor_list[0]->z;
					dest->a = getAngleByCoordinate(pt_cross, coor_list[0]); // 运行方向
				}else{ // 在当前方向上运行, 理论上不存在
					dist_temp = (getDistance(hist, pt_cross) - (coor_list[0]->d - dist_temp)); // 位移
					calcCoordinate(hist, dist_temp, hist->a, dest);
				}
			}
			break;
		}
	case 2:
		{
			double angle_anc = getAngleByAngle(coor_list[0], coor_list[1]);
			if(0 == tan(angle_anc)){ // 在同一直线
				double dist_anc = getDistance(coor_list[0], coor_list[1]);
				double dist_offset;
				if(dist_anc < coor_list[0]->d || dist_anc < coor_list[1]->d ) {//分站到标识卡的距离大于分站之间的距离，同一侧
					dist_offset = fabs(coor_list[0]->d - coor_list[1]->d) - dist_anc;
					double angle_anc_run = getAngleByCoordinate(coor_list[0], coor_list[1]); // 分站2一侧
					if(coor_list[0]->d < coor_list[1]->d){ // 分站1一侧
						angle_anc_run += M_PI;
					}
					calcCoordinate(coor_list[0], coor_list[0]->d - dist_offset / 2, angle_anc_run, dest);
				}else{ //在分站之间
					dist_offset = coor_list[0]->d + coor_list[1]->d - dist_anc;
					calcCoordinate(coor_list[0], coor_list[0]->d + dist_offset / 2, getAngleByCoordinate(coor_list[0], coor_list[1]), dest);
				}
			}else{ // 有夹角, 一定在两分站之间
				if(hist){
					double angle_anc1 = getAngleByAngle(coor_list[0], hist);
					double angle_anc2 = getAngleByAngle(coor_list[1], hist);
					// 找到交点
					_coordinate* pt_cross = getCross(coor_list[0], coor_list[1]);
					// 交点到分站的距离
					double dist_anc1 = getDistance(pt_cross, coor_list[0]);
					double dist_anc2 = getDistance(pt_cross, coor_list[1]);

					if(0 == tan(angle_anc1)){
						if(dist_anc1 >= coor_list[0]->d){ // 以直线上为准
							calcCoordinate(hist, fabs(dist_anc1 - coor_list[0]->d), hist->a, dest);
							//calcCoordinate(coor_list[0], coor_list[0]->d, getAngleByCoordinate(coor_list[0], hist), dest);
						}else{ // 超过直接定位到交点上
							dest->x = pt_cross->x;
							dest->y = pt_cross->y;
							dest->a = getAngleByCoordinate(pt_cross, coor_list[1]);
						}
					}else if(0 == tan(angle_anc2)){ // 与分站1在同一直线
						if(dist_anc2 >= coor_list[1]->d){ // 以直线上为准
							calcCoordinate(hist, fabs(dist_anc2 - coor_list[1]->d), hist->a, dest);
							//calcCoordinate(coor_list[1], coor_list[1]->d, getAngleByCoordinate(coor_list[1], hist), dest);
						}else{ // 超过直接定位到交点上
							dest->x = pt_cross->x;
							dest->y = pt_cross->y;
							dest->a = getAngleByCoordinate(pt_cross, coor_list[0]);
						}
					}else { // 三叉口
						// 找到交点
						_coordinate* pt_cross1 = getCross(hist, coor_list[0]);
						_coordinate* pt_cross2 = getCross(hist, coor_list[1]);

						if(coor_list[0]->d >= coor_list[1]->d){
							dest->x = pt_cross2->x;
							dest->y = pt_cross2->y;							
							dest->z = coor_list[1]->z;
							dest->a = coor_list[1]->a;
						}else{
							dest->x = pt_cross1->x;
							dest->y = pt_cross1->y;							
							dest->z = coor_list[0]->z;
							dest->a = coor_list[0]->a;
						}
					}
				}else{ // 先定位到交点,避免穿墙
					_coordinate* pt_cross = getCross(coor_list[0], coor_list[1]);
					dest->x = pt_cross->x;
					dest->y = pt_cross->y;
					// 比较距离,定位到距离近的方向上
					if(fabs(getDistance(pt_cross, coor_list[0]) - coor_list[0]->d) >= fabs(getDistance(pt_cross, coor_list[1]) - coor_list[1]->d)){
						dest->z = coor_list[1]->z;
						dest->a = coor_list[1]->a;
					}else{
						dest->z = coor_list[0]->z;
						dest->a = coor_list[0]->a;
					}
				}
			}
			break;
		}
	case 3:
		{
			// 判断分站是否在同一直线上
			if(is_all_inline(coor_list, coor_size)){
				algorithm_locate_ex(coor_list, 2, hist, dest, dist_limit);
				break;
			}
			locatebycordinate(coor_list, dest);
			break;
		}
	case 4:
		{
			// 判断分站是否在同一直线上
			if(is_all_inline(coor_list, coor_size)){
				algorithm_locate_ex(coor_list, 2, hist, dest, dist_limit);
				break;
			}
			locatebycordinate(coor_list, dest);
			break;
		}
	default:
		break;
	}
}

double getAngleByAngle( _coordinate* p1, _coordinate* p2 )
{
	if(p1->x == p2->x) return 0;
	if(p1->y == p2->y) return 0;
	return double(p2->a - p1->a);
}

double getAngleByCoordinate( _coordinate* p1, _coordinate* p2 )
{
	if(p1->x == p2->x && p1->y == p2->y){ // 如果坐标相等，则跨越，不考虑折返情况
		return p1->a;
	}
	if(p1->x == p2->x){
		if(p1->y >= p2->y){
			return M_PI_2 * 3;
		}
		return M_PI_2;
	}
	if(p1->y == p2->y){
		if(p1->x > p2->x){
			return M_PI;
		}
		return 0;
	}
	return atan((p2->y - p1->y) / (p2->x - p1->x));
}

_coordinate* getCross( _coordinate* p1, _coordinate* p2 )
{
	_coordinate* p = new _coordinate; 
	double b1 = p1->y - tan(p1->a) * p1->x;
	double b2 = p2->y - tan(p2->a) * p2->x;

	if(p1->a == 0 || p1->a == M_PI){ // 第一个点水平
		p->y = p1->y;
		if(p2->a == M_PI_2 || p2->a == M_PI_2 * 3){ // 第二个点垂直
			p->x = p2->x;
		}else{ // 第二个点有角度
			p->x = (p->y - b2) / tan(p2->a);
		}
		return p;
	}else if(p1->a == M_PI_2 || p1->a == M_PI_2 * 3){ // 第一个点垂直
		p->x = p1->x;
		if(p2->a == 0 || p2->a == M_PI){ // 第二个点水平
			p->y = p2->y;
		}else{ // 第二个点有角度
			p->y = tan(p2->a) * p->x + b2;
		}
		return p;
	}else { // 有角度
		if(p2->a == 0 || p2->a == M_PI){ // 第二点水平
			p->y = p2->y;
			p->x = (p->y - b1) / tan(p1->a);
			return p;
		}else if(p2->a == M_PI_2 || p2->a == M_PI_2 * 3){ // 第二点垂直
			p->x = p2->x;
			p->y = p->x * tan(p1->a) + b1;
			return p;
		}
	}
	p->x = (b1-b2)/(tan(p2->a) - tan(p1->a));
	p->y = tan(p1->a) * p->x + b1;
	return p;
}

// 不考虑折返
void calcCoordinate( _coordinate* p1, double dist, double angle, _coordinate* dest )
{
	dest->x = p1->x;
	dest->y = p1->y;
	dest->z = p1->z;

	dest->x += dist * cos(angle);
	dest->y += dist * sin(angle);

	dest->a = angle;
}

double getDistance( _coordinate* p1, _coordinate *p2 )
{
	return sqrt(pow(p1->x - p2->x, 2) + pow(p1->y - p2->y, 2));
}

bool is_all_inline( _coordinate** coor_list, int cnt )
{
	bool ret = false;
	if(cnt < 3) return ret;
	if(coor_list[0]->x == coor_list[1]->x && coor_list[0]->x == coor_list[2]->x){
		ret = true;
	}else if(coor_list[0]->y == coor_list[1]->y && coor_list[0]->y == coor_list[2]->y){
		ret = true;
	}else{
		ret = false;
	}	
	return ret;
}

//陈扬算法 2016-09-12
/*POS* TOFLocateAlgorithm::Pos(STATION *s,int sta_num,int ant,double dist,INFO_PRE info_pre)
{
	POS* p = new POS;
	p->posx = 0;
	p->posy = 0;
	p->pos_radius = 999999.9; //inf在matlab中表示负无穷

	double d[2];
	double xcross[2];
	double ycross[2];

	//size(station(sta_num).px,2)为取矩阵px的列数，按station定义中px要么是2，要么是3
	int seg_num = 0;
	seg_num = s[sta_num].nRealCalcPoints - 1;

	if(seg_num < 0 ){
		return NULL;
	}

	int j = 0;
	for(int i =0;i<seg_num;i++){

		if(s[sta_num].px[i]==-1||s[sta_num].px[i+1]==-1)
		{
			continue;
		}

		RESULT *r = NULL;
		r = GetPos(s,sta_num,ant,dist,i);
		
		//r = &g_result;
		if(r == NULL){
			continue;
		}
		//r = &result;
		//x的列数count取2，是因为解最多为2
		int count = 2; 
		for(j =0;j<2;j++){
			xcross[j] = r->x[j];
			ycross[j] = r->y[j];
		}

		for(j = 0;j < count;j++){
			double d1 = xcross[j] - s[info_pre.sta_num].x[info_pre.ant];
			double d2 = ycross[j] - s[info_pre.sta_num].y[info_pre.ant];
			d[j] = sqrt(pow(d1,2) + pow(d2,2));
		}
		for(j = 0;j < count;j++){
			d[j] = fabs(d[j] - info_pre.dist);
		}

		double c = 999999.9;
		int k = -1;
		for(j = 0;j < count;j++){
			if(d[j] < c){
				c = d[j];
				k = j;
			}
		}

		if(c<p->pos_radius){
			p->posx = xcross[k];
			p->posy = ycross[k];
			p->pos_radius = c;
		}
	}

	return p;
}

RESULT* TOFLocateAlgorithm::GetPos(STATION *s,int sta_num,int ant,double dist,int i)
{
	double x1 = 0.0;
	double y1 = 0.0;
	double x2 = 0.0;
	double y2 = 0.0;

	double deta = 0.0;  //b^2 - 4*a*c;
	int count = 0;	//方程解的个数

	if((s[sta_num].px[i] - s[sta_num].px[i+1]) == 0){
		//方程一的解
		x1 = s[sta_num].px[i];
		y1 = s[sta_num].y[ant] - dist;
		x2 = s[sta_num].px[i];
		y2 = dist + s[sta_num].y[ant];
		count = 2;
	}else{
		//计算斜率
		double k = (s[sta_num].py[i] - s[sta_num].py[i+1])/(s[sta_num].px[i] - s[sta_num].px[i+1]);

		//方程二
		//确保有两解的情况
		//圆心为s[sta_num].x[ant],s[sta_num].y[ant];
		double a = pow(k,2) + 1;
		double b = 2*(k*(s[sta_num].py[i] - k*s[sta_num].px[i] - s[sta_num].y[ant])- s[sta_num].x[ant]);
		double d = s[sta_num].py[i] - k * s[sta_num].px[i] - s[sta_num].y[ant]; 
		double c = pow(s[sta_num].x[ant],2) + pow(d,2) - pow(dist,2);

		//根据deta = b^2 - 4*a*c 判断方程是否有解
		deta = pow(b,2) - 4*a*c; //
		if(deta>0){
			count = 2;
		}else if(deta == 0){
			count = 1;
		}
		else{
			count = 0;
		}

		if(count == 0){
			return NULL;
		}

		//得两解
		//deta = pow(b,2) - 4*a*c; //
		x1 = -(b + sqrt(deta))/(2*a);
		x2 = (-b + sqrt(deta) )/(2*a);
		y1 = k*x1 + s[sta_num].py[i] - k*s[sta_num].px[i];
		y2 = k*x2 + s[sta_num].py[i] - k*s[sta_num].px[i];
	}

	double x[3] = {0,0,0};
	double y[3] = {0,0,0};
	
	x[0] = x1;
	x[1] = x2;
	y[0] = y1;
	y[1] = y2;

	if(count>0){
		for(int t = 0;t<count;t++){
			if(x[t] < min(s[sta_num].px[i],s[sta_num].px[i+1]) - 0.01
				||x[t] > max(s[sta_num].px[i],s[sta_num].px[i+1]) + 0.01
				||y[t] < min(s[sta_num].py[i],s[sta_num].py[i+1])
				||y[t] > max(s[sta_num].py[i],s[sta_num].py[i+1])){
				x[t] = 0;
				y[t] = 0;
			}
		}
	}

	RESULT* result = new RESULT;
	memset(result->x,0,3*sizeof(double));
	memset(result->y,0,3*sizeof(double));
	result->x[0] = x[0];
	result->x[1] = x[1];
	result->y[0] = y[0];
	result->y[1] = y[1];

	return result;
}*/

POS* LocateAlgorithm::Pos(ReaderPathMap rpm,int sta_num,int ant,double dist,INFO_PRE info_pre)
{
	POS* p = new POS;
	p->posx = 0;
	p->posy = 0;
	p->pos_radius = 999999.9; //inf在matlab中表示负无穷

	double d[2];
	double xcross[2];
	double ycross[2];

	//size(station(sta_num).px,2)为取矩阵px的列数，按station定义中px要么是2，要么是3
	int seg_num = 0;
	//seg_num = s[sta_num].nRealCalcPoints - 1;
	seg_num = rpm.find(sta_num)->second->nRealCalcPoints - 1;

	if(seg_num < 0 ){
		return NULL;
	}

	int j = 0;
	for(int i =0;i<seg_num;i++){

		if(rpm.find(sta_num)->second->px[i]==-1||rpm.find(sta_num)->second->px[i+1]==-1)
		{
			continue;
		}

		RESULT *r = NULL;
		r = GetPos(rpm,sta_num,ant,dist,i);
		
		//r = &g_result;
		if(r == NULL){
			continue;
		}
		//r = &result;
		//x的列数count取2，是因为解最多为2
		int count = 2; 
		for(j =0;j<2;j++){
			xcross[j] = r->x[j];
			ycross[j] = r->y[j];
		}

		for(j = 0;j < count;j++){
			double d1 = xcross[j] - rpm.find(info_pre.sta_num)->second->x[info_pre.ant];
			double d2 = ycross[j] - rpm.find(info_pre.sta_num)->second->y[info_pre.ant];
			d[j] = sqrt(pow(d1,2) + pow(d2,2));
		}
		for(j = 0;j < count;j++){
			d[j] = fabs(d[j] - info_pre.dist);
		}

		double c = 999999.9;
		int k = -1;
		for(j = 0;j < count;j++){
			if(d[j] < c){
				c = d[j];
				k = j;
			}
		}

		if(c<p->pos_radius){
			p->posx = xcross[k];
			p->posy = ycross[k];
			p->pos_radius = c;
		}
	}

	return p;
}

RESULT* LocateAlgorithm::GetPos(ReaderPathMap rpm,int sta_num,int ant,double dist,int i)
{
	double x1 = 0.0;
	double y1 = 0.0;
	double x2 = 0.0;
	double y2 = 0.0;

	double deta = 0.0;  //b^2 - 4*a*c;
	int count = 0;	//方程解的个数

	if((rpm.find(sta_num)->second->px[i] - rpm.find(sta_num)->second->px[i+1]) == 0){
		//方程一的解
		x1 = rpm.find(sta_num)->second->px[i];
		y1 = rpm.find(sta_num)->second->y[ant] - dist;
		x2 = rpm.find(sta_num)->second->px[i];
		y2 = dist + rpm.find(sta_num)->second->y[ant];
		count = 2;
	}else{
		//计算斜率
		double k = (rpm.find(sta_num)->second->py[i] - rpm.find(sta_num)->second->py[i+1])/(rpm.find(sta_num)->second->px[i] - rpm.find(sta_num)->second->px[i+1]);

		//方程二
		//确保有两解的情况
		//圆心为s[sta_num].x[ant],s[sta_num].y[ant];
		double a = pow(k,2) + 1;
		double b = 2*(k*(rpm.find(sta_num)->second->py[i] - k*rpm.find(sta_num)->second->px[i] - rpm.find(sta_num)->second->y[ant])- rpm.find(sta_num)->second->x[ant]);
		double d = rpm.find(sta_num)->second->py[i] - k * rpm.find(sta_num)->second->px[i] - rpm.find(sta_num)->second->y[ant]; 
		double c = pow(rpm.find(sta_num)->second->x[ant],2) + pow(d,2) - pow(dist,2);

		//根据deta = b^2 - 4*a*c 判断方程是否有解
		deta = pow(b,2) - 4*a*c; //
		if(deta>0){
			count = 2;
		}else if(deta == 0){
			count = 1;
		}
		else{
			count = 0;
		}

		if(count == 0){
			return NULL;
		}

		//得两解
		//deta = pow(b,2) - 4*a*c; //
		x1 = -(b + sqrt(deta))/(2*a);
		x2 = (-b + sqrt(deta) )/(2*a);
		y1 = k*x1 + rpm.find(sta_num)->second->py[i] - k*rpm.find(sta_num)->second->px[i];
		y2 = k*x2 + rpm.find(sta_num)->second->py[i] - k*rpm.find(sta_num)->second->px[i];
	}

	double x[3] = {0,0,0};
	double y[3] = {0,0,0};
	
	x[0] = x1;
	x[1] = x2;
	y[0] = y1;
	y[1] = y2;

	if(count>0){
		for(int t = 0;t<count;t++){
			if(x[t] < min(rpm.find(sta_num)->second->px[i],rpm.find(sta_num)->second->px[i+1]) - 0.01
				||x[t] > max(rpm.find(sta_num)->second->px[i],rpm.find(sta_num)->second->px[i+1]) + 0.01
				||y[t] < min(rpm.find(sta_num)->second->py[i],rpm.find(sta_num)->second->py[i+1])
				||y[t] > max(rpm.find(sta_num)->second->py[i],rpm.find(sta_num)->second->py[i+1])){
				x[t] = 0;
				y[t] = 0;
			}
		}
	}

	RESULT* result = new RESULT;
	memset(result->x,0,3*sizeof(double));
	memset(result->y,0,3*sizeof(double));
	memset(result->z,0,3*sizeof(double));
	result->x[0] = x[0];
	result->x[1] = x[1];
	result->y[0] = y[0];
	result->y[1] = y[1];
	result->z[0] = 0;
	result->z[1] = 0;

	return result;
}

/*
 * TDOA算法实现
 * 函数名：Pos
 *		遍历求解
 * param
 *		pRdm	------		存放参与计算的分站信息，主要信息包含同步后的时间戳
 *	    trpm    ------		地图集
 *
 * return
 *	    返回最终结果坐标
 *
*/
POS* LocateAlgorithm::Pos(ReceiveDataMap* pRdm,TDOAReaderPathMap trpm)
{
	POS *pos = NULL;
	pos = new POS;
	/*pos->posx = 0;
	pos->posy = 0;
	pos->posz = 0;
	pos->pos_radius = 0;
	pos->dFirstDiff = INVALID_COORDINATE;
	pos->dSecondDiff = INVALID_COORDINATE;
	pos->nFirstReader = INVALID_COORDINATE;
	pos->nSecondReader = INVALID_COORDINATE;*/

	double a = 0;
	double ref_dist = 0.0;   //两分站之间无路径的距离差
	
	const int Totals = 2;
	_coordinate res[Totals];
	int res_idx = 0;
	for(int i = 0;i < Totals;i++){
		res[0].x = INVALID_COORDINATE;
		res[0].y = INVALID_COORDINATE;
		res[0].z = INVALID_COORDINATE;
	}

	bool bFirst = true;
	int nNoReaderPathIdx = 0;
	int nFirstReader[Totals] = {-1};
	int nSecondReader[Totals] ={-1};

	//存储无路径的两分站的id和坐标
	//ReceiveData* tmp_reader = new ReceiveData[Totals];
	ReceiveData tmp_reader[Totals];

	//存储和第一条分站存在路径的分站信息
	ReceiveData tmp_dist_reader[Totals];
	int nDistReaders = 0;

	ReceiveDataMap::iterator first = pRdm->begin();
	//for(ReceiveDataMap::iterator first = pRdm->begin();first!=pRdm->end();++first){///11
		//ReceiveDataMap::iterator second = pRdm->begin();
	ReceiveDataMap::iterator second = first;
	//偏移到第二个元素
	std::advance(second,1);

	//获取第一个时间戳
	ReceiveData f1;
	f1.antenna_id = first->second.antenna_id;
	f1.reader_id = first->second.reader_id;
	f1.rec_time_stamp = first->second.rec_time_stamp;
	f1.x = first->second.x;
	f1.y = first->second.y;
	f1.z = first->second.z;

	//从第二个开始遍历
	for(;second != pRdm->end();++second){
		//获取第二个时间戳
		ReceiveData f2;
		f2.antenna_id = second->second.antenna_id;
		f2.reader_id = second->second.reader_id;
		f2.rec_time_stamp = second->second.rec_time_stamp;
		f2.x = second->second.x;
		f2.y = second->second.y;
		f2.z = second->second.z;

		//时间戳异常
		if(f1.rec_time_stamp == LLONG_MAX || f2.rec_time_stamp == LLONG_MAX){
			continue;
		}

		if(f1.reader_id == f2.reader_id){
			continue;
		}

		//如果两级都能找到才运行继续后续操作，否则，表明没有此路径地图集
		TDOAReaderPathMap::iterator rdm_it = trpm.find(f1.reader_id);
		if(rdm_it == trpm.end()){
			continue;
		}

		ReaderPathMap::iterator rpm_it = trpm.find(f1.reader_id)->second->find(f2.reader_id);
		if(rpm_it == trpm.find(f1.reader_id)->second->end()){
			continue;
		}else{
			//如果和第一条分站存在地图集
			tmp_dist_reader[nDistReaders].reader_id = f2.reader_id;
			tmp_dist_reader[nDistReaders].x = f2.x;
			tmp_dist_reader[nDistReaders].y = f2.y;
			tmp_dist_reader[nDistReaders].z = f2.z;
			tmp_dist_reader[nDistReaders].rec_time_stamp = f2.rec_time_stamp;
			nDistReaders++;
		}

		//根据距离的正负，后续判断计算位置取舍时使用
		int nSign = 1;
		long long diffTime = f1.rec_time_stamp - f2.rec_time_stamp;

		//计算位置
		double dist = CFunctions::getDistance(diffTime,CFunctions::TDOA);

		double readers_dist = sqrt(pow(f1.x - f2.x,2) + pow(f1.y - f2.y,2));

		if(fabs(dist) - readers_dist > 0){
			continue;
		}

		////如果两级都能找到才运行继续后续操作，否则，表明没有此路径地图集
		//TDOAReaderPathMap::iterator rdm_it = trpm.find(f1.reader_id);
		//if(rdm_it == trpm.end()){
		//	continue;
		//}

		//获取地图集路径
		ReaderPath * pRP = trpm.find(f1.reader_id)->second->find(f2.reader_id)->second;
		ReaderPath pRP2 = *trpm.find(f1.reader_id)->second->find(f2.reader_id)->second;

		tmp_reader[nNoReaderPathIdx].reader_id = f2.reader_id;
		tmp_reader[nNoReaderPathIdx].x = f2.x;
		tmp_reader[nNoReaderPathIdx].y = f2.y;
		tmp_reader[nNoReaderPathIdx].z = f2.z;
		tmp_reader[nNoReaderPathIdx].rec_time_stamp = f2.rec_time_stamp;
		nNoReaderPathIdx++;

		//两分站之间的线段个数
		int seg_num = pRP->nRealCalcPoints - 1;

		if(seg_num == 0 || seg_num > 100){
			continue;
		}

		//因为双曲线与分站之间第i条线段或者第j条线段分别有两焦点
		//或者分站之间就一条直线，有两焦点
		double xcross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};
		double ycross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};
		double zcross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};

		int nIdx = 0;

		//根据线段个数开始计算
		for(int i = 0;i < seg_num;i++){
			RESULT * r = NULL;

			//计算位置坐标，双曲线和线段相交的交点
			r = LocateAlgorithm::GetPos(pRP,dist,i);

			//无解或解无效
			if(r == NULL ){
				continue;
			}
			if(r->nCount == 0){
				delete r;
				r = NULL;
				continue;
			}

			/*nFirstReader = f1.reader_id;
			nSecondReader = f2.reader_id;*/

			if(r->nCount == 1){
				xcross[nIdx] = r->x[0];
				ycross[nIdx] = r->y[0];
				nIdx++;
			}
			if(r->nCount == 2){
				for(int j = 0; j < 2;j++){
					xcross[j] = r->x[j];
					ycross[j] = r->y[j];
					nIdx++;
				}
				if(r){
					delete r;
					r = NULL;
				}
			}

			if(nIdx == 2){
				//解到两焦点之间的距离
				double deltad[2] = {0};
				for(int j = 0; j < 2;j ++){
					double d[2] = {0};
					double dx1 = xcross[j] - pRP->x[0];
					double dy1 = ycross[j] - pRP->y[0];
					d[0] = sqrt(pow(dx1,2) + pow(dy1,2));

					double dx2 = xcross[j] - pRP->x[1];
					double dy2 = ycross[j] - pRP->y[1];
					d[1] = sqrt(pow(dx2,2) + pow(dy2,2));

					deltad[j] = d[0] - d[1];
				}

				int idx = 0;	

				//应该和之前计算的dist同方向
				if(dist > 0){
					for(int j = 0;j < 2;j++){
						if(deltad[j] > 0){
							idx = j;
							break;
						}
					}	
				}else{
					for(int j = 0;j < 2;j++){
						if(deltad[j] < 0){
							idx = j;
							break;
						}
					}	
				}
				nFirstReader[res_idx]  = f1.reader_id;
				nSecondReader[res_idx] = f2.reader_id;

				res[res_idx].x = xcross[idx];
				res[res_idx].y = ycross[idx];
				res[res_idx].z = zcross[idx];
				res_idx++;
			}		

			if(r){
				delete r;
				r = NULL;
			}
			continue;
		}
	}


	_coordinate r;
	r.x = INVALID_COORDINATE;
	r.y = INVALID_COORDINATE;
	r.z = INVALID_COORDINATE;

	if(res_idx == 1){
		bool bValid = true;

		//如果定位坐标在分站附近，需要判断此分站是否为special，如果为special则接受此定位结果
		for(ReceiveDataMap::iterator it = pRdm->begin();it!=pRdm->end();++it){
			double dist = sqrt(pow(res[0].x - it->second.x,2) + pow(res[0].y - it->second.y,2));	
			if(dist<4){
				if(it->second.special == 0){
					bValid = false;
				}
			}
		}

		if(pRdm->size()>=3&&bValid){
			/*ReceiveDataMap::iterator it = pRdm->begin();
			ReceiveDataMap::iterator f_it = pRdm->end();
			int k = 0;
			int readersNo[2] = {0};
			readersNo[0] = nFirstReader[0];
			readersNo[1] = nSecondReader[0];*/

			//找出没有参与定位的任意一个分站信息
			//ReceiveData rd;
			//bool isExist = false;
			//for(it;it!=pRdm->end();++it){
			//	for(int i=0;i<2;i++){
			//		if(it->first == readersNo[i]){
			//			f_it = pRdm->end();
			//			isExist = false;
			//			break;
			//		}else{
			//			isExist = true;
			//		}
			//	}
			//	if(isExist){
			//		//如果存在
			//		rd.reader_id = it->first;
			//		rd.rec_time_stamp = it->second.rec_time_stamp;
			//		rd.antenna_id = it->second.antenna_id;
			//		rd.special = it->second.special;
			//		rd.x = it->second.x;
			//		rd.y = it->second.y;
			//		rd.z = it->second.z;
			//	}
			//}

			double dist[2] = {0};
			//计算定位结果到参与定位结果之一的分站和未参与定位的分站之间的距离差
			//比较此距离差和这两个
			//if(rd.reader_id!=0){
			//	ReceiveDataMap::iterator it = pRdm->find(nFirstReader[0]);

			//	dist[0] = sqrt(pow(res[0].x - it->second.x,2) + pow(res[0].y - it->second.y,2));
			//	dist[1] = sqrt(pow(res[0].x - rd.x,2) + pow(res[0].y - rd.y,2));

			//	double dif1 = fabs(dist[0] - dist[1]);

			//	double dt = it->second.rec_time_stamp - rd.rec_time_stamp;
			//	double dif2 = fabs(CFunctions::getDistance(dt,CFunctions::TDOA));

			//	double dif = fabs(dif1 - dif2);

			//	if(dif>10){//4
			//		bValid = false;
			//	}	
			//}
			//判断下定位坐标距离定位分站的距离


			//2016/12/06 更改选取分站方式:
			//选取和第一个分站有地图集的两个分站来参与判别，
			//例如：分站顺序是2，1,3，其中（1,2）和（2,3）之间有地图集，那么就选取1,3来参与判别
			//如果分站顺序是1,2，3，其中（1，2）有地图集，(1,3)之间无地图集，那么此时就不使用以下方法进行判别
			if(nDistReaders == 2){
				dist[0] = sqrt(pow(res[0].x - tmp_dist_reader[0].x,2) + pow(res[0].y - tmp_dist_reader[0].y,2));
				dist[1] = sqrt(pow(res[0].x - tmp_dist_reader[1].x,2) + pow(res[0].y - tmp_dist_reader[1].y,2));

				double dif1 = fabs(dist[0] - dist[1]);

				double dt = tmp_dist_reader[0].rec_time_stamp - tmp_dist_reader[1].rec_time_stamp;
				double dif2 = fabs(CFunctions::getDistance(dt,CFunctions::TDOA));

				double dif = fabs(dif1 - dif2);

				if(dif>10){//4
					bValid = false;
				}	
			}
		}

		if(bValid){
			r.x = res[0].x;
			r.y = res[0].y;
			r.z = res[0].z;
			pos->dFirstDiff = 0.0;
			pos->dSecondDiff = 0.0;
		}
	}

	int r_idx = 0;

	if(res_idx >= 2){
		double d = 99999.9;
		
		long long dt = tmp_reader[0].rec_time_stamp - tmp_reader[1].rec_time_stamp;
		//计算位置
		double ref_dist = CFunctions::getDistance(dt,CFunctions::TDOA);

		double diff[2] = {0};
		for(int i = 0;i<res_idx;i++){//2
			double d_tmp = sqrt(pow(res[i].x - tmp_reader[0].x,2) + pow(res[i].y - tmp_reader[0].y,2)) - sqrt(pow(res[i].x - tmp_reader[1].x,2) + pow(res[i].y - tmp_reader[1].y,2));
			double d_diff = abs(d_tmp - ref_dist);
			switch(i){
			case 0:
				pos->dFirstDiff = d_diff;
				break;
			case 1:
				pos->dSecondDiff = d_diff;
				break;
			}
			diff[i] = d_diff;
			if(d_diff < d){
				d = d_diff;
				r_idx = i;
			}
		}

		if(pRdm->size()>=3){
			if(diff[r_idx] < 4){
				r.x = res[r_idx].x;
				r.y = res[r_idx].y;
				r.z = res[r_idx].z;
			}
		}
	}

	//最终得到的结果解
	pos->posx = r.x;
	pos->posy = r.y;
	//pos->posz = res.z;
	pos->posz = 0;

	if(pos->posx != INVALID_COORDINATE && pos->posy != INVALID_COORDINATE){
		pos->nFirstReader = nFirstReader[r_idx];
		pos->nSecondReader = nSecondReader[r_idx];
	}

	return pos;
}

POS* LocateAlgorithm::Pos(POS* pos,TDOAReaderPathMap trpm)
{
	if(trpm.find(pos->nFirstReader) == trpm.end()){
		return NULL;
	}
	if(trpm.find(pos->nFirstReader)->second->find(pos->nSecondReader)==trpm.find(pos->nFirstReader)->second->end()){
		return NULL;
	}

	POS *p = NULL;
	p = new POS;
	p->posx = pos->posx;
	p->posy = pos->posy;
	p->posz = pos->posz;
	p->pos_radius = pos->pos_radius;
	p->nFirstReader = pos->nFirstReader;
	p->nSecondReader = pos->nSecondReader;

	_coordinate res[2];
	int res_idx = 0;
	for(int i = 0;i < 2;i++){
		res[i].x = INVALID_COORDINATE;
		res[i].y = INVALID_COORDINATE;
		res[i].z = INVALID_COORDINATE;
	}

	//距离差
	double distance = 0.0;

	distance = sqrt(pow(p->posx - trpm.find(pos->nFirstReader)->second->find(pos->nSecondReader)->second->x[0],2 ) + pow(p->posy - trpm.find(pos->nFirstReader)->second->find(pos->nSecondReader)->second->y[0],2 )) - 
		sqrt(pow(p->posx - trpm.find(pos->nFirstReader)->second->find(pos->nSecondReader)->second->x[1],2 ) + pow(p->posy - trpm.find(pos->nFirstReader)->second->find(pos->nSecondReader)->second->y[1],2 ));

	//获取地图集路径
	ReaderPath * pRP = trpm.find(pos->nFirstReader)->second->find(pos->nSecondReader)->second;

	//两分站之间的线段个数
	int seg_num = pRP->nRealCalcPoints - 1;

	if(seg_num == 0){
		return NULL;
	}

	double xcross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};
	double ycross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};
	double zcross[2] = {INVALID_COORDINATE,INVALID_COORDINATE};

	int nIdx = 0;
	int nResIdx = 0;

	for(int i = 0;i < seg_num;i++){
		RESULT * r = NULL;

		//计算位置坐标，双曲线和线段相交的交点
		r = GetPos(pRP,distance,i);

		//无解或解无效
		if(r == NULL ){
			continue;
		}
		if(r->nCount == 0){
			delete r;
			r = NULL;
			continue;
		}

		if(r->nCount == 1){
			xcross[nIdx] = r->x[0];
			ycross[nIdx] = r->y[0];
			nIdx++;
		}
		if(r->nCount == 2){
			for(int j = 0; j < 2;j++){
				xcross[j] = r->x[j];
				ycross[j] = r->y[j];
				nIdx++;
			}
			if(r){
				delete r;
				r = NULL;
			}
		}

		if(nIdx == 2){
			//解到两焦点之间的距离
			double deltad[2] = {0};
			for(int j = 0; j < 2;j ++){
				double d[2] = {0};
				double dx1 = xcross[j] - pRP->x[0];
				double dy1 = ycross[j] - pRP->y[0];
				d[0] = sqrt(pow(dx1,2) + pow(dy1,2));

				double dx2 = xcross[j] - pRP->x[1];
				double dy2 = ycross[j] - pRP->y[1];
				d[1] = sqrt(pow(dx2,2) + pow(dy2,2));

				deltad[j] = d[0] - d[1];
			}

			int idx = 0;	

			//应该和之前计算的dist同方向
			if(distance > 0){
				for(int j = 0;j < 2;j++){
					if(deltad[j] > 0){
						idx = j;
						break;
					}
				}	
			}else{
				for(int j = 0;j < 2;j++){
					if(deltad[j] < 0){
						idx = j;
						break;
					}
				}	
			}
			res[nResIdx].x = xcross[idx];
			res[nResIdx].y = ycross[idx];
			res[nResIdx].z = zcross[idx];
			//nResIdx = idx;
		}		
		continue;
	}

	p->posx = res[nResIdx].x;
	p->posy = res[nResIdx].y;
	p->posz = res[nResIdx].z;

	return p;
}

/*
 * TDOA算法求解实现
 * 函数名：GetPos
 *		实现直线和双曲线求交点
 * param
 *		pRP    ------ 分站路径
 *      dist   ------ 距离差
 *      i	   ------ 第i条线段
 *
 * return
 *      满足条件的解，最多两个
 *
*/
RESULT* LocateAlgorithm::GetPos(ReaderPath * pRP,double dist,int i)
{
	//解的个数
	int count = 0;

	//解的坐标
	double x[2] = {INVALID_COORDINATE,INVALID_COORDINATE};
	double y[2] = {INVALID_COORDINATE,INVALID_COORDINATE};

	//双曲线的两个焦点分别是
	double x1 = pRP->x[0];
	double y1 = pRP->y[0];

	double x2 = pRP->x[1];
	double y2 = pRP->y[1];

	//ReaderPath * pRP = trpm.find(start.reader_id)->second.find(end.reader_id)->second;
	if(pRP->px[i] - pRP->px[i+1] == 0){
		//x相等，双曲线在Y轴上
		x[0] = x[1] = pRP->px[i];
		y[0] = (y1 + y2 + dist)/2.0;
		y[1] = (y1 + y2 - dist)/2.0;
		count = 2;
	}
	else{
		//双曲线和直线相交
		//直线常数求解
		double k = (pRP->py[i+1] - pRP->py[i])/(pRP->px[i+1] - pRP->px[i]);
		double kb = pRP->py[i] - k*pRP->px[i];

		//求解的常数中间量
		double d1 = 2*(x2 - x1);//m
		//double d2 = 2*(y1 + y2);//n
		double d2 = 2*(y2 - y1);//n
		double d3 = pow(y1,2) + pow(x1,2)- pow(dist,2) - pow(y2,2)  - pow(x2,2);//p
		//方程ax^2 + bx + c = 0
		double a = pow(d1 + d2*k,2) - 4*pow(dist,2)*(pow(k,2) + 1);
		double b = 2*((d3 + d2*kb)*(d1 + d2*k) - 4*pow(dist,2)*(k*(kb - y2) - x2));
		double c = pow(d3 + d2*kb,2) - 4*pow(dist,2)*(pow(x2,2) + pow(kb-y2,2));

		double delta = pow(b,2) - 4*a*c;
		if(delta > 0){
			count = 2;
		}
		else if(delta == 0){
			count = 1;
		}
		else{
			count = 0;
		}

		if(count == 0){
			return NULL;
		}

		//计算解
		x[0] = -(b + sqrt(delta))/(2*a);
		x[1] = (-b + sqrt(delta) )/(2*a);
		y[0] = k*x[0] + kb;
		y[1] = k*x[1] + kb;	
	}
	int nIdx = 0;
	int nValidCount = count;
	//引入误差
	double deviation = 0.0;
	//判断两解是否在线段范围内
	if(count > 0){
		for(int t = 0; t < count ;t++){
			//两分站之间第i条线段的两端点
			if(x[t] < min(pRP->px[i],pRP->px[i+1]) - deviation||
			   x[t] > max(pRP->px[i],pRP->px[i+1]) + deviation||
			   y[t] < min(pRP->py[i],pRP->py[i+1]) - deviation||
			   y[t] > max(pRP->py[i],pRP->py[i+1]) + deviation
				)
			{
				x[t] = INVALID_COORDINATE;
				y[t] = INVALID_COORDINATE;
				nValidCount--;
				nIdx = t;
			}
		}
	}
	
	RESULT* result = new RESULT;
	memset(result->x,0,3*sizeof(double));
	memset(result->y,0,3*sizeof(double));
	//memset(result->z,0,3*sizeof(double));
	result->nCount = 0;
	
	result->nCount = nValidCount;
	//result->z[0] = 0;
	//result->z[1] = 0;

	nIdx = -1;
	switch(nValidCount){
	case 1:
		for(int i = 0;i<2;i++){
			if(x[i] != INVALID_COORDINATE && y[i] != INVALID_COORDINATE){
				nIdx = i;
			}
		}
		result->x[0] = x[nIdx];
		result->y[0] = y[nIdx];
		break;
	case 2:
		result->x[0] = x[0];
		result->x[1] = x[1];
		result->y[0] = y[0];
		result->y[1] = y[1];
		break;
	}

	return result;
}

/*
 * 判断坐标在地图集上
 *
 * param
 *		pos   ------		定位的结果
 *		trpm  ------		地图集
 *
 * return
 *		false，不在地图集上，true在地图集上
 *
*/
bool LocateAlgorithm::IsOnMap(POS* pos,TDOAReaderPathMap trpm)
{
	//如果地图集中都找不到此分站开始的地图集，就返回false
	if(trpm.find(pos->nFirstReader) == trpm.end()){
		return false;
	}

	//如果地图集找不到此两分站的地图集，就返回false
	if(trpm.find(pos->nFirstReader)->second->find(pos->nSecondReader) == trpm.find(pos->nFirstReader)->second->end())
	{
		return false;
	}

	//保存两分站的坐标
	double x[2] = {0};
	double y[2] = {0};

	for(int i=0;i<2;i++){
		x[i] = trpm.find(pos->nFirstReader)->second->find(pos->nSecondReader)->second->x[i];
		y[i] = trpm.find(pos->nFirstReader)->second->find(pos->nSecondReader)->second->y[i];
	}

	if(x[0] == x[1]){
		//误差1cm
		if(abs(pos->posx - x[0]) > 1E-2){
			return false;
		}
	}else{
		double k = (y[1] - y[0])/(x[1] - x[0]);
		double b = y[1] - k*x[1];

		double calc_y = k*pos->posx + b;

		if(abs(calc_y - pos->posy) > 1E-2){
			return false;
		}
	}
	
	return true;
}

bool LocateAlgorithm::CheckPosInValid(POS* pos,ReceiveDataMap* pRdm,double dScale)
{
	if(dScale <= 0.0){
		return false;
	}

	if(pos->posx == INVALID_COORDINATE || pos->posy == INVALID_COORDINATE){
		return false;
	}
	//判断解的无效性
	int nSize = 0;
	bool bRet = false;

	nSize = pRdm->size();
	double dist[2] = {0.0};

	if(nSize == 2){
		int i = 0;
		for(ReceiveDataMap::iterator it = pRdm->begin();it!=pRdm->end();++it,i++){
			dist[i] = sqrt(pow(pos->posx - it->second.x,2)+pow(pos->posy - it->second.y,2));
			if(fabs(dist[i]) < 4){
				//误差在范围内，判断此分站是否属于特殊分站
				if(it->second.special){
					bRet = false;
				}else{
					bRet = true;
				}	
			}
		}	
	}

	//if(nSize >= 3){
	//	//先找出未参与定位的分站
	//	ReceiveDataMap::iterator f_it = pRdm->end();
	//	ReceiveDataMap::iterator it = pRdm->begin();
	//	int readers[2] = {0};
	//	readers[0] = pos->nFirstReader;
	//	readers[1] = pos->nSecondReader;

	//	for(it;it!=pRdm->end();++it){
	//		for(int i=0;i<2;i++){
	//			if(it->first == readers[i]){
	//				f_it = pRdm->end();
	//				break;
	//			}else{
	//				f_it = it;
	//			}
	//		}
	//	}

	//	if(f_it != pRdm->end()){
	//		ReceiveDataMap::iterator it = pRdm->find(pos->nFirstReader);

	//		dist[0] = sqrt(pow(pos->posx - it->second.x,2) + pow(pos->posy - it->second.y,2)) / dScale;
	//		dist[1] = sqrt(pow(pos->posx - f_it->second.x,2) + pow(pos->posy - f_it->second.y,2)) / dScale;

	//		double dif1 = dist[0] - dist[1];

	//		double dt = it->second.rec_time_stamp - f_it->second.rec_time_stamp;
	//		double dif2 = CFunctions::getDistance(dt,CFunctions::TDOA);

	//		double dif = fabs(dif1 - dif2);

	//		if(dif - 1 <= 1E-4){
	//			bRet = true;
	//		}

	//	}
	//}

	return bRet;
}

void HeapSort(_coordinate** pCoordinateArray,int nLen)
{
	BuildMaxHeap(pCoordinateArray,nLen);
	
	for(int i = nLen - 1;i > 0;i--){
		SwapElement(pCoordinateArray[0],pCoordinateArray[i]);
		AdjustMaxHeap(pCoordinateArray,0,i-1);
	}
}

void BuildMaxHeap(_coordinate** pCoordinateArray,int nLen)
{
	for(int i = nLen/2 - 1;i > 0;i--){
		AdjustMaxHeap(pCoordinateArray,i,nLen-1);
	}
}

void AdjustMaxHeap(_coordinate** pCoordinateArray,int n,int nHeapSize)
{
	int l = (n+1)*2-1;
	int r = (n+1)*2;

	int max;

	if(l <= nHeapSize && pCoordinateArray[l]->tt > pCoordinateArray[n]->tt){
		max = l;
	}else{
		max = n;
	}

	if(r <= nHeapSize && pCoordinateArray[r]->tt > pCoordinateArray[max]->tt){
		max = r;
	}

	if(max != n){
		SwapElement(pCoordinateArray[n],pCoordinateArray[max]);
		AdjustMaxHeap(pCoordinateArray,max,nHeapSize);
	}
}

void SwapElement(_coordinate* a,_coordinate*b)
{
	_coordinate tmp;
	tmp = *a;
	*a = *b;
	*b = tmp;
}

void SelectSort(_coordinate** pCoordinateArray,int nLen)
{  
    for (int i=0; i<nLen; i++)  
    {  
        int k = i;  
        unsigned long long key = pCoordinateArray[i]->tt;  
        for (int j=i+1; j<nLen; j++)  
        {  
            if (pCoordinateArray[j]->tt < key) 
            {  
                k = j;  
                key = pCoordinateArray[j]->tt;  
            }  
        }  
        if (k!=i)  
            SwapElement(pCoordinateArray[i], pCoordinateArray[k]);  
    }  
}