机场跑道入侵检测（C++Qt）

Posted 2022-11-29 好儿郎-志在四方

机场跑道入侵检测

• • 实现原理：
  • 效果
  • 关键代码（只提供思路和部分代码，并非完整代码。）

实现原理：

1、读取并解析机场跑道、车道数据，显示和存储。
2、根据本机的经纬度坐标，确定本机所在跑道，并计算出一个多边形区域。
3、实时获取它机、车辆坐标数据，判断是否位于本机起飞跑道的多边形范围内。
4、如果它机、车辆坐标位于本机跑道，则预警。

通过面积法，判断点P是否在四边形(A,B,C,D)内。如果在四边形内,则四边形的面积=面积(P,A,B)+面积(P,B,C)+面积(P,C,D)+面积(P,D,A),反之不在四边形内。

如下图：

效果

视频地址：https://www.bilibili.com/video/BV1bK4y147R1/

关键代码（只提供思路和部分代码，并非完整代码。）

	//judge quadrangle is contain point p
	bool isInQuadrangle(QVector<QPointF> &pointfs, QPointF p);

	//calculate triangle area
	double getTriangleArea(QPointF p1, QPointF p2, QPointF p3);

	//get Vertex of Quadrangle 
	void getQuadrangleVertex(QPointF m1, QPointF m2, float direction, double len, QVector<QPointF> &pointfs);

	//polygon vertexes can be sorted clockwise
	void clockwiseSort(QVector<QPointF> & vPoints);

	bool PointCmp(const QPointF &a, const QPointF &b, const QPointF &center);

	double getAngle(const QPointF &oldPos, const QPointF &newPos) const;

//跑道入侵检测
void runwayIncursionCheck(std::vector<OtherAircraftInfo> &aircraftInfo)

	for (std::vector<OtherAircraftInfo>::iterator itr = aircraftInfo.begin(); itr != aircraftInfo.end(); itr++)
	
		if (mCurrentRunWay.count() == 4)
		
			if (mCalculate.isInQuadrangle(mCurrentRunWay, QPointF(itr->lon, itr->lat)))
			
				itr->incursion = true;
				qWarning() << "Runway Incursion: " << itr->ID;
			
			else
			
				itr->incursion = false;
			
				
	

计算本机所在的跑道
void whichRunWay(const AircraftInfo & aircraftInfo)

	//foreach runWay
	mCurrentRunWay.clear();
	for (int runwayIndex = 0; runwayIndex < mRunWays.count() ; runwayIndex++)
			
		QVector<QPointF> runWay(4);
		double angle = mCalculate.getAngle(mRunWays[runwayIndex].at(0), mRunWays[runwayIndex].at(1));  //calculate runway angle
		mCalculate.getQuadrangleVertex(mRunWays[runwayIndex].at(0), mRunWays[runwayIndex].at(1), angle , 60/1852.0, runWay);
		if (mCalculate.isInQuadrangle(runWay, QPointF(aircraftInfo.lon, aircraftInfo.lat)))
		
			mCurrentRunWay = runWay;
		
	

//通过一个已知坐标点、方位角、和距离，计算另一个点的经纬度坐标
void CalculatePositionByAngle(double & DestLat, double & DestLon, double orgLat, double orgLon, double distNM, double angle)

    double rad_lati = ToRadian(orgLat);
    double rad_longi = ToRadian(orgLon);
    double Ec = PolarRadiusMeter + (EquatorRadiusMeter - PolarRadiusMeter)*(90.0 - orgLat) / 90.0;
    double Ed = EquatorRadiusMeter * cos(rad_lati);

    double difx = distNM * 1852.0 * cos(ToRadian(90 - angle));
    double dify = distNM * 1852.0 * sin(ToRadian(90 - angle));

    DestLat = ToAngle(1.0*dify / Ec + rad_lati);
    DestLon = ToAngle(1.0*difx / Ed + rad_longi);

    if (DestLon > 180)
    
        DestLon = DestLon - 360;
    
    if (DestLat > 180)
    
        DestLat = DestLat - 360;
    

const qreal pi = 3.141592653589793238462643383;
//判断坐标点，是否在一个四边形区域内
bool isInQuadrangle(QVector<QPointF> &pointfs, QPointF p)

	double dTriangle = getTriangleArea(pointfs.at(0), pointfs.at(1), p) + getTriangleArea(pointfs.at(1), pointfs.at(2), p) + getTriangleArea(pointfs.at(2), pointfs.at(3), p) + getTriangleArea(pointfs.at(3), pointfs.at(0), p);
	double dQuadrangle = getTriangleArea(pointfs.at(0), pointfs.at(1), pointfs.at(2)) + getTriangleArea(pointfs.at(2), pointfs.at(3), pointfs.at(0));

	
	if ((dTriangle >0) && (dQuadrangle > 0) && (qAbs(dTriangle - dQuadrangle) < 0.00001))
	
		qDebug() << "isInQuadrangle: " << qAbs(dTriangle - dQuadrangle);
		return true;
	
	else
		return false;
	//return dTriangle == dQuadrangle;

//计算一个三角形的面积
double getTriangleArea(QPointF p1, QPointF p2, QPointF p3)

	return qAbs((p1.x()*p2.y() + p2.x()*p3.y() + p3.x()*p1.y() - p2.x()*p1.y() - p3.x()*p2.y() - p1.x()*p3.y()) / 2.0);


//根据四边形两条对边的中线，方位、宽度，计算出四个顶点坐标
void getQuadrangleVertex(QPointF m1, QPointF m2, float direction, double len, QVector<QPointF> &pointfs)

	float angle1 = 0.0;
	float angle2 = 0.0;

	angle1 = (direction + 90.0) > 360.0 ? direction + 90.0 - 360.0 : direction + 90.0;
	angle2 = (direction + 270.0) > 360.0 ? direction + 270.0 - 360.0 : direction + 270.0;

	Conversions cver;
	cver.CalculatePositionByAngle(pointfs[0].ry(), pointfs[0].rx(), m1.y(), m1.x(), len / 2, angle1);
	cver.CalculatePositionByAngle(pointfs[1].ry(), pointfs[1].rx(), m1.y(), m1.x(), len / 2, angle2);
	cver.CalculatePositionByAngle(pointfs[2].ry(), pointfs[2].rx(), m2.y(), m2.x(), len / 2, angle1);
	cver.CalculatePositionByAngle(pointfs[3].ry(), pointfs[3].rx(), m2.y(), m2.x(), len / 2, angle2);

	clockwiseSort(pointfs);


//四边形四个顶点 按照顺时针排序
void clockwiseSort(QVector<QPointF>& vPoints)

	//calculate centre point
	QPointF center;
	double x = 0, y = 0;
	for (int i = 0; i < vPoints.size(); i++)
	
		x += vPoints[i].x();
		y += vPoints[i].y();
	
	center.rx() = (int)x / vPoints.size();
	center.ry() = (int)y / vPoints.size();
	
	//sort point
	for (int i = 0; i < vPoints.size(); i++)
	
		for (int j = 0; j < vPoints.size() - i - 1; j++)
		
			if (PointCmp(vPoints[j], vPoints[j + 1], center))
			
				QPointF tmp = vPoints[j];
				vPoints[j] = vPoints[j + 1];
				vPoints[j + 1] = tmp;
			
		
	


bool PointCmp(const QPointF &a, const QPointF &b, const QPointF &center)

	if (a.x() >= 0 && b.x() < 0)
		return true;
	/*if (a.x() == 0 && b.x() == 0)
		return a.y() > b.y();*/
	//向量OA和向量OB的叉积
	int det = (a.x() - center.x()) * (b.y() - center.y()) - (b.x() - center.x()) * (a.y() - center.y());
	if (det < 0)
		return true;
	if (det > 0)
		return false;
	//向量OA和向量OB共线，以距离判断大小
	int d1 = (a.x() - center.x()) * (a.x() - center.x()) + (a.y() - center.y()) * (a.y() - center.y());
	int d2 = (b.x() - center.x()) * (b.x() - center.y()) + (b.y() - center.y()) * (b.y() - center.y());
	return d1 > d2;

//根据两个点，计算方位
double getAngle(const QPointF &oldPos, const QPointF &newPos) const

	//根据两个点的经纬度，以一个点（old）为坐标中心，y轴正方向为正北方向（即为 0°），求出另一个点（new）关于正北方向的角度（0° ~ 360°）

	double a = newPos.x() - oldPos.x();  //经度差
	double b = newPos.y() - oldPos.y();    //纬度差
	double c = hypot(fabs(a), fabs(b));
	double cosy = 0.0;
	double angle = 0;

	if (a > 0 && b > 0)         // 判断road点在user点的东北位置
	
		cosy = b / c;
		angle = 0;
	
	else if ((a == 0) && (b > 0))  //在正北位置
	
		angle = -90;
	
	else if ((a > 0) && (b < 0))   // 判断road点在user点的东南位置
	
		cosy = a / c;
		angle = 90;
	
	else if ((a > 0) && (b == 0))  //在正东位置
	
		angle = 90;
	
	else if ((a < 0) && (b < 0))    // 判断road点在user点的西南位置
	
		cosy = fabs(b) / c;
		angle = 180;
	
	else if ((a == 0) && (b < 0))   //在正南位置
	
		angle = 90;
	
	else if ((a < 0) && (b > 0))    // 判断road点在user点的西北位置
	
		cosy = fabs(a) / c;
		angle = 270;
	
	else if ((a < 0) && (b == 0))   //在正西位置
	
		angle = 180;
	

	double m = acos(cosy);
	//n 即以正北为 0 的总角度
	double n = (m / pi) * 180 + angle;

	return n;