src/ATCDCL/ground.cxx

   1 // FGGround - a class to provide ground control at larger airports.
   2 //
   3 // Written by David Luff, started March 2002.
   4 //
   5 // Copyright (C) 2002  David C. Luff - david.luff@nottingham.ac.uk
   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 #ifdef HAVE_CONFIG_H
  22 #  include <config.h>
  23 #endif
  24
  25 #include <iostream>
  26
  27 #include <simgear/misc/sg_path.hxx>
  28 #include <simgear/math/sg_random.h>
  29 #include <simgear/debug/logstream.hxx>
  30 #include <simgear/misc/sgstream.hxx>
  31 #include <simgear/constants.h>
  32 #include <Main/globals.hxx>
  33
  34 #include <stdlib.h>
  35 #include <fstream>
  36
  37 #include "ground.hxx"
  38 #include "ATCutils.hxx"
  39 #include "ATCmgr.hxx"
  40
  41 using std::ifstream;
  42 using std::cout;
  43
  44 node::node() {
  45 }
  46
  47 node::~node() {
  48         for(unsigned int i=0; i < arcs.size(); ++i) {
  49                 delete arcs[i];
  50         }
  51 }
  52
  53 // Make sure that a_path.cost += distance is safe from the moment it's created.
  54 a_path::a_path() {
  55         cost = 0;
  56 }
  57
  58 FGGround::FGGround() {
  59         ATCmgr = globals->get_ATC_mgr();
  60         _type = GROUND;
  61         networkLoadOK = false;
  62         ground_traffic.erase(ground_traffic.begin(), ground_traffic.end());
  63         ground_traffic_itr = ground_traffic.begin();
  64
  65         // Init the property nodes - TODO - need to make sure we're getting surface winds.
  66         wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
  67         wind_speed_knots = fgGetNode("/environment/wind-speed-kt", true);
  68
  69         // TODO - get the actual airport elevation
  70         aptElev = 0.0;
  71 }
  72
  73 FGGround::FGGround(const string& id) {
  74         ATCmgr = globals->get_ATC_mgr();
  75         networkLoadOK = false;
  76         ground_traffic.erase(ground_traffic.begin(), ground_traffic.end());
  77         ground_traffic_itr = ground_traffic.begin();
  78         ident = id;
  79
  80         // Init the property nodes - TODO - need to make sure we're getting surface winds.
  81         wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
  82         wind_speed_knots = fgGetNode("/environment/wind-speed-kt", true);
  83
  84         // TODO - get the actual airport elevation
  85         aptElev = 0.0;
  86 }
  87
  88 FGGround::~FGGround() {
  89 }
  90
  91 void FGGround::ParseRwyExits(node* np, char* es) {
  92         char* token;
  93         char estr[20];
  94         strcpy(estr, es);
  95         const char delimiters[] = "-";
  96         token = strtok(estr, delimiters);
  97         while(token != NULL) {
  98                 int i = atoi(token);
  99                 //cout << "token = " << token << endl;
 100                 //cout << "rwy number = " << i << endl;
 101                 //runways[(atoi(token))].exits.push_back(np);
 102                 runways[i].exits.push_back(np);
 103                 //cout << "token = " << token << '\n';
 104                 token = strtok(NULL, delimiters);
 105         }
 106 }
 107
 108
 109 // Load the ground logical network of the current instances airport
 110 // Return true if successfull.
 111 // TODO - currently the file is assumed to reside in the base/ATC directory.
 112 // This might change to something more thought out in the future.
 113 // NOTE - currently it is assumed that all nodes are loaded before any arcs.
 114 // It won't work ATM if this doesn't hold true.
 115 bool FGGround::LoadNetwork() {
 116         node* np;
 117         arc* ap;
 118         Gate* gp;
 119
 120         int gateCount = 0;      // This is used to allocate gateID's from zero upwards
 121         // This may well change in the future - probably to reading in the real-world
 122         // gate numbers from file.
 123
 124         ifstream fin;
 125         SGPath path = globals->get_fg_root();
 126         //string taxiPath = "ATC/" + ident + ".taxi";
 127         string taxiPath = "ATC/KEMT.taxi";      // FIXME - HARDWIRED FOR TESTING
 128         path.append(taxiPath);
 129
 130         SG_LOG(SG_ATC, SG_INFO, "Trying to read taxiway data for " << ident << "...");
 131         //cout << "Trying to read taxiway data for " << ident << "..." << endl;
 132         fin.open(path.c_str(), ios::in);
 133         if(!fin) {
 134                 SG_LOG(SG_ATC, SG_ALERT, "Unable to open taxiway data input file " << path.c_str());
 135                 //cout << "Unable to open taxiway data input file " << path.c_str() << endl;
 136                 return(false);
 137         }
 138
 139         char ch;
 140         char buf[30];
 141         while(!fin.eof()) {
 142                 fin >> buf;
 143                 // Node, arc, or [End]?
 144                 //cout << "Read in ground network element type = " << buf << endl;
 145                 if(!strcmp(buf, "[End]")) {             // TODO - maybe make this more robust to spelling errors by just looking for '['
 146                         SG_LOG(SG_ATC, SG_INFO, "Done reading " << path.c_str() << endl);
 147                         break;
 148                 } else if(!strcmp(buf, "N")) {
 149                         // Node
 150                         np = new node;
 151                         np->struct_type = NODE;
 152                         fin >> buf;
 153                         np->nodeID = atoi(buf);
 154                         fin >> buf;
 155                         np->pos.setLongitudeDeg(atof(buf));
 156                         fin >> buf;
 157                         np->pos.setLatitudeDeg(atof(buf));
 158                         fin >> buf;
 159                         np->pos.setElevationM(atof(buf));
 160                         fin >> buf;             // node type
 161                         if(!strcmp(buf, "J")) {
 162                                 np->type = JUNCTION;
 163                         } else if(!strcmp(buf, "T")) {
 164                                 np->type = TJUNCTION;
 165                         } else if(!strcmp(buf, "H")) {
 166                                 np->type = HOLD;
 167                         } else {
 168                                 SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown node type in taxi network...\n");
 169                                 delete np;
 170                                 return(false);
 171                         }
 172                         fin >> buf;             // rwy exit information - gets parsed later - FRAGILE - will break if buf is reused.
 173                         // Now the name
 174                         fin >> ch;      // strip the leading " off
 175                         np->name = "";
 176                         while(1) {
 177                                 fin.unsetf(ios::skipws);
 178                                 fin >> ch;
 179                                 if((ch == '"') || (ch == 0x0A)) {
 180                                         break;
 181                                 }   // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
 182                                 np->name += ch;
 183                         }
 184                         fin.setf(ios::skipws);
 185                         network.push_back(np);
 186                         // FIXME - fragile - replies on buf not getting modified from exits read to here
 187                         // see if we also need to push it onto the runway exit list
 188                         //cout << "strlen(buf) = " << strlen(buf) << endl;
 189                         if(strlen(buf) > 2) {
 190                                 //cout << "Calling ParseRwyExits for " << buf << endl;
 191                                 ParseRwyExits(np, buf);
 192                         }
 193                 } else if(!strcmp(buf, "A")) {
 194                         ap = new arc;
 195                         ap->struct_type = ARC;
 196                         fin >> buf;
 197                         ap->n1 = atoi(buf);
 198                         fin >> buf;
 199                         ap->n2 = atoi(buf);
 200                         fin >> buf;
 201                         if(!strcmp(buf, "R")) {
 202                                 ap->type = RUNWAY;
 203                         } else if(!strcmp(buf, "T")) {
 204                                 ap->type = TAXIWAY;
 205                         } else {
 206                                 SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown arc type in taxi network...\n");
 207                                 delete ap;
 208                                 return(false);
 209                         }
 210                         // directed?
 211                         fin >> buf;
 212                         if(!strcmp(buf, "Y")) {
 213                                 ap->directed = true;
 214                         } else if(!strcmp(buf, "N")) {
 215                                 ap->directed = false;
 216                         } else {
 217                                 SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown arc directed value in taxi network - should be Y/N !!!\n");
 218                                 delete ap;
 219                                 return(false);
 220                         }
 221                         // Now the name
 222                         ap->name = "";
 223                         while(1) {
 224                                 fin.unsetf(ios::skipws);
 225                                 fin >> ch;
 226                                 ap->name += ch;
 227                                 if((ch == '"') || (ch == 0x0A)) {
 228                                         break;
 229                                 }   // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
 230                         }
 231                         fin.setf(ios::skipws);
 232                         ap->distance = (int)dclGetHorizontalSeparation(network[ap->n1]->pos, network[ap->n2]->pos);
 233                         //cout << "Distance  = " << ap->distance << '\n';
 234                         network[ap->n1]->arcs.push_back(ap);
 235                         network[ap->n2]->arcs.push_back(ap);
 236                 } else if(!strcmp(buf, "G")) {
 237                         gp = new Gate;
 238                         gp->struct_type = NODE;
 239                         gp->type = GATE;
 240                         fin >> buf;
 241                         gp->nodeID = atoi(buf);
 242                         fin >> buf;
 243                         gp->pos.setLongitudeDeg(atof(buf));
 244                         fin >> buf;
 245                         gp->pos.setLatitudeDeg(atof(buf));
 246                         fin >> buf;
 247                         gp->pos.setElevationM(atof(buf));
 248                         fin >> buf;             // gate type - ignore this for now
 249                         fin >> buf;             // gate heading
 250                         gp->heading = atoi(buf);
 251                         // Now the name
 252                         gp->name = "";
 253                         while(1) {
 254                                 fin.unsetf(ios::skipws);
 255                                 fin >> ch;
 256                                 gp->name += ch;
 257                                 if((ch == '"') || (ch == 0x0A)) {
 258                                         break;
 259                                 }   // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
 260                         }
 261                         fin.setf(ios::skipws);
 262                         gp->id = gateCount;             // Warning - this will likely change in the future.
 263                         gp->used = false;
 264                         network.push_back(gp);
 265                         gates[gateCount] = gp;
 266                         gateCount++;
 267                 } else {
 268                         // Something has gone seriously pear-shaped
 269                         SG_LOG(SG_ATC, SG_ALERT, "********* ERROR - unknown ground network element type... aborting read of " << path.c_str() << '\n');
 270                         return(false);
 271                 }
 272
 273                 fin >> skipeol;
 274         }
 275         return(true);
 276 }
 277
 278 void FGGround::Init() {
 279         untowered = false;
 280
 281         // Figure out which is the active runway - TODO - it would be better to have ground call tower
 282         // for runway operation details, but at the moment we can't guarantee that tower control at a given airport
 283         // will be initialised before ground so we can't do that.
 284         DoRwyDetails();
 285         //cout << "In FGGround::Init, active rwy is " << activeRwy << '\n';
 286         ortho.Init(rwy.threshold_pos, rwy.hdg);
 287
 288         networkLoadOK = LoadNetwork();
 289 }
 290
 291 void FGGround::Update(double dt) {
 292
 293         // Call the base class update for the response time handling.
 294         FGATC::Update(dt);
 295 }
 296
 297 // Figure out which runways are active.
 298 // For now we'll just be simple and do one active runway - eventually this will get much more complex
 299 // Copied from FGTower - TODO - it would be better to implement this just once, and have ground call tower
 300 // for runway operation details, but at the moment we can't guarantee that tower control at a given airport
 301 // will be initialised before ground so we can't do that.
 302 void FGGround::DoRwyDetails() {
 303         //cout << "GetRwyDetails called" << endl;
 304
 305   const FGAirport* apt = fgFindAirportID(ident);
 306   if (!apt) {
 307     SG_LOG(SG_ATC, SG_WARN, "FGGround::DoRwyDetails: unknown ICAO:" << ident);
 308     return;
 309   }
 310
 311         FGRunway* runway = apt->getActiveRunwayForUsage();
 312
 313   activeRwy = runway->ident();
 314   rwy.rwyID = runway->ident();
 315   SG_LOG(SG_ATC, SG_INFO, "In FGGround, active runway for airport " << ident << " is " << activeRwy);
 316
 317   // Get the threshold position
 318   double other_way = runway->headingDeg() - 180.0;
 319   while(other_way <= 0.0) {
 320     other_way += 360.0;
 321   }
 322     // move to the +l end/center of the runway
 323   //cout << "Runway center is at " << runway._lon << ", " << runway._lat << '\n';
 324   double tshlon = 0.0, tshlat = 0.0, tshr = 0.0;
 325   double tolon = 0.0, tolat = 0.0, tor = 0.0;
 326   rwy.length = runway->lengthM();
 327   geo_direct_wgs_84 ( aptElev, runway->latitude(), runway->longitude(), other_way,
 328                       rwy.length / 2.0 - 25.0, &tshlat, &tshlon, &tshr );
 329   geo_direct_wgs_84 ( aptElev, runway->latitude(), runway->longitude(), runway->headingDeg(),
 330                       rwy.length / 2.0 - 25.0, &tolat, &tolon, &tor );
 331   // Note - 25 meters in from the runway end is a bit of a hack to put the plane ahead of the user.
 332   // now copy what we need out of runway into rwy
 333   rwy.threshold_pos = SGGeod::fromDegM(tshlon, tshlat, aptElev);
 334   SGGeod takeoff_end = SGGeod::fromDegM(tolon, tolat, aptElev);
 335   //cout << "Threshold position = " << tshlon << ", " << tshlat << ", " << aptElev << '\n';
 336   //cout << "Takeoff position = " << tolon << ", " << tolat << ", " << aptElev << '\n';
 337   rwy.hdg = runway->headingDeg();
 338   // Set the projection for the local area based on this active runway
 339   ortho.Init(rwy.threshold_pos, rwy.hdg);
 340   rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos);      // should come out as zero
 341   rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
 342 }
 343
 344 // Return a random gate ID of an unused gate.
 345 // Two error values may be returned and must be checked for by the calling function:
 346 // -2 signifies that no gates exist at this airport.
 347 // -1 signifies that all gates are currently full.
 348 int FGGround::GetRandomGateID() {
 349         // Check that this airport actually has some gates!!
 350         if(!gates.size()) {
 351                 return(-2);
 352         }
 353
 354         gate_vec_type gateVec;
 355         int num = 0;
 356         int thenum;
 357         int ID;
 358
 359         gatesItr = gates.begin();
 360         while(gatesItr != gates.end()) {
 361                 if((gatesItr->second)->used == false) {
 362                         gateVec.push_back(gatesItr->second);
 363                         num++;
 364                 }
 365                 ++gatesItr;
 366         }
 367
 368         // Check that there are some unused gates!
 369         if(!gateVec.size()) {
 370                 return(-1);
 371         }
 372
 373         // Randomly select one from the list
 374         sg_srandom_time();
 375         thenum = (int)(sg_random() * gateVec.size());
 376         ID = gateVec[thenum]->id;
 377
 378         return(ID);
 379 }
 380
 381 // Return a pointer to an unused gate node
 382 Gate* FGGround::GetGateNode() {
 383         int id = GetRandomGateID();
 384         if(id < 0) {
 385                 return(NULL);
 386         } else {
 387                 return(gates[id]);
 388         }
 389 }
 390
 391
 392 node* FGGround::GetHoldShortNode(const string& rwyID) {
 393         return(NULL);   // TODO - either implement me or remove me!!!
 394 }
 395
 396
 397 // WARNING - This is hardwired to my prototype logical network format
 398 // and will almost certainly change when Bernie's stuff comes on-line.
 399 // Returns NULL if it can't find a valid node.
 400 node* FGGround::GetThresholdNode(const string& rwyID) {
 401         // For now go through all the nodes and parse their names
 402         // Maybe in the future we'll map threshold nodes by ID
 403         //cout << "Size of network is " << network.size() << '\n';
 404         for(unsigned int i=0; i<network.size(); ++i) {
 405                 //cout << "Name = " << network[i]->name << '\n';
 406                 if(network[i]->name.size()) {
 407                         string s = network[i]->name;
 408                         // Warning - the next bit is fragile and dependent on my current naming scheme
 409                         //cout << "substr = " << s.substr(0,3) << '\n';
 410                         //cout << "size of s = " << s.size() << '\n';
 411                         if(s.substr(0,3) == "rwy") {
 412                                 //cout << "subsubstr = " << s.substr(4, s.size() - 4) << '\n';
 413                                 if(s.substr(4, s.size() - 4) == rwyID) {
 414                                         return network[i];
 415                                 }
 416                         }
 417                 }
 418         }
 419         return NULL;
 420 }
 421
 422
 423 // Get a path from a point on a runway to a gate
 424 // TODO !!
 425
 426 // Get a path from a node to another node
 427 // Eventually we will need complex algorithms for this taking other traffic,
 428 // shortest path and suitable paths into accout.
 429 // For now we'll just call the shortest path algorithm.
 430 ground_network_path_type FGGround::GetPath(node* A, node* B) {
 431         return(GetShortestPath(A, B));
 432 };
 433
 434 // Get a path from a node to a runway threshold
 435 ground_network_path_type FGGround::GetPath(node* A, const string& rwyID) {
 436         node* b = GetThresholdNode(rwyID);
 437         if(b == NULL) {
 438                 SG_LOG(SG_ATC, SG_ALERT, "ERROR - unable to find path to runway theshold in ground.cxx for airport " << ident << '\n');
 439                 ground_network_path_type emptyPath;
 440                 emptyPath.erase(emptyPath.begin(), emptyPath.end());
 441                 return(emptyPath);
 442         }
 443         return GetShortestPath(A, b);
 444 }
 445
 446 // Get a path from a node to a runway hold short point
 447 // Bit of a hack this at the moment!
 448 ground_network_path_type FGGround::GetPathToHoldShort(node* A, const string& rwyID) {
 449         ground_network_path_type path = GetPath(A, rwyID);
 450         path.pop_back();        // That should be the threshold stripped of
 451         path.pop_back();        // and that should be the arc from hold short to threshold
 452         // This isn't robust though - TODO - implement properly!
 453         return(path);
 454 }
 455
 456 // A shortest path algorithm from memory (ie. I can't find the bl&*dy book again!)
 457 // I'm sure there must be enchancements that we can make to this, such as biasing the
 458 // order in which the nodes are searched out from in favour of those geographically
 459 // closer to the destination.
 460 // Note that we are working with the master set of nodes and arcs so we mustn't change
 461 // or delete them -  we only delete the paths that we create during the algorithm.
 462 ground_network_path_type FGGround::GetShortestPath(node* A, node* B) {
 463         a_path* pathPtr;
 464         shortest_path_map_type pathMap;
 465         node_array_type nodesLeft;
 466
 467         // Debugging check
 468         int pathsCreated = 0;
 469
 470         // Initialise the algorithm
 471         nodesLeft.push_back(A);
 472         pathPtr = new a_path;
 473         pathsCreated++;
 474         pathPtr->path.push_back(A);
 475         pathPtr->cost = 0;
 476         pathMap[A->nodeID] = pathPtr;
 477         bool solution_found = false;    // Flag to indicate that at least one candidate path has been found
 478         int solution_cost = -1;                 // Cost of current best cost solution.  -1 indicates no solution found yet.
 479         a_path solution_path;
 480
 481         node* nPtr;     // nPtr is used to point to the node we are currently working with
 482
 483         while(nodesLeft.size()) {
 484                 //cout << "\n*****nodesLeft*****\n";
 485                 //for(unsigned int i=0; i<nodesLeft.size(); ++i) {
 486                         //cout << nodesLeft[i]->nodeID << '\n';
 487                 //}
 488                 //cout << "*******************\n\n";
 489                 nPtr = *nodesLeft.begin();              // Thought - definate optimization possibilities here in the choice of which nodes we process first.
 490                 nodesLeft.erase(nodesLeft.begin());
 491                 //cout << "Walking out from node " << nPtr->nodeID << '\n';
 492                 for(unsigned int i=0; i<nPtr->arcs.size(); ++i) {
 493                         //cout << "ARC TO " << ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1) << '\n';
 494                 }
 495                 if((solution_found) && (solution_cost <= pathMap[nPtr->nodeID]->cost)) {
 496                         // Do nothing - we've already found a solution and this partial path is already more expensive
 497                 } else {
 498                         // This path could still be better than the current solution - check it out
 499                         for(unsigned int i=0; i<(nPtr->arcs.size()); i++) {
 500                                 // Map the new path against the end node, ie. *not* the one we just started with.
 501                                 unsigned int end_nodeID = ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1);
 502                                 //cout << "end_nodeID = " << end_nodeID << '\n';
 503                                 //cout << "pathMap size is " << pathMap.size() << '\n';
 504                                 if(end_nodeID == nPtr->nodeID) {
 505                                         //cout << "Circular arc!\n";
 506                                         // Then its a circular arc - don't bother!!
 507                                         //nPtr->arcs.erase(nPtr->arcs.begin() + i);
 508                                 } else {
 509                                         // see if the end node is already in the map or not
 510                                         if(pathMap.find(end_nodeID) == pathMap.end()) {
 511                                                 //cout << "Not in the map" << endl;;
 512                                                 // Not in the map - easy!
 513                                                 pathPtr = new a_path;
 514                                                 pathsCreated++;
 515                                                 *pathPtr = *pathMap[nPtr->nodeID];      // *copy* the path
 516                                                 pathPtr->path.push_back(nPtr->arcs[i]);
 517                                                 pathPtr->path.push_back(network[end_nodeID]);
 518                                                 pathPtr->cost += nPtr->arcs[i]->distance;
 519                                                 pathMap[end_nodeID] = pathPtr;
 520                                                 nodesLeft.push_back(network[end_nodeID]);       // By definition this can't be in the list already, or
 521                                                 // it would also have been in the map and hence OR'd with this one.
 522                                                 if(end_nodeID == B->nodeID) {
 523                                                         //cout << "Solution found!!!" << endl;
 524                                                         // Since this node wasn't in the map this is by definition the first solution
 525                                                         solution_cost = pathPtr->cost;
 526                                                         solution_path = *pathPtr;
 527                                                         solution_found = true;
 528                                                 }
 529                                         } else {
 530                                                 //cout << "Already in the map" << endl;
 531                                                 // In the map - not so easy - need to get rid of an arc from the higher cost one.
 532                                                 //cout << "Current cost of node " << end_nodeID << " is " << pathMap[end_nodeID]->cost << endl;
 533                                                 int newCost = pathMap[nPtr->nodeID]->cost + nPtr->arcs[i]->distance;
 534                                                 //cout << "New cost is of node " << nPtr->nodeID << " is " << newCost << endl;
 535                                                 if(newCost >= pathMap[end_nodeID]->cost) {
 536                                                         // No need to do anything.
 537                                                         //cout << "Not doing anything!" << endl;
 538                                                 } else {
 539                                                         delete pathMap[end_nodeID];
 540                                                         pathsCreated--;
 541
 542                                                         pathPtr = new a_path;
 543                                                         pathsCreated++;
 544                                                         *pathPtr = *pathMap[nPtr->nodeID];      // *copy* the path
 545                                                         pathPtr->path.push_back(nPtr->arcs[i]);
 546                                                         pathPtr->path.push_back(network[end_nodeID]);
 547                                                         pathPtr->cost += nPtr->arcs[i]->distance;
 548                                                         pathMap[end_nodeID] = pathPtr;
 549
 550                                                         // We need to add this node to the list-to-do again to force a recalculation
 551                                                         // onwards from this node with the new lower cost to node cost.
 552                                                         nodesLeft.push_back(network[end_nodeID]);
 553
 554                                                         if(end_nodeID == B->nodeID) {
 555                                                                 //cout << "Solution found!!!" << endl;
 556                                                                 // Need to check if there is a previous better solution
 557                                                                 if((solution_cost < 0) || (pathPtr->cost < solution_cost)) {
 558                                                                         solution_cost = pathPtr->cost;
 559                                                                         solution_path = *pathPtr;
 560                                                                         solution_found = true;
 561                                                                 }
 562                                                         }
 563                                                 }
 564                                         }
 565                                 }
 566                         }
 567                 }
 568         }
 569
 570         // delete all the paths before returning
 571         shortest_path_map_iterator spItr = pathMap.begin();
 572         while(spItr != pathMap.end()) {
 573                 if(spItr->second != NULL) {
 574                         delete spItr->second;
 575                         --pathsCreated;
 576                 }
 577                 ++spItr;
 578         }
 579
 580         //cout << "pathsCreated = " << pathsCreated << '\n';
 581         if(pathsCreated > 0) {
 582                 SG_LOG(SG_ATC, SG_ALERT, "WARNING - Possible memory leak in FGGround::GetShortestPath\n\
 583                                                                           Please report to flightgear-devel@flightgear.org\n");
 584         }
 585
 586         //cout << (solution_found ? "Result: solution found\n" : "Result: no solution found\n");
 587         return(solution_path.path);             // TODO - we really ought to have a fallback position incase a solution isn't found.
 588 }
 589
 590
 591
 592 // Randomly or otherwise populate some of the gates with parked planes
 593 // (might eventually be done by the AIMgr if and when lots of AI traffic is generated)
 594
 595 // Return a list of exits from a given runway
 596 // It is up to the calling function to check for non-zero size of returned array before use
 597 node_array_type FGGround::GetExits(const string& rwyID) {
 598         // FIXME - get a 07L or similar in here and we're stuffed!!!
 599         return(runways[atoi(rwyID.c_str())].exits);
 600 }