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