機場跑道入侵檢測
實現原理:
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 ¢er);
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 ¢er)
{
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;
}