src/Airports/groundnetwork.cxx

   1 // groundnet.cxx - Implimentation of the FlightGear airport ground handling code
   2 //
   3 // Written by Durk Talsma, started June 2005.
   4 //
   5 // Copyright (C) 2004 Durk Talsma.
   6 //
   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.
  11 //
  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.
  16 //
  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.
  20 //
  21 // $Id$
  22
  23 #ifdef HAVE_CONFIG_H
  24 #  include <config.h>
  25 #endif
  26
  27 #include "groundnetwork.hxx"
  28
  29 #include <cmath>
  30 #include <algorithm>
  31 #include <fstream>
  32 #include <map>
  33 #include <boost/foreach.hpp>
  34
  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>
  39
  40 #include <Airports/airport.hxx>
  41 #include <Airports/dynamics.hxx>
  42 #include <Airports/runways.hxx>
  43
  44 #include <Scenery/scenery.hxx>
  45
  46 using std::string;
  47 using flightgear::NavDataCache;
  48
  49 /***************************************************************************
  50  * FGTaxiSegment
  51  **************************************************************************/
  52
  53 FGTaxiSegment::FGTaxiSegment(FGTaxiNode* aStart, FGTaxiNode* aEnd) :
  54   startNode(aStart),
  55   endNode(aEnd),
  56   isActive(0),
  57   index(0),
  58   oppositeDirection(0)
  59 {
  60     if (!aStart || !aEnd) {
  61         throw sg_exception("Missing node arguments creating FGTaxiSegment");
  62     }
  63 }
  64
  65 SGGeod FGTaxiSegment::getCenter() const
  66 {
  67   FGTaxiNode* start(getStart()), *end(getEnd());
  68   double heading, length, az2;
  69   SGGeodesy::inverse(start->geod(), end->geod(), heading, az2, length);
  70   return SGGeodesy::direct(start->geod(), heading, length * 0.5);
  71 }
  72
  73 FGTaxiNodeRef FGTaxiSegment::getEnd() const
  74 {
  75   return const_cast<FGTaxiNode*>(endNode);
  76 }
  77
  78 FGTaxiNodeRef FGTaxiSegment::getStart() const
  79 {
  80   return const_cast<FGTaxiNode*>(startNode);
  81 }
  82
  83 double FGTaxiSegment::getLength() const
  84 {
  85   return dist(getStart()->cart(), getEnd()->cart());
  86 }
  87
  88 double FGTaxiSegment::getHeading() const
  89 {
  90   return SGGeodesy::courseDeg(getStart()->geod(), getEnd()->geod());
  91 }
  92
  93
  94 void FGTaxiSegment::block(int id, time_t blockTime, time_t now)
  95 {
  96     BlockListIterator i = blockTimes.begin();
  97     while (i != blockTimes.end()) {
  98         if (i->getId() == id)
  99             break;
 100         i++;
 101     }
 102     if (i == blockTimes.end()) {
 103         blockTimes.push_back(Block(id, blockTime, now));
 104         sort(blockTimes.begin(), blockTimes.end());
 105     } else {
 106         i->updateTimeStamps(blockTime, now);
 107     }
 108 }
 109
 110 // The segment has a block if any of the block times listed in the block list is
 111 // smaller than the current time.
 112 bool FGTaxiSegment::hasBlock(time_t now)
 113 {
 114     for (BlockListIterator i = blockTimes.begin(); i != blockTimes.end(); i++) {
 115         if (i->getBlockTime() < now)
 116             return true;
 117     }
 118     return false;
 119 }
 120
 121 void FGTaxiSegment::unblock(time_t now)
 122 {
 123     if (blockTimes.size()) {
 124         BlockListIterator i = blockTimes.begin();
 125         if (i->getTimeStamp() < (now - 30)) {
 126             blockTimes.erase(i);
 127         }
 128     }
 129 }
 130
 131
 132
 133 /***************************************************************************
 134  * FGTaxiRoute
 135  **************************************************************************/
 136 bool FGTaxiRoute::next(FGTaxiNodeRef& node, int *rte)
 137 {
 138     if (nodes.size() != (routes.size()) + 1) {
 139         SG_LOG(SG_GENERAL, SG_ALERT, "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size());
 140         throw sg_range_exception("Misconfigured taxi route");
 141     }
 142     if (currNode == nodes.end())
 143         return false;
 144     node = *(currNode);
 145     if (currNode != nodes.begin()) {
 146         *rte = *(currRoute);
 147         currRoute++;
 148     } else {
 149         // Handle special case for the first node.
 150         *rte = -1 * *(currRoute);
 151     }
 152     currNode++;
 153     return true;
 154 };
 155
 156 /***************************************************************************
 157  * FGGroundNetwork()
 158  **************************************************************************/
 159
 160 FGGroundNetwork::FGGroundNetwork() :
 161     dynamics(NULL),
 162     parent(NULL)
 163 {
 164     hasNetwork = false;
 165     version = 0;
 166     networkInitialized = false;
 167 }
 168
 169 FGGroundNetwork::~FGGroundNetwork()
 170 {
 171
 172   BOOST_FOREACH(FGTaxiSegment* seg, segments) {
 173     delete seg;
 174   }
 175
 176   // owning references to ground-net nodes will also drop
 177 }
 178
 179 void FGGroundNetwork::init(FGAirportDynamics* dyn)
 180 {
 181     if (networkInitialized) {
 182         SG_LOG(SG_GENERAL, SG_WARN, "duplicate ground-network init");
 183         return;
 184     }
 185
 186     dynamics = dyn;
 187     parent = dyn->parent();
 188     assert(parent);
 189     hasNetwork = true;
 190     int index = 1;
 191
 192   // establish pairing of segments
 193     BOOST_FOREACH(FGTaxiSegment* segment, segments) {
 194       segment->setIndex(index++);
 195
 196       if (segment->oppositeDirection) {
 197         continue; // already established
 198       }
 199
 200       FGTaxiSegment* opp = findSegment(segment->endNode, segment->startNode);
 201       if (opp) {
 202         assert(opp->oppositeDirection == NULL);
 203         segment->oppositeDirection = opp;
 204         opp->oppositeDirection = segment;
 205       }
 206     }
 207
 208     networkInitialized = true;
 209 }
 210
 211 FGTaxiNodeRef FGGroundNetwork::findNearestNode(const SGGeod & aGeod) const
 212 {
 213     double d = DBL_MAX;
 214     SGVec3d cartPos = SGVec3d::fromGeod(aGeod);
 215     FGTaxiNodeRef result;
 216
 217     FGTaxiNodeVector::const_iterator it;
 218     for (it = m_nodes.begin(); it != m_nodes.end(); ++it) {
 219         double localDistanceSqr = distSqr(cartPos, (*it)->cart());
 220         if (localDistanceSqr < d) {
 221             d = localDistanceSqr;
 222             result = *it;
 223         }
 224     }
 225
 226     return result;
 227 }
 228
 229 FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGRunway* aRunway) const
 230 {
 231     SG_UNUSED(aRunway);
 232
 233     double d = DBL_MAX;
 234     SGVec3d cartPos = SGVec3d::fromGeod(aGeod);
 235     FGTaxiNodeRef result = 0;
 236     FGTaxiNodeVector::const_iterator it;
 237     for (it = m_nodes.begin(); it != m_nodes.end(); ++it) {
 238         if (!(*it)->getIsOnRunway())
 239             continue;
 240
 241         double localDistanceSqr = distSqr(cartPos, (*it)->cart());
 242         if (localDistanceSqr < d) {
 243             d = localDistanceSqr;
 244             result = *it;
 245         }
 246     }
 247
 248     return result;
 249 }
 250
 251 FGTaxiSegment *FGGroundNetwork::findOppositeSegment(unsigned int index) const
 252 {
 253     FGTaxiSegment* seg = findSegment(index);
 254     if (!seg)
 255         return NULL;
 256     return seg->opposite();
 257 }
 258
 259 const FGParkingList &FGGroundNetwork::allParkings() const
 260 {
 261     return m_parkings;
 262 }
 263
 264 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx) const
 265 {
 266     if ((idx > 0) && (idx <= segments.size()))
 267         return segments[idx - 1];
 268     else {
 269         //cerr << "Alert: trying to find invalid segment " << idx << endl;
 270         return 0;
 271     }
 272 }
 273
 274 FGTaxiSegment* FGGroundNetwork::findSegment(const FGTaxiNode* from, const FGTaxiNode* to) const
 275 {
 276   if (from == 0) {
 277     return NULL;
 278   }
 279
 280   // completely boring linear search of segments. Can be improved if/when
 281   // this ever becomes a hot-spot
 282     BOOST_FOREACH(FGTaxiSegment* seg, segments) {
 283       if (seg->startNode != from) {
 284         continue;
 285       }
 286
 287       if ((to == 0) || (seg->endNode == to)) {
 288         return seg;
 289       }
 290     }
 291
 292     return NULL; // not found
 293 }
 294
 295 static int edgePenalty(FGTaxiNode* tn)
 296 {
 297   return (tn->type() == FGPositioned::PARKING ? 10000 : 0) +
 298     (tn->getIsOnRunway() ? 1000 : 0);
 299 }
 300
 301 class ShortestPathData
 302 {
 303 public:
 304   ShortestPathData() :
 305     score(HUGE_VAL)
 306   {}
 307
 308   double score;
 309   FGTaxiNodeRef previousNode;
 310 };
 311
 312 FGTaxiRoute FGGroundNetwork::findShortestRoute(FGTaxiNode* start, FGTaxiNode* end, bool fullSearch)
 313 {
 314     if (!start || !end) {
 315         throw sg_exception("Bad arguments to findShortestRoute");
 316     }
 317 //implements Dijkstra's algorithm to find shortest distance route from start to end
 318 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
 319     FGTaxiNodeVector unvisited(m_nodes);
 320     std::map<FGTaxiNode*, ShortestPathData> searchData;
 321
 322     searchData[start].score = 0.0;
 323
 324     while (!unvisited.empty()) {
 325         // find lowest scored unvisited
 326         FGTaxiNodeRef best = unvisited.front();
 327         BOOST_FOREACH(FGTaxiNodeRef i, unvisited) {
 328             if (searchData[i].score < searchData[best].score) {
 329                 best = i;
 330             }
 331         }
 332
 333       // remove 'best' from the unvisited set
 334         FGTaxiNodeVectorIterator newend =
 335             remove(unvisited.begin(), unvisited.end(), best);
 336         unvisited.erase(newend, unvisited.end());
 337
 338         if (best == end) { // found route or best not connected
 339             break;
 340         }
 341
 342         BOOST_FOREACH(FGTaxiNodeRef target, segmentsFrom(best)) {
 343             double edgeLength = dist(best->cart(), target->cart());
 344             double alt = searchData[best].score + edgeLength + edgePenalty(target);
 345             if (alt < searchData[target].score) {    // Relax (u,v)
 346                 searchData[target].score = alt;
 347                 searchData[target].previousNode = best;
 348             }
 349         } // of outgoing arcs/segments from current best node iteration
 350     } // of unvisited nodes remaining
 351
 352     if (searchData[end].score == HUGE_VAL) {
 353         // no valid route found
 354         if (fullSearch) {
 355             SG_LOG(SG_GENERAL, SG_ALERT,
 356                    "Failed to find route from waypoint " << start << " to "
 357                    << end << " at " << parent->getId());
 358         }
 359
 360         return FGTaxiRoute();
 361     }
 362
 363     // assemble route from backtrace information
 364     FGTaxiNodeVector nodes;
 365     intVec routes;
 366     FGTaxiNode *bt = end;
 367
 368     while (searchData[bt].previousNode != 0) {
 369         nodes.push_back(bt);
 370         FGTaxiSegment *segment = findSegment(searchData[bt].previousNode, bt);
 371         int idx = segment->getIndex();
 372         routes.push_back(idx);
 373         bt = searchData[bt].previousNode;
 374
 375     }
 376     nodes.push_back(start);
 377     reverse(nodes.begin(), nodes.end());
 378     reverse(routes.begin(), routes.end());
 379     return FGTaxiRoute(nodes, routes, searchData[end].score, 0);
 380 }
 381
 382 void FGGroundNetwork::unblockAllSegments(time_t now)
 383 {
 384     FGTaxiSegmentVector::iterator tsi;
 385     for ( tsi = segments.begin(); tsi != segments.end(); tsi++) {
 386         (*tsi)->unblock(now);
 387     }
 388 }
 389
 390 void FGGroundNetwork::blockSegmentsEndingAt(FGTaxiSegment *seg, int blockId, time_t blockTime, time_t now)
 391 {
 392     if (!seg)
 393         throw sg_exception("Passed invalid segment");
 394
 395     FGTaxiNode *node = seg->getEnd();
 396     FGTaxiSegmentVector::iterator tsi;
 397     for ( tsi = segments.begin(); tsi != segments.end(); tsi++) {
 398         FGTaxiSegment* otherSegment = *tsi;
 399         if ((otherSegment->getEnd() == node) && (otherSegment != seg)) {
 400             otherSegment->block(blockId, blockTime, now);
 401         }
 402     }
 403 }
 404
 405 FGTaxiNodeRef FGGroundNetwork::findNodeByIndex(int index) const
 406 {
 407    FGTaxiNodeVector::const_iterator it;
 408    for (it = m_nodes.begin(); it != m_nodes.end(); ++it) {
 409        if ((*it)->getIndex() == index) {
 410            return *it;
 411        }
 412    }
 413
 414    return FGTaxiNodeRef();
 415 }
 416
 417 void FGGroundNetwork::addSegment(const FGTaxiNodeRef &from, const FGTaxiNodeRef &to)
 418 {
 419     FGTaxiSegment* seg = new FGTaxiSegment(from, to);
 420     segments.push_back(seg);
 421
 422     FGTaxiNodeVector::iterator it = std::find(m_nodes.begin(), m_nodes.end(), from);
 423     if (it == m_nodes.end()) {
 424         m_nodes.push_back(from);
 425     }
 426
 427     it = std::find(m_nodes.begin(), m_nodes.end(), to);
 428     if (it == m_nodes.end()) {
 429         m_nodes.push_back(to);
 430     }
 431 }
 432
 433 void FGGroundNetwork::addParking(const FGParkingRef &park)
 434 {
 435     m_parkings.push_back(park);
 436
 437
 438     FGTaxiNodeVector::iterator it = std::find(m_nodes.begin(), m_nodes.end(), park);
 439     if (it == m_nodes.end()) {
 440         m_nodes.push_back(park);
 441     }
 442 }
 443
 444 FGTaxiNodeVector FGGroundNetwork::segmentsFrom(const FGTaxiNodeRef &from) const
 445 {
 446     FGTaxiNodeVector result;
 447     FGTaxiSegmentVector::const_iterator it;
 448     for (it = segments.begin(); it != segments.end(); ++it) {
 449         if ((*it)->getStart() == from) {
 450             result.push_back((*it)->getEnd());
 451         }
 452     }
 453
 454     return result;
 455 }
 456