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/runways.hxx>
  42
  43 #include <Scenery/scenery.hxx>
  44
  45 using std::string;
  46
  47 /***************************************************************************
  48  * FGTaxiSegment
  49  **************************************************************************/
  50
  51 FGTaxiSegment::FGTaxiSegment(FGTaxiNode* aStart, FGTaxiNode* aEnd) :
  52   startNode(aStart),
  53   endNode(aEnd),
  54   isActive(0),
  55   index(0),
  56   oppositeDirection(0)
  57 {
  58     if (!aStart || !aEnd) {
  59         throw sg_exception("Missing node arguments creating FGTaxiSegment");
  60     }
  61 }
  62
  63 SGGeod FGTaxiSegment::getCenter() const
  64 {
  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);
  69 }
  70
  71 FGTaxiNodeRef FGTaxiSegment::getEnd() const
  72 {
  73   return const_cast<FGTaxiNode*>(endNode);
  74 }
  75
  76 FGTaxiNodeRef FGTaxiSegment::getStart() const
  77 {
  78   return const_cast<FGTaxiNode*>(startNode);
  79 }
  80
  81 double FGTaxiSegment::getLength() const
  82 {
  83   return dist(getStart()->cart(), getEnd()->cart());
  84 }
  85
  86 double FGTaxiSegment::getHeading() const
  87 {
  88   return SGGeodesy::courseDeg(getStart()->geod(), getEnd()->geod());
  89 }
  90
  91
  92 void FGTaxiSegment::block(int id, time_t blockTime, time_t now)
  93 {
  94     BlockListIterator i = blockTimes.begin();
  95     while (i != blockTimes.end()) {
  96         if (i->getId() == id)
  97             break;
  98         i++;
  99     }
 100     if (i == blockTimes.end()) {
 101         blockTimes.push_back(Block(id, blockTime, now));
 102         sort(blockTimes.begin(), blockTimes.end());
 103     } else {
 104         i->updateTimeStamps(blockTime, now);
 105     }
 106 }
 107
 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)
 111 {
 112     for (BlockListIterator i = blockTimes.begin(); i != blockTimes.end(); i++) {
 113         if (i->getBlockTime() < now)
 114             return true;
 115     }
 116     return false;
 117 }
 118
 119 void FGTaxiSegment::unblock(time_t now)
 120 {
 121     if (blockTimes.size()) {
 122         BlockListIterator i = blockTimes.begin();
 123         if (i->getTimeStamp() < (now - 30)) {
 124             blockTimes.erase(i);
 125         }
 126     }
 127 }
 128
 129
 130
 131 /***************************************************************************
 132  * FGTaxiRoute
 133  **************************************************************************/
 134 bool FGTaxiRoute::next(FGTaxiNodeRef& node, int *rte)
 135 {
 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");
 139     }
 140     if (currNode == nodes.end())
 141         return false;
 142     node = *(currNode);
 143     if (currNode != nodes.begin()) {
 144         *rte = *(currRoute);
 145         currRoute++;
 146     } else {
 147         // Handle special case for the first node.
 148         *rte = -1 * *(currRoute);
 149     }
 150     currNode++;
 151     return true;
 152 };
 153
 154 /***************************************************************************
 155  * FGGroundNetwork()
 156  **************************************************************************/
 157
 158 FGGroundNetwork::FGGroundNetwork(FGAirport* airport) :
 159     parent(airport)
 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   SG_LOG(SG_NAVAID, SG_INFO, "destroying ground net for " << parent->ident());
 176 }
 177
 178 void FGGroundNetwork::init()
 179 {
 180     if (networkInitialized) {
 181         SG_LOG(SG_GENERAL, SG_WARN, "duplicate ground-network init");
 182         return;
 183     }
 184
 185     hasNetwork = true;
 186     int index = 1;
 187
 188   // establish pairing of segments
 189     BOOST_FOREACH(FGTaxiSegment* segment, segments) {
 190       segment->setIndex(index++);
 191
 192       if (segment->oppositeDirection) {
 193         continue; // already established
 194       }
 195
 196       FGTaxiSegment* opp = findSegment(segment->endNode, segment->startNode);
 197       if (opp) {
 198         assert(opp->oppositeDirection == NULL);
 199         segment->oppositeDirection = opp;
 200         opp->oppositeDirection = segment;
 201       }
 202     }
 203
 204     networkInitialized = true;
 205 }
 206
 207 FGTaxiNodeRef FGGroundNetwork::findNearestNode(const SGGeod & aGeod) const
 208 {
 209     double d = DBL_MAX;
 210     SGVec3d cartPos = SGVec3d::fromGeod(aGeod);
 211     FGTaxiNodeRef result;
 212
 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;
 218             result = *it;
 219         }
 220     }
 221
 222     return result;
 223 }
 224
 225 FGTaxiNodeRef FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGRunway* aRunway) const
 226 {
 227     SG_UNUSED(aRunway);
 228
 229     double d = DBL_MAX;
 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())
 235             continue;
 236
 237         double localDistanceSqr = distSqr(cartPos, (*it)->cart());
 238         if (localDistanceSqr < d) {
 239             d = localDistanceSqr;
 240             result = *it;
 241         }
 242     }
 243
 244     return result;
 245 }
 246
 247 FGTaxiSegment *FGGroundNetwork::findOppositeSegment(unsigned int index) const
 248 {
 249     FGTaxiSegment* seg = findSegment(index);
 250     if (!seg)
 251         return NULL;
 252     return seg->opposite();
 253 }
 254
 255 const FGParkingList &FGGroundNetwork::allParkings() const
 256 {
 257     return m_parkings;
 258 }
 259
 260 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx) const
 261 {
 262     if ((idx > 0) && (idx <= segments.size()))
 263         return segments[idx - 1];
 264     else {
 265         //cerr << "Alert: trying to find invalid segment " << idx << endl;
 266         return 0;
 267     }
 268 }
 269
 270 FGTaxiSegment* FGGroundNetwork::findSegment(const FGTaxiNode* from, const FGTaxiNode* to) const
 271 {
 272   if (from == 0) {
 273     return NULL;
 274   }
 275
 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) {
 280         continue;
 281       }
 282
 283       if ((to == 0) || (seg->endNode == to)) {
 284         return seg;
 285       }
 286     }
 287
 288     return NULL; // not found
 289 }
 290
 291 static int edgePenalty(FGTaxiNode* tn)
 292 {
 293   return (tn->type() == FGPositioned::PARKING ? 10000 : 0) +
 294     (tn->getIsOnRunway() ? 1000 : 0);
 295 }
 296
 297 class ShortestPathData
 298 {
 299 public:
 300   ShortestPathData() :
 301     score(HUGE_VAL)
 302   {}
 303
 304   double score;
 305   FGTaxiNodeRef previousNode;
 306 };
 307
 308 FGTaxiRoute FGGroundNetwork::findShortestRoute(FGTaxiNode* start, FGTaxiNode* end, bool fullSearch)
 309 {
 310     if (!start || !end) {
 311         throw sg_exception("Bad arguments to findShortestRoute");
 312     }
 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;
 317
 318     searchData[start].score = 0.0;
 319
 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) {
 325                 best = i;
 326             }
 327         }
 328
 329       // remove 'best' from the unvisited set
 330         FGTaxiNodeVectorIterator newend =
 331             remove(unvisited.begin(), unvisited.end(), best);
 332         unvisited.erase(newend, unvisited.end());
 333
 334         if (best == end) { // found route or best not connected
 335             break;
 336         }
 337
 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;
 344             }
 345         } // of outgoing arcs/segments from current best node iteration
 346     } // of unvisited nodes remaining
 347
 348     if (searchData[end].score == HUGE_VAL) {
 349         // no valid route found
 350         if (fullSearch) {
 351             SG_LOG(SG_GENERAL, SG_ALERT,
 352                    "Failed to find route from waypoint " << start << " to "
 353                    << end << " at " << parent->getId());
 354         }
 355
 356         return FGTaxiRoute();
 357     }
 358
 359     // assemble route from backtrace information
 360     FGTaxiNodeVector nodes;
 361     intVec routes;
 362     FGTaxiNode *bt = end;
 363
 364     while (searchData[bt].previousNode != 0) {
 365         nodes.push_back(bt);
 366         FGTaxiSegment *segment = findSegment(searchData[bt].previousNode, bt);
 367         int idx = segment->getIndex();
 368         routes.push_back(idx);
 369         bt = searchData[bt].previousNode;
 370
 371     }
 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);
 376 }
 377
 378 void FGGroundNetwork::unblockAllSegments(time_t now)
 379 {
 380     FGTaxiSegmentVector::iterator tsi;
 381     for ( tsi = segments.begin(); tsi != segments.end(); tsi++) {
 382         (*tsi)->unblock(now);
 383     }
 384 }
 385
 386 void FGGroundNetwork::blockSegmentsEndingAt(FGTaxiSegment *seg, int blockId, time_t blockTime, time_t now)
 387 {
 388     if (!seg)
 389         throw sg_exception("Passed invalid segment");
 390
 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);
 397         }
 398     }
 399 }
 400
 401 FGTaxiNodeRef FGGroundNetwork::findNodeByIndex(int index) const
 402 {
 403    FGTaxiNodeVector::const_iterator it;
 404    for (it = m_nodes.begin(); it != m_nodes.end(); ++it) {
 405        if ((*it)->getIndex() == index) {
 406            return *it;
 407        }
 408    }
 409
 410    return FGTaxiNodeRef();
 411 }
 412
 413 void FGGroundNetwork::addSegment(const FGTaxiNodeRef &from, const FGTaxiNodeRef &to)
 414 {
 415     FGTaxiSegment* seg = new FGTaxiSegment(from, to);
 416     segments.push_back(seg);
 417
 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);
 421     }
 422
 423     it = std::find(m_nodes.begin(), m_nodes.end(), to);
 424     if (it == m_nodes.end()) {
 425         m_nodes.push_back(to);
 426     }
 427 }
 428
 429 void FGGroundNetwork::addParking(const FGParkingRef &park)
 430 {
 431     m_parkings.push_back(park);
 432
 433
 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);
 437     }
 438 }
 439
 440 FGTaxiNodeVector FGGroundNetwork::segmentsFrom(const FGTaxiNodeRef &from) const
 441 {
 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());
 447         }
 448     }
 449
 450     return result;
 451 }
 452
 453 const intVec& FGGroundNetwork::getTowerFrequencies() const
 454 {
 455     return freqTower;
 456 }
 457
 458 const intVec& FGGroundNetwork::getGroundFrequencies() const
 459 {
 460     return freqGround;
 461 }
 462