FlightGear next
AirportGroundRadar.cxx
Go to the documentation of this file.
1/*
2 * SPDX-FileName: AirportGroundRadar.cxx
3 * SPDX-FileComment: Implementation of the FlightGear Ground radar
4 * SPDX-FileCopyrightText: Copyright (C) 2023 Keith Paterson
5 * SPDX-License-Identifier: GPL-2.0-or-later
6 */
7
8#include <ATC/AirportGroundRadar.hxx>
9#include <simgear/debug/logstream.hxx>
10#include <simgear/math/SGGeod.hxx>
11#include <Airports/airport.hxx>
12#include <Airports/pavement.hxx>
13#include <AIModel/AIAircraft.hxx>
14#include <AIModel/performancedata.hxx>
15#include <AIModel/AIConstants.hxx>
16#include <AIModel/VectorMath.hxx>
17
18
19AirportGroundRadar::AirportGroundRadar(SGGeod min, SGGeod max): index(getBox, equal), min(min) {
20 double w = max.getLatitudeDeg() - min.getLatitudeDeg();
21 double h = max.getLongitudeDeg() - min.getLongitudeDeg();
22 assert(w>0);
23 assert(h>0);
24 index.resize(SGRect<double>(min.getLatitudeDeg(), min.getLongitudeDeg(), w, h));
25}
26
27AirportGroundRadar::AirportGroundRadar(FGAirportRef airport): index(getBox, equal) {
28 double INDEX_SIZE_DEG = 1;
29 double minLat = airport->getLatitude() - INDEX_SIZE_DEG/2;
30 double minLon = airport->getLongitude() - INDEX_SIZE_DEG/2;
31 SG_LOG(SG_ATC, SG_DEBUG, "Creating AirportGroundRadar for " << airport->getId());
32 AirportGroundRadar::airport = airport;
33 index.resize(SGRect<double>(minLat, minLon, INDEX_SIZE_DEG, INDEX_SIZE_DEG));
34}
35
38
39bool AirportGroundRadar::add(FGTrafficRef aiObject) {
40 bool ret = index.add(aiObject);
41 if (ret) {
42 SG_LOG(SG_ATC, SG_DEBUG, "Added Aircraft " << aiObject->getCallsign() << "(" << aiObject->getId() << ")" << " Leg : " << aiObject->getLeg() << " " << aiObject->getPos() );
43 //index.printPath(aiObject);
44 } else {
45 double distM = SGGeodesy::distanceM(aiObject->getPos(), airport->geod());
46 SG_LOG(SG_ATC, SG_ALERT, "Couldn't add Aircraft " << aiObject->getCallsign() << "(" << aiObject->getId() << ") to " << airport->getId() << " Dist " << distM << "m Leg " << aiObject->getLeg() );
47 }
48 return ret;
49}
50
51bool AirportGroundRadar::move(const SGRectd& newPos, FGTrafficRef aiObject)
52{
53 bool ret = index.move(newPos, aiObject);
54 if (!ret) {
55 SG_LOG(SG_ATC, SG_DEBUG, "Couldn't move Aircraft " << aiObject->getCallsign() << "(" << aiObject->getId() << ") to " << airport->getId() << " Leg " << aiObject->getLeg() );
56 }
57 return ret;
58}
59
60bool AirportGroundRadar::remove(FGTrafficRef aiObject)
61{
62 if (!aiObject) {
63 SG_LOG(SG_ATC, SG_ALERT, "Couldn't remove aiObject null" );
64 return false;
65 }
66
67 bool ret = index.remove(aiObject);
68 if (!ret) {
69 SG_LOG(SG_ATC, SG_DEV_ALERT, "Couldn't remove " << aiObject->getCallsign() << "(" << aiObject->getId() << ")");
70 } else {
71 SG_LOG(SG_ATC, SG_DEBUG, "Removed Aircraft " << aiObject->getCallsign() << "(" << aiObject->getId() << ")");
72 }
73 return ret;
74}
75
76size_t AirportGroundRadar::size() const {return index.size();}
77
78int AirportGroundRadar::getSize(FGTrafficRef aiObject){
79 int speedCorrection = (*aiObject).getSpeed()!=0?std::abs(5*(*aiObject).getSpeed()):20;
80 return std::abs((*aiObject).getRadius() + speedCorrection);
81}
82
83bool AirportGroundRadar::blocking(FGTrafficRef aiObject, FGTrafficRef other)
84{
85 const double courseTowardOther = SGGeodesy::courseDeg(aiObject->getPos(), other->getPos());
86 const double turningRate = aiObject->getHeadingDiff();
87 // For right before left priority
88 const double distM = SGGeodesy::distanceM(aiObject->getPos(), other->getPos());
89 const double headingDiff = SGMiscd::normalizePeriodic(-180, 180, aiObject->getHeading() - courseTowardOther - turningRate);
90 const double otherHeadingDiff = SGMiscd::normalizePeriodic(-180, 180, other->getHeading() - courseTowardOther);
91 const double threshold = getSize(aiObject) + getSize(other);
92 SG_LOG(SG_ATC, SG_BULK, "Search Id : " << aiObject->getId() << " Found Id : " << other->getId() << " Dist \t" << distM << "m Threshold " << threshold << " Headingdiff " << headingDiff << " Other heading diff " << otherHeadingDiff << " courseTowardOther " << courseTowardOther << " Turning " << aiObject->getHeadingDiff() << " Speeds : " << aiObject->getSpeed() << "/" << other->getSpeed() << " " << (other->getSpeed() == 0 ? "Other Stopped" : ""));
93 if (headingDiff < 0 // from the left
94 && aiObject->getSpeed() >= 0 && abs(headingDiff) < 90 // in front
95 && abs(otherHeadingDiff) > 90 // towards us
96 && other->getLeg() != AILeg::STARTUP_PUSHBACK) {
97 // from the right and in front or other is stopped
98 SG_LOG(SG_ATC, SG_BULK, aiObject->getId() << " blocked by " << other->getId() << " Dist " << distM << " Headingdiff " << headingDiff << " Other heading diff " << otherHeadingDiff);
99 return true;
100 }
101 if (aiObject->getSpeed() < 0 && abs(headingDiff) > 90) {
102 // moving backwards
103 SG_LOG(SG_ATC, SG_BULK, aiObject->getId() << " blocked reversing by " << other->getId() << " Dist " << distM << " Headingdiff " << headingDiff << " Other heading diff " << otherHeadingDiff);
104 return true;
105 }
106 if (other->getSpeed() == 0 && abs(headingDiff) < 20 && abs(otherHeadingDiff) > 90) {
107 // in front or other is stopped which means other heading is not relevant
108 SG_LOG(SG_ATC, SG_WARN, aiObject->getId() << " blocked by stopped opposing " << other->getId() << " Dist " << distM << " Headingdiff " << headingDiff << " Other heading diff " << otherHeadingDiff);
109 return true;
110 }
111 if (other->getSpeed() >= 0 && abs(headingDiff) < 20 && abs(otherHeadingDiff) < 30) {
112 // in front and pointing away
113 SG_LOG(SG_ATC, SG_BULK, aiObject->getId() << " blocked by stopped pointing away" << other->getId() << " Dist " << distM << " Headingdiff " << headingDiff << " Other heading diff " << otherHeadingDiff);
114 return true;
115 }
116 return false;
117}
118
119bool AirportGroundRadar::isBlocked(FGTrafficRef aiObject)
120{
121 auto values = std::vector<FGTrafficRef>();
122 const SGRectd queryBox(aiObject->getPos().getLatitudeDeg()-QUERY_BOX_SIZE/2,
123 aiObject->getPos().getLongitudeDeg()-QUERY_BOX_SIZE/2,
124 QUERY_BOX_SIZE,
125 QUERY_BOX_SIZE);
126 SG_LOG(SG_ATC, SG_BULK, "Rect : ( " << queryBox.x() << "," << queryBox.y() << "x" << queryBox.width() << "," << queryBox.height() << ")");
127 index.query(queryBox, values);
128 SG_LOG(SG_ATC, SG_BULK, "Search Id : " << aiObject->getCallsign() << "(" << aiObject->getId() << ") Index Size : " << index.size() << " Result Size : " << values.size() );
129 for (FGTrafficRef other: values) {
130 double distM = SGGeodesy::distanceM(aiObject->getPos(), other->getPos());
131 if (other->getId()!=aiObject->getId()){
132
133 const double courseTowardOther = SGGeodesy::courseDeg(aiObject->getPos(), other->getPos());
134 // For right before left priority
135 const double headingDiff = SGMiscd::normalizePeriodic(-180, 180, aiObject->getHeading() - courseTowardOther);
136 const double otherHeadingDiff = SGMiscd::normalizePeriodic(-180, 180, other->getHeading() - courseTowardOther);
137 SG_LOG(SG_ATC, SG_BULK, "Found " << other->getCallsign() << "(" << other->getId() << ") " << other->getPos().getLatitudeDeg() << other->getPos().getLongitudeDeg() << "Dist " << distM << " Headingdiff " << headingDiff << " Other heading diff " << otherHeadingDiff << " courseTowardOther " << courseTowardOther);
138 const int threshold = getSize(aiObject) + getSize(other) + SEPARATION;
139 if ( distM < threshold ) {
140 if (headingDiff < 0 && aiObject->getSpeed() > 0 && abs(headingDiff) < 90){
141 // from the right and in front or other is stopped
142 SG_LOG(SG_ATC, SG_BULK, aiObject->getId() << " blocked by " << other->getId() << " Dist " << distM << " Headingdiff " << headingDiff << " Other heading diff " << otherHeadingDiff << " Heading " << aiObject->getHeading() << " Other Heading " << other->getHeading());
143 return true;
144 }
145 if (headingDiff < 0 && aiObject->getSpeed() < 0 && abs(headingDiff) > 90){
146 // from the right and in front or other is stopped
147 SG_LOG(SG_ATC, SG_BULK, aiObject->getId() << " blocked by " << other->getId() << " while reversing Dist " << distM << " Headingdiff " << headingDiff << " Other heading diff " << otherHeadingDiff);
148 return true;
149 }
150 if (other->getSpeed() == 0 && abs(headingDiff) < 5) {
151 // from the right and in front or other is stopped
152 SG_LOG(SG_ATC, SG_BULK, aiObject->getId() << " blocked by stopped " << other->getId() << " Dist " << distM << " Headingdiff " << headingDiff << " Other heading diff " << otherHeadingDiff);
153 return true;
154 }
155 }
156 } else {
157 if (distM>10)
158 {
159 SG_LOG(SG_ATC, SG_ALERT, aiObject->getCallsign() << "(" << aiObject->getId() << ") is not near it's shadow in index " << other->getId() << " Dist " << distM );
160 }
161
162 }
163 }
164 return false;
165}
166
170
171bool AirportGroundRadar::isBlockedForPushback(FGTrafficRef aiObject)
172{
173 auto values = std::vector<FGTrafficRef>();
174 const SGRectd queryBox(aiObject->getPos().getLatitudeDeg()-QUERY_BOX_SIZE/2,
175 aiObject->getPos().getLongitudeDeg()-QUERY_BOX_SIZE/2,
176 QUERY_BOX_SIZE,
177 QUERY_BOX_SIZE);
178 SG_LOG(SG_ATC, SG_BULK, "Rect : ( " << queryBox.x() << "," << queryBox.y() << "x" << queryBox.width() << "," << queryBox.height() << ")");
179 index.query(queryBox, values);
180 SG_LOG(SG_ATC, SG_BULK, "Search Id : " << aiObject->getId() << " Index Size : " << index.size() << " Result Size : " << values.size() );
181 for (FGTrafficRef other: values) {
182 if (other->getId()!=aiObject->getId()){
183 double distM = SGGeodesy::distanceM(aiObject->getPos(), other->getPos());
184
185 const double courseTowardOther = SGGeodesy::courseDeg(aiObject->getPos(), other->getPos());
186 // For right before left priority
187 const double headingDiff = SGMiscd::normalizePeriodic(-180, 180, aiObject->getHeading() - courseTowardOther);
188 const double otherHeadingDiff = SGMiscd::normalizePeriodic(-180, 180, other->getHeading() - courseTowardOther);
189
190 // We want ample space
191 const int threshold = 2 * getSize(aiObject) + 2 * getSize(other) + SEPARATION;
192 SG_LOG(SG_ATC, SG_BULK, "Search Id : " << aiObject->getId() << " Found Id : " << other->getId() << " Dist \t" << distM << "m Threshold " << threshold << " Headingdiff " << headingDiff << " Other heading diff " << otherHeadingDiff << " courseTowardOther " << courseTowardOther << " Turning " << aiObject->getHeadingDiff() << " Speeds : " << aiObject->getSpeed() << "/" << other->getSpeed() << " " << (other->getSpeed()==0?"Other Stopped":""));
193
194 if ( distM < threshold && (abs(headingDiff) > 135) ){
195 // from the right and in front or other is stopped
196 SG_LOG(SG_ATC, SG_BULK, aiObject->getCallsign() << "(" << aiObject->getId() << ") blocked for pushback by " << other->getCallsign() << "(" << other->getId() << ")");
197 return true;
198 }
199 }
200 }
201 return false;
202}
203
204const FGTrafficRef AirportGroundRadar::getBlockedBy(FGTrafficRef aiObject)
205{
206 auto values = std::vector<FGTrafficRef>();
207 const SGRectd queryBox(aiObject->getPos().getLatitudeDeg()-QUERY_BOX_SIZE/2,
208 aiObject->getPos().getLongitudeDeg()-QUERY_BOX_SIZE/2,
209 QUERY_BOX_SIZE,
210 QUERY_BOX_SIZE);
211 SG_LOG(SG_ATC, SG_BULK, "Rect : ( " << queryBox.x() << "," << queryBox.y() << "x" << queryBox.width() << "," << queryBox.height() << ")");
212 index.query(queryBox, values);
213 SG_LOG(SG_ATC, SG_BULK, "Search Id : " << aiObject->getId() << " Index Size : " << index.size() << " Result Size : " << values.size() );
214 FGTrafficRef nearestTrafficRecord = nullptr;
215 double nearestDist = HUGE_VAL;
216 for (FGTrafficRef other : values) {
217 double distM = SGGeodesy::distanceM(aiObject->getPos(), other->getPos());
218 const double threshold = getSize(aiObject) + getSize(other);
219 if (other->getId() != aiObject->getId()) {
220 if (distM < 20 && aiObject->getSpeed() != 0) {
221 const double courseTowardOther = SGGeodesy::courseDeg(aiObject->getPos(), other->getPos());
222 const double headingDiff = SGMiscd::normalizePeriodic(-180, 180, aiObject->getHeading() - courseTowardOther);
223 const double otherHeadingDiff = SGMiscd::normalizePeriodic(-180, 180, other->getHeading() - courseTowardOther);
224 // We can't have aircraft < 10m of each other
225 SG_LOG(SG_ATC, SG_ALERT, aiObject->getCallsign() << "(" << aiObject->getId() << ") running into " << other->getCallsign() << "(" << other->getId() << ") Dist " << distM << " Heading " << aiObject->getHeading() << " Other Heading " << other->getHeading() << " Headingdiff " << headingDiff << " Other heading diff " << otherHeadingDiff << " courseTowardOther " << courseTowardOther << " Speeds : " << aiObject->getSpeed() << "/" << other->getSpeed() << " Turning: " << aiObject->getHeadingDiff() << " Legs: " << aiObject->getLeg() << "/" << other->getLeg() << " Threshold : " << threshold << " = " << getSize(aiObject) << " + " << getSize(other));
226 if (aiObject->getAircraft() != nullptr && other->getAircraft() != nullptr) {
227 SG_LOG(SG_ATC, SG_ALERT, "Offending type " << aiObject->getAircraft()->getAcType() << " " << aiObject->getAircraft()->getCompany() << " " << aiObject->getAircraft()->getPerformance()->decelerationOnGround());
228 SG_LOG(SG_ATC, SG_ALERT, "Speeds " << aiObject->getSpeed() << " " << other->getAircraft()->getSpeed());
229 }
230 }
231 if (distM < threshold && distM < nearestDist) {
232 if (blocking(aiObject, other)) {
233 nearestDist = distM;
234 nearestTrafficRecord = other;
235 }
236 }
237 } else {
238 if (distM > 10) {
239 SG_LOG(SG_ATC, SG_ALERT, aiObject->getCallsign() << "(" << aiObject->getId() << ") is not near it's shadow in index Leg : " << aiObject->getLeg() << "/" << other->getLeg() << " Dist " << distM);
240 }
241 }
242 }
243 return nearestTrafficRecord;
244}
FGTrafficRef
SGSharedPtr< FGTrafficRecord > FGTrafficRef
Definition AirportGroundRadar.hxx:18
FGAirportRef
SGSharedPtr< FGAirport > FGAirportRef
Definition airports_fwd.hxx:76
AirportGroundRadar::remove
bool remove(FGTrafficRef aiObject)
Definition AirportGroundRadar.cxx:60
AirportGroundRadar::isBlockedForPushback
bool isBlockedForPushback(FGTrafficRef aiObject)
Check if the aircraft could push back.
Definition AirportGroundRadar.cxx:171
AirportGroundRadar::add
bool add(FGTrafficRef aiObject)
Definition AirportGroundRadar.cxx:39
AirportGroundRadar::move
bool move(const SGRectd &newPos, FGTrafficRef aiObject)
Definition AirportGroundRadar.cxx:51
AirportGroundRadar::getBox
static SGRect< double > getBox(FGTrafficRef aiObject)
Function implementing calculation of dimension for Quadtree.
Definition AirportGroundRadar.hxx:28
AirportGroundRadar::equal
static bool equal(FGTrafficRef o, FGTrafficRef o2)
Function implementing equals for Quadtree.
Definition AirportGroundRadar.hxx:38
AirportGroundRadar::getBlockedBy
const FGTrafficRef getBlockedBy(FGTrafficRef aiObject)
Returns which AI object is blocking this traffic.
Definition AirportGroundRadar.cxx:204
AirportGroundRadar::isBlocked
bool isBlocked(FGTrafficRef aiObject)
Returns if this AI object is blocked by any other "known" aka visible to the Radar.
Definition AirportGroundRadar.cxx:119
AirportGroundRadar::AirportGroundRadar
AirportGroundRadar(SGGeod min, SGGeod max)
Definition AirportGroundRadar.cxx:19
AILeg::STARTUP_PUSHBACK
@ STARTUP_PUSHBACK
Definition AIConstants.hxx:13
yasim::abs
float abs(float f)
Definition Airplane.cpp:21