1 // groundnet.cxx - Implimentation of the FlightGear airport ground handling code
3 // Written by Durk Talsma, started June 2005.
5 // Copyright (C) 2004 Durk Talsma.
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
27 #include "groundnetwork.hxx"
33 #include <boost/foreach.hpp>
35 #include <simgear/debug/logstream.hxx>
36 #include <simgear/scene/util/OsgMath.hxx>
37 #include <simgear/structure/exception.hxx>
38 #include <simgear/timing/timestamp.hxx>
40 #include <Airports/airport.hxx>
41 #include <Airports/runways.hxx>
43 #include <Scenery/scenery.hxx>
47 /***************************************************************************
49 **************************************************************************/
51 FGTaxiSegment::FGTaxiSegment(FGTaxiNode* aStart, FGTaxiNode* aEnd) :
58 if (!aStart || !aEnd) {
59 throw sg_exception("Missing node arguments creating FGTaxiSegment");
63 SGGeod FGTaxiSegment::getCenter() const
65 FGTaxiNode* start(getStart()), *end(getEnd());
66 double heading, length, az2;
67 SGGeodesy::inverse(start->geod(), end->geod(), heading, az2, length);
68 return SGGeodesy::direct(start->geod(), heading, length * 0.5);
71 FGTaxiNodeRef FGTaxiSegment::getEnd() const
73 return const_cast<FGTaxiNode*>(endNode);
76 FGTaxiNodeRef FGTaxiSegment::getStart() const
78 return const_cast<FGTaxiNode*>(startNode);
81 double FGTaxiSegment::getLength() const
83 return dist(getStart()->cart(), getEnd()->cart());
86 double FGTaxiSegment::getHeading() const
88 return SGGeodesy::courseDeg(getStart()->geod(), getEnd()->geod());
92 void FGTaxiSegment::block(int id, time_t blockTime, time_t now)
94 BlockListIterator i = blockTimes.begin();
95 while (i != blockTimes.end()) {
100 if (i == blockTimes.end()) {
101 blockTimes.push_back(Block(id, blockTime, now));
102 sort(blockTimes.begin(), blockTimes.end());
104 i->updateTimeStamps(blockTime, now);
108 // The segment has a block if any of the block times listed in the block list is
109 // smaller than the current time.
110 bool FGTaxiSegment::hasBlock(time_t now)
112 for (BlockListIterator i = blockTimes.begin(); i != blockTimes.end(); i++) {
113 if (i->getBlockTime() < now)
119 void FGTaxiSegment::unblock(time_t now)
121 if (blockTimes.size()) {
122 BlockListIterator i = blockTimes.begin();
123 if (i->getTimeStamp() < (now - 30)) {
131 /***************************************************************************
133 **************************************************************************/
134 bool FGTaxiRoute::next(FGTaxiNodeRef& node, int *rte)
136 if (nodes.size() != (routes.size()) + 1) {
137 SG_LOG(SG_GENERAL, SG_ALERT, "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size());
138 throw sg_range_exception("Misconfigured taxi route");
140 if (currNode == nodes.end())
143 if (currNode != nodes.begin()) {
147 // Handle special case for the first node.
148 *rte = -1 * *(currRoute);
154 /***************************************************************************
156 **************************************************************************/
158 FGGroundNetwork::FGGroundNetwork(FGAirport* airport) :
163 networkInitialized = false;
166 FGGroundNetwork::~FGGroundNetwork()
169 BOOST_FOREACH(FGTaxiSegment* seg, segments) {
173 // owning references to ground-net nodes will also drop
175 SG_LOG(SG_NAVAID, SG_INFO, "destroying ground net for " << parent->ident());
178 void FGGroundNetwork::init()
180 if (networkInitialized) {
181 SG_LOG(SG_GENERAL, SG_WARN, "duplicate ground-network init");
188 // establish pairing of segments
189 BOOST_FOREACH(FGTaxiSegment* segment, segments) {
190 segment->setIndex(index++);
192 if (segment->oppositeDirection) {
193 continue; // already established
196 FGTaxiSegment* opp = findSegment(segment->endNode, segment->startNode);
198 assert(opp->oppositeDirection == NULL);
199 segment->oppositeDirection = opp;
200 opp->oppositeDirection = segment;
204 networkInitialized = true;
207 FGTaxiNodeRef FGGroundNetwork::findNearestNode(const SGGeod & aGeod) const
210 SGVec3d cartPos = SGVec3d::fromGeod(aGeod);
211 FGTaxiNodeRef result;
213 FGTaxiNodeVector::const_iterator it;
214 for (it = m_nodes.begin(); it != m_nodes.end(); ++it) {
215 double localDistanceSqr = distSqr(cartPos, (*it)->cart());
216 if (localDistanceSqr < d) {
217 d = localDistanceSqr;
225 FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGRunway* aRunway) const
230 SGVec3d cartPos = SGVec3d::fromGeod(aGeod);
231 FGTaxiNodeRef result = 0;
232 FGTaxiNodeVector::const_iterator it;
233 for (it = m_nodes.begin(); it != m_nodes.end(); ++it) {
234 if (!(*it)->getIsOnRunway())
237 double localDistanceSqr = distSqr(cartPos, (*it)->cart());
238 if (localDistanceSqr < d) {
239 d = localDistanceSqr;
247 FGTaxiSegment *FGGroundNetwork::findOppositeSegment(unsigned int index) const
249 FGTaxiSegment* seg = findSegment(index);
252 return seg->opposite();
255 const FGParkingList &FGGroundNetwork::allParkings() const
260 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx) const
262 if ((idx > 0) && (idx <= segments.size()))
263 return segments[idx - 1];
265 //cerr << "Alert: trying to find invalid segment " << idx << endl;
270 FGTaxiSegment* FGGroundNetwork::findSegment(const FGTaxiNode* from, const FGTaxiNode* to) const
276 // completely boring linear search of segments. Can be improved if/when
277 // this ever becomes a hot-spot
278 BOOST_FOREACH(FGTaxiSegment* seg, segments) {
279 if (seg->startNode != from) {
283 if ((to == 0) || (seg->endNode == to)) {
288 return NULL; // not found
291 static int edgePenalty(FGTaxiNode* tn)
293 return (tn->type() == FGPositioned::PARKING ? 10000 : 0) +
294 (tn->getIsOnRunway() ? 1000 : 0);
297 class ShortestPathData
305 FGTaxiNodeRef previousNode;
308 FGTaxiRoute FGGroundNetwork::findShortestRoute(FGTaxiNode* start, FGTaxiNode* end, bool fullSearch)
310 if (!start || !end) {
311 throw sg_exception("Bad arguments to findShortestRoute");
313 //implements Dijkstra's algorithm to find shortest distance route from start to end
314 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
315 FGTaxiNodeVector unvisited(m_nodes);
316 std::map<FGTaxiNode*, ShortestPathData> searchData;
318 searchData[start].score = 0.0;
320 while (!unvisited.empty()) {
321 // find lowest scored unvisited
322 FGTaxiNodeRef best = unvisited.front();
323 BOOST_FOREACH(FGTaxiNodeRef i, unvisited) {
324 if (searchData[i].score < searchData[best].score) {
329 // remove 'best' from the unvisited set
330 FGTaxiNodeVectorIterator newend =
331 remove(unvisited.begin(), unvisited.end(), best);
332 unvisited.erase(newend, unvisited.end());
334 if (best == end) { // found route or best not connected
338 BOOST_FOREACH(FGTaxiNodeRef target, segmentsFrom(best)) {
339 double edgeLength = dist(best->cart(), target->cart());
340 double alt = searchData[best].score + edgeLength + edgePenalty(target);
341 if (alt < searchData[target].score) { // Relax (u,v)
342 searchData[target].score = alt;
343 searchData[target].previousNode = best;
345 } // of outgoing arcs/segments from current best node iteration
346 } // of unvisited nodes remaining
348 if (searchData[end].score == HUGE_VAL) {
349 // no valid route found
351 SG_LOG(SG_GENERAL, SG_ALERT,
352 "Failed to find route from waypoint " << start << " to "
353 << end << " at " << parent->getId());
356 return FGTaxiRoute();
359 // assemble route from backtrace information
360 FGTaxiNodeVector nodes;
362 FGTaxiNode *bt = end;
364 while (searchData[bt].previousNode != 0) {
366 FGTaxiSegment *segment = findSegment(searchData[bt].previousNode, bt);
367 int idx = segment->getIndex();
368 routes.push_back(idx);
369 bt = searchData[bt].previousNode;
372 nodes.push_back(start);
373 reverse(nodes.begin(), nodes.end());
374 reverse(routes.begin(), routes.end());
375 return FGTaxiRoute(nodes, routes, searchData[end].score, 0);
378 void FGGroundNetwork::unblockAllSegments(time_t now)
380 FGTaxiSegmentVector::iterator tsi;
381 for ( tsi = segments.begin(); tsi != segments.end(); tsi++) {
382 (*tsi)->unblock(now);
386 void FGGroundNetwork::blockSegmentsEndingAt(FGTaxiSegment *seg, int blockId, time_t blockTime, time_t now)
389 throw sg_exception("Passed invalid segment");
391 FGTaxiNode *node = seg->getEnd();
392 FGTaxiSegmentVector::iterator tsi;
393 for ( tsi = segments.begin(); tsi != segments.end(); tsi++) {
394 FGTaxiSegment* otherSegment = *tsi;
395 if ((otherSegment->getEnd() == node) && (otherSegment != seg)) {
396 otherSegment->block(blockId, blockTime, now);
401 FGTaxiNodeRef FGGroundNetwork::findNodeByIndex(int index) const
403 FGTaxiNodeVector::const_iterator it;
404 for (it = m_nodes.begin(); it != m_nodes.end(); ++it) {
405 if ((*it)->getIndex() == index) {
410 return FGTaxiNodeRef();
413 void FGGroundNetwork::addSegment(const FGTaxiNodeRef &from, const FGTaxiNodeRef &to)
415 FGTaxiSegment* seg = new FGTaxiSegment(from, to);
416 segments.push_back(seg);
418 FGTaxiNodeVector::iterator it = std::find(m_nodes.begin(), m_nodes.end(), from);
419 if (it == m_nodes.end()) {
420 m_nodes.push_back(from);
423 it = std::find(m_nodes.begin(), m_nodes.end(), to);
424 if (it == m_nodes.end()) {
425 m_nodes.push_back(to);
429 void FGGroundNetwork::addParking(const FGParkingRef &park)
431 m_parkings.push_back(park);
434 FGTaxiNodeVector::iterator it = std::find(m_nodes.begin(), m_nodes.end(), park);
435 if (it == m_nodes.end()) {
436 m_nodes.push_back(park);
440 FGTaxiNodeVector FGGroundNetwork::segmentsFrom(const FGTaxiNodeRef &from) const
442 FGTaxiNodeVector result;
443 FGTaxiSegmentVector::const_iterator it;
444 for (it = segments.begin(); it != segments.end(); ++it) {
445 if ((*it)->getStart() == from) {
446 result.push_back((*it)->getEnd());
453 const intVec& FGGroundNetwork::getTowerFrequencies() const
458 const intVec& FGGroundNetwork::getGroundFrequencies() const