server/locate_algorithm.h

#ifndef _LOCATE_ALGORITHM_H
#define _LOCATE_ALGORITHM_H

#include <stdio.h>
#include <stdlib.h>
#include "classdef.h"
#include "tdoa/TDOAFunctor.h"

#define M_PI       3.14159265358979323846 // 角度转弧度 角度 * pi / 180
#define M_PI_2     1.57079632679489661923
#define M_PI_4     0.785398163397448309616

// 最终定位坐标计算公式
void AOAformula(struct _coordinate* ant, double angle, struct _coordinate* tar, bool is_line = false, double z_offset = 0);
// 两个坐标之间的距离计算公式
double Distanceformula(struct _coordinate* ant, struct _coordinate* last);
// 三点定位
void locatebycordinate(struct _coordinate** coor_list, struct _coordinate* dest);
// 定位算法
void algorithm_locate(struct _coordinate** coor_list, int size, struct _coordinate* hist, struct _coordinate* dest, double dist_limit, double z_offset);

void algorithm_locate_ex(_coordinate** coor_list, int coor_size, _coordinate* hist, _coordinate* dest, double dist_limit);

double getAngleByCoordinate(_coordinate* p1, _coordinate* p2);
double getAngleByAngle(_coordinate* p1, _coordinate* p2);
double getDistance(_coordinate* p1, _coordinate *p2);
_coordinate* getCross(_coordinate* p1, _coordinate* p2);
void calcCoordinate(_coordinate* p1, double dist, double angle, _coordinate* dest);
bool is_all_inline(_coordinate** coor_list, int cnt );
void HeapSort(_coordinate** pCoordinateArray,int nLen);
void BuildMaxHeap(_coordinate** pCoordinateArray,int nLen);
void AdjustMaxHeap(_coordinate** pCoordinateArray,int n,int nHeapSize);
void SwapElement(_coordinate* a,_coordinate*b);
void SelectSort(_coordinate** pCoordinateArray,int nLen);

//static double Pnt2SegmentDist(Point A, Point B, Point C,Point* pD = NULL)
//{
//
//	//AB为线段，C为点，pD为需要返回C距离AB最近的点，返回值为C到AB的距离
//
//	//Dist为求两点之间的距离，第三个参数若是0不开方，若使用默认参数值1则需要开方。
//	Point AB = B - A; //向量AB
//	Point AC = C - A;
//	double r = AB.x * AC.x + AB.y * AC.y;//AB与AC的点乘积
//	r /= Dist(A,B,0);//AC在AB上的投影比上AB。调用Dist(),不开方
//	//若C的投影在AB外
//	if(r < 0)
//	{
//		if(pD)
//			*pD = A;
//		return Dist(A, C);//调用Dist(),开方
//	}
//	if(r > 1)
//	{
//		if(pD)
//			*pD = B;
//		return Dist(B, C);
//	}
//	//若C的投影在AB之间
//	Point D = AB;
//	D.x *= r;
//	D.y *= r;//因为AB是向量，所以可以这样做。得到AC在AB上的投影向量。
//	D.Offset(A);//点D的绝对坐标
//	if(pD)
//	{
//		*pD = D;
//	}
//	return Dist(C, D);
//}

/*
 * 定位算法
 *
*/
class LocateAlgorithm{
public:
	//TOF算法函数
	static POS* Pos(ReaderPathMap rpm,int sta_num,int ant,double dist,INFO_PRE info_pre);
	static SOLUTION* GetPos(ReaderPathMap rpm,int sta_num,int ant,double dist,int i);
	//TDOA算法函数
	static std::unique_ptr<POS> Pos(std::shared_ptr<ReceiveDataMap> pRdm,std::shared_ptr<TDOAReaderPathMap> trpm);
	static std::unique_ptr<POS> LocatePos(std::shared_ptr<ReceiveDataMap> pRdm,std::shared_ptr<TDOAReaderPathMap> trpm);
	static std::shared_ptr<POS> Pos(std::shared_ptr<POS> pos,std::shared_ptr<TDOAReaderPathMap> trpm);
	static std::shared_ptr<POS> MappingToPath(std::shared_ptr<POS> pos, std::shared_ptr<TDOAReaderPathMap> trpm);
	static std::shared_ptr<POS> TdoaLocate2d(std::shared_ptr<ReceiveDataMap> pRdm);

	static std::unique_ptr<POS> CalcCardPosition(std::shared_ptr<ReceiveDataMap> pRdm,std::shared_ptr<TDOAReaderPathMap> trpm);
	static std::unique_ptr<SOLUTION> GetPos(std::shared_ptr<ReaderPath> pRP,double dist,int i);
	static bool CheckPosInValid(POS* pos,ReceiveDataMap* pRdm,double dScale);
	static bool IsOnMap(std::shared_ptr<POS>& pos,std::shared_ptr<TDOAReaderPathMap> trpm);
	
	static bool IsInTriangle(std::vector<_point> vtp,_point p);
	static double GetTriangleArea(_point p0,_point p1,_point p2);
	static bool IsInLine(_point p,_point start_p,_point end_p);
	static bool PointIsInRect(point p,point tp,point lp,point bp,point rp);
	//
	static int CalcTdoaPosition(std::shared_ptr<ReceiveDataMap> pRdm,std::shared_ptr<TDOAReaderPathMap> trpm,std::vector<std::shared_ptr<POS>>& udm_pos);
};

#endif