src/ATC/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 <simgear/misc/sg_path.hxx>
  26 #include <simgear/math/sg_random.h>
  27 #include <simgear/debug/logstream.hxx>
  28 #include <simgear/misc/sgstream.hxx>
  29 #include <simgear/constants.h>
  30 #include <Main/globals.hxx>
  31
  32 #include <stdlib.h>
  33 #include STL_FSTREAM
  34
  35 #include "ground.hxx"
  36 #include "ATCutils.hxx"
  37 #include "AILocalTraffic.hxx"
  38 #include "ATCmgr.hxx"
  39
  40 SG_USING_STD(ifstream);
  41 SG_USING_STD(cout);
  42
  43 node::node() {
  44 }
  45
  46 node::~node() {
  47         for(unsigned int i=0; i < arcs.size(); ++i) {
  48                 delete arcs[i];
  49         }
  50 }
  51
  52 // Make sure that a_path.cost += distance is safe from the moment it's created.
  53 a_path::a_path() {
  54         cost = 0;
  55 }
  56
  57 FGGround::FGGround() {
  58         ATCmgr = globals->get_ATC_mgr();
  59         _type = GROUND;
  60         networkLoadOK = false;
  61         ground_traffic.erase(ground_traffic.begin(), ground_traffic.end());
  62         ground_traffic_itr = ground_traffic.begin();
  63
  64         // Init the property nodes - TODO - need to make sure we're getting surface winds.
  65         wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
  66         wind_speed_knots = fgGetNode("/environment/wind-speed-kt", true);
  67
  68         // TODO - get the actual airport elevation
  69         aptElev = 0.0;
  70 }
  71
  72 FGGround::FGGround(const string& id) {
  73         ATCmgr = globals->get_ATC_mgr();
  74         networkLoadOK = false;
  75         ground_traffic.erase(ground_traffic.begin(), ground_traffic.end());
  76         ground_traffic_itr = ground_traffic.begin();
  77         ident = id;
  78
  79         // Init the property nodes - TODO - need to make sure we're getting surface winds.
  80         wind_from_hdg = fgGetNode("/environment/wind-from-heading-deg", true);
  81         wind_speed_knots = fgGetNode("/environment/wind-speed-kt", true);
  82
  83         // TODO - get the actual airport elevation
  84         aptElev = 0.0;
  85 }
  86
  87 FGGround::~FGGround() {
  88 }
  89
  90 void FGGround::ParseRwyExits(node* np, char* es) {
  91         char* token;
  92         char estr[20];
  93         strcpy(estr, es);
  94         const char delimiters[] = "-";
  95         token = strtok(estr, delimiters);
  96         while(token != NULL) {
  97                 int i = atoi(token);
  98                 //cout << "token = " << token << endl;
  99                 //cout << "rwy number = " << i << endl;
 100                 //runways[(atoi(token))].exits.push_back(np);
 101                 runways[i].exits.push_back(np);
 102                 //cout << "token = " << token << '\n';
 103                 token = strtok(NULL, delimiters);
 104         }
 105 }
 106
 107
 108 // Load the ground logical network of the current instances airport
 109 // Return true if successfull.
 110 // TODO - currently the file is assumed to reside in the base/ATC directory.
 111 // This might change to something more thought out in the future.
 112 // NOTE - currently it is assumed that all nodes are loaded before any arcs.
 113 // It won't work ATM if this doesn't hold true.
 114 bool FGGround::LoadNetwork() {
 115         node* np;
 116         arc* ap;
 117         Gate* gp;
 118
 119         int gateCount = 0;      // This is used to allocate gateID's from zero upwards
 120         // This may well change in the future - probably to reading in the real-world
 121         // gate numbers from file.
 122
 123         ifstream fin;
 124         SGPath path = globals->get_fg_root();
 125         //string taxiPath = "ATC/" + ident + ".taxi";
 126         string taxiPath = "ATC/KEMT.taxi";      // FIXME - HARDWIRED FOR TESTING
 127         path.append(taxiPath);
 128
 129         SG_LOG(SG_ATC, SG_INFO, "Trying to read taxiway data for " << ident << "...");
 130         //cout << "Trying to read taxiway data for " << ident << "..." << endl;
 131         fin.open(path.c_str(), ios::in);
 132         if(!fin) {
 133                 SG_LOG(SG_ATC, SG_ALERT, "Unable to open taxiway data input file " << path.c_str());
 134                 //cout << "Unable to open taxiway data input file " << path.c_str() << endl;
 135                 return(false);
 136         }
 137
 138         char ch;
 139         char buf[30];
 140         while(!fin.eof()) {
 141                 fin >> buf;
 142                 // Node, arc, or [End]?
 143                 //cout << "Read in ground network element type = " << buf << endl;
 144                 if(!strcmp(buf, "[End]")) {             // TODO - maybe make this more robust to spelling errors by just looking for '['
 145                         SG_LOG(SG_ATC, SG_INFO, "Done reading " << path.c_str() << endl);
 146                         break;
 147                 } else if(!strcmp(buf, "N")) {
 148                         // Node
 149                         np = new node;
 150                         np->struct_type = NODE;
 151                         fin >> buf;
 152                         np->nodeID = atoi(buf);
 153                         fin >> buf;
 154                         np->pos.setlon(atof(buf));
 155                         fin >> buf;
 156                         np->pos.setlat(atof(buf));
 157                         fin >> buf;
 158                         np->pos.setelev(atof(buf));
 159                         fin >> buf;             // node type
 160                         if(!strcmp(buf, "J")) {
 161                                 np->type = JUNCTION;
 162                         } else if(!strcmp(buf, "T")) {
 163                                 np->type = TJUNCTION;
 164                         } else if(!strcmp(buf, "H")) {
 165                                 np->type = HOLD;
 166                         } else {
 167                                 SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown node type in taxi network...\n");
 168                                 delete np;
 169                                 return(false);
 170                         }
 171                         fin >> buf;             // rwy exit information - gets parsed later - FRAGILE - will break if buf is reused.
 172                         // Now the name
 173                         fin >> ch;      // strip the leading " off
 174                         np->name = "";
 175                         while(1) {
 176                                 fin.unsetf(ios::skipws);
 177                                 fin >> ch;
 178                                 if((ch == '"') || (ch == 0x0A)) {
 179                                         break;
 180                                 }   // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
 181                                 np->name += ch;
 182                         }
 183                         fin.setf(ios::skipws);
 184                         network.push_back(np);
 185                         // FIXME - fragile - replies on buf not getting modified from exits read to here
 186                         // see if we also need to push it onto the runway exit list
 187                         //cout << "strlen(buf) = " << strlen(buf) << endl;
 188                         if(strlen(buf) > 2) {
 189                                 //cout << "Calling ParseRwyExits for " << buf << endl;
 190                                 ParseRwyExits(np, buf);
 191                         }
 192                 } else if(!strcmp(buf, "A")) {
 193                         ap = new arc;
 194                         ap->struct_type = ARC;
 195                         fin >> buf;
 196                         ap->n1 = atoi(buf);
 197                         fin >> buf;
 198                         ap->n2 = atoi(buf);
 199                         fin >> buf;
 200                         if(!strcmp(buf, "R")) {
 201                                 ap->type = RUNWAY;
 202                         } else if(!strcmp(buf, "T")) {
 203                                 ap->type = TAXIWAY;
 204                         } else {
 205                                 SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown arc type in taxi network...\n");
 206                                 delete ap;
 207                                 return(false);
 208                         }
 209                         // directed?
 210                         fin >> buf;
 211                         if(!strcmp(buf, "Y")) {
 212                                 ap->directed = true;
 213                         } else if(!strcmp(buf, "N")) {
 214                                 ap->directed = false;
 215                         } else {
 216                                 SG_LOG(SG_ATC, SG_ALERT, "**** ERROR ***** Unknown arc directed value in taxi network - should be Y/N !!!\n");
 217                                 delete ap;
 218                                 return(false);
 219                         }
 220                         // Now the name
 221                         ap->name = "";
 222                         while(1) {
 223                                 fin.unsetf(ios::skipws);
 224                                 fin >> ch;
 225                                 ap->name += ch;
 226                                 if((ch == '"') || (ch == 0x0A)) {
 227                                         break;
 228                                 }   // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
 229                         }
 230                         fin.setf(ios::skipws);
 231                         ap->distance = (int)dclGetHorizontalSeparation(network[ap->n1]->pos, network[ap->n2]->pos);
 232                         //cout << "Distance  = " << ap->distance << '\n';
 233                         network[ap->n1]->arcs.push_back(ap);
 234                         network[ap->n2]->arcs.push_back(ap);
 235                 } else if(!strcmp(buf, "G")) {
 236                         gp = new Gate;
 237                         gp->struct_type = NODE;
 238                         gp->type = GATE;
 239                         fin >> buf;
 240                         gp->nodeID = atoi(buf);
 241                         fin >> buf;
 242                         gp->pos.setlon(atof(buf));
 243                         fin >> buf;
 244                         gp->pos.setlat(atof(buf));
 245                         fin >> buf;
 246                         gp->pos.setelev(atof(buf));
 247                         fin >> buf;             // gate type - ignore this for now
 248                         fin >> buf;             // gate heading
 249                         gp->heading = atoi(buf);
 250                         // Now the name
 251                         gp->name = "";
 252                         while(1) {
 253                                 fin.unsetf(ios::skipws);
 254                                 fin >> ch;
 255                                 gp->name += ch;
 256                                 if((ch == '"') || (ch == 0x0A)) {
 257                                         break;
 258                                 }   // we shouldn't need the 0x0A but it makes a nice safely in case someone leaves off the "
 259                         }
 260                         fin.setf(ios::skipws);
 261                         gp->id = gateCount;             // Warning - this will likely change in the future.
 262                         gp->used = false;
 263                         network.push_back(gp);
 264                         gates[gateCount] = gp;
 265                         gateCount++;
 266                 } else {
 267                         // Something has gone seriously pear-shaped
 268                         SG_LOG(SG_ATC, SG_ALERT, "********* ERROR - unknown ground network element type... aborting read of " << path.c_str() << '\n');
 269                         return(false);
 270                 }
 271
 272                 fin >> skipeol;
 273         }
 274         return(true);
 275 }
 276
 277 void FGGround::Init() {
 278         untowered = false;
 279
 280         // Figure out which is the active runway - TODO - it would be better to have ground call tower
 281         // for runway operation details, but at the moment we can't guarantee that tower control at a given airport
 282         // will be initialised before ground so we can't do that.
 283         DoRwyDetails();
 284         //cout << "In FGGround::Init, active rwy is " << activeRwy << '\n';
 285         ortho.Init(rwy.threshold_pos, rwy.hdg);
 286
 287         networkLoadOK = LoadNetwork();
 288 }
 289
 290 void FGGround::Update(double dt) {
 291         // Each time step, what do we need to do?
 292         // We need to go through the list of outstanding requests and acknowedgements
 293         // and process at least one of them.
 294         // We need to go through the list of planes under our control and check if
 295         // any need to be addressed.
 296         // We need to check for planes not under our control coming within our
 297         // control area and address if necessary.
 298
 299         // Lets take the example of a plane which has just contacted ground
 300         // following landing - presumably requesting where to go?
 301         // First we need to establish the position of the plane within the logical network.
 302         // Next we need to decide where its going.
 303
 304         if(ground_traffic.size()) {
 305                 if(ground_traffic_itr == ground_traffic.end()) {
 306                         ground_traffic_itr = ground_traffic.begin();
 307                 }
 308
 309                 //Process(*ground_traffic_itr);
 310                 GroundRec* g = *ground_traffic_itr;
 311                 if(g->taxiRequestOutstanding) {
 312                         double responseTime = 10.0;             // seconds - this should get more sophisticated at some point
 313                         if(g->clearanceCounter > responseTime) {
 314                                 // DO CLEARANCE
 315                                 // TODO - move the mechanics of making up the transmission out of the main Update(...) routine.
 316                                 string trns = "";
 317                                 trns += g->plane.callsign;
 318                                 trns += " taxi holding point runway ";  // TODO - add the holding point name
 319                                 // eg " taxi holding point G2 runway "
 320                                 trns += ConvertRwyNumToSpokenString(activeRwy);
 321                                 if(_display) {
 322                                         fgSetString("/sim/messages/ground", trns.c_str());
 323                                 }
 324                                 g->planePtr->RegisterTransmission(1);   // cleared to taxi
 325                                 g->clearanceCounter = 0.0;
 326                                 g->taxiRequestOutstanding = false;
 327                         } else {
 328                                 g->clearanceCounter += (dt * ground_traffic.size());
 329                         }
 330                 } else if(((FGAILocalTraffic*)(g->planePtr))->AtHoldShort()) {          // That's a hack - eventually we should monitor actual position
 331                         // HACK ALERT - the automatic cast to AILocalTraffic has to go once we have other sorts working!!!!! FIXME TODO
 332                         // NOTE - we don't need to do the contact tower bit unless we have separate tower and ground
 333                         string trns = g->plane.callsign;
 334                         trns += " contact Tower ";
 335                         double f = globals->get_ATC_mgr()->GetFrequency(ident, TOWER) / 100.0;
 336                         char buf[10];
 337                         sprintf(buf, "%.2f", f);
 338                         trns += buf;
 339                         if(_display) {
 340                                 fgSetString("/sim/messages/ground", trns.c_str());
 341                         }
 342                         g->planePtr->RegisterTransmission(2);   // contact tower
 343                         delete *ground_traffic_itr;
 344                         ground_traffic.erase(ground_traffic_itr);
 345                         ground_traffic_itr = ground_traffic.begin();
 346                 }
 347                 ++ground_traffic_itr;
 348         }
 349
 350         // Call the base class update for the response time handling.
 351         FGATC::Update(dt);
 352 }
 353
 354 // Figure out which runways are active.
 355 // For now we'll just be simple and do one active runway - eventually this will get much more complex
 356 // Copied from FGTower - TODO - it would be better to implement this just once, and have ground call tower
 357 // for runway operation details, but at the moment we can't guarantee that tower control at a given airport
 358 // will be initialised before ground so we can't do that.
 359 void FGGround::DoRwyDetails() {
 360         //cout << "GetRwyDetails called" << endl;
 361
 362         // Based on the airport-id and wind get the active runway
 363
 364         //wind
 365         double hdg = wind_from_hdg->getDoubleValue();
 366         double speed = wind_speed_knots->getDoubleValue();
 367         hdg = (speed == 0.0 ? 270.0 : hdg);
 368         //cout << "Heading = " << hdg << '\n';
 369
 370         FGRunway runway;
 371         bool rwyGood = globals->get_runways()->search(ident, int(hdg), &runway);
 372         if(rwyGood) {
 373                 activeRwy = runway._rwy_no;
 374                 rwy.rwyID = runway._rwy_no;
 375                 SG_LOG(SG_ATC, SG_INFO, "In FGGround, active runway for airport " << ident << " is " << activeRwy);
 376
 377                 // Get the threshold position
 378                 double other_way = runway._heading - 180.0;
 379                 while(other_way <= 0.0) {
 380                         other_way += 360.0;
 381                 }
 382         // move to the +l end/center of the runway
 383                 //cout << "Runway center is at " << runway._lon << ", " << runway._lat << '\n';
 384         Point3D origin = Point3D(runway._lon, runway._lat, aptElev);
 385                 Point3D ref = origin;
 386         double tshlon, tshlat, tshr;
 387                 double tolon, tolat, tor;
 388                 rwy.length = runway._length * SG_FEET_TO_METER;
 389         geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), other_way,
 390                                 rwy.length / 2.0 - 25.0, &tshlat, &tshlon, &tshr );
 391         geo_direct_wgs_84 ( aptElev, ref.lat(), ref.lon(), runway._heading,
 392                                 rwy.length / 2.0 - 25.0, &tolat, &tolon, &tor );
 393                 // Note - 25 meters in from the runway end is a bit of a hack to put the plane ahead of the user.
 394                 // now copy what we need out of runway into rwy
 395         rwy.threshold_pos = Point3D(tshlon, tshlat, aptElev);
 396                 Point3D takeoff_end = Point3D(tolon, tolat, aptElev);
 397                 //cout << "Threshold position = " << tshlon << ", " << tshlat << ", " << aptElev << '\n';
 398                 //cout << "Takeoff position = " << tolon << ", " << tolat << ", " << aptElev << '\n';
 399                 rwy.hdg = runway._heading;
 400                 // Set the projection for the local area based on this active runway
 401                 ortho.Init(rwy.threshold_pos, rwy.hdg);
 402                 rwy.end1ortho = ortho.ConvertToLocal(rwy.threshold_pos);        // should come out as zero
 403                 rwy.end2ortho = ortho.ConvertToLocal(takeoff_end);
 404         } else {
 405                 SG_LOG(SG_ATC, SG_ALERT, "Help  - can't get good runway in FGTower!!");
 406                 activeRwy = "NN";
 407         }
 408 }
 409
 410 // Return a random gate ID of an unused gate.
 411 // Two error values may be returned and must be checked for by the calling function:
 412 // -2 signifies that no gates exist at this airport.
 413 // -1 signifies that all gates are currently full.
 414 int FGGround::GetRandomGateID() {
 415         // Check that this airport actually has some gates!!
 416         if(!gates.size()) {
 417                 return(-2);
 418         }
 419
 420         gate_vec_type gateVec;
 421         int num = 0;
 422         int thenum;
 423         int ID;
 424
 425         gatesItr = gates.begin();
 426         while(gatesItr != gates.end()) {
 427                 if((gatesItr->second)->used == false) {
 428                         gateVec.push_back(gatesItr->second);
 429                         num++;
 430                 }
 431                 ++gatesItr;
 432         }
 433
 434         // Check that there are some unused gates!
 435         if(!gateVec.size()) {
 436                 return(-1);
 437         }
 438
 439         // Randomly select one from the list
 440         sg_srandom_time();
 441         thenum = (int)(sg_random() * gateVec.size());
 442         ID = gateVec[thenum]->id;
 443
 444         return(ID);
 445 }
 446
 447 // Return a pointer to an unused gate node
 448 Gate* FGGround::GetGateNode() {
 449         int id = GetRandomGateID();
 450         if(id < 0) {
 451                 return(NULL);
 452         } else {
 453                 return(gates[id]);
 454         }
 455 }
 456
 457
 458 node* FGGround::GetHoldShortNode(const string& rwyID) {
 459         return(NULL);   // TODO - either implement me or remove me!!!
 460 }
 461
 462
 463 // WARNING - This is hardwired to my prototype logical network format
 464 // and will almost certainly change when Bernie's stuff comes on-line.
 465 // Returns NULL if it can't find a valid node.
 466 node* FGGround::GetThresholdNode(const string& rwyID) {
 467         // For now go through all the nodes and parse their names
 468         // Maybe in the future we'll map threshold nodes by ID
 469         //cout << "Size of network is " << network.size() << '\n';
 470         for(unsigned int i=0; i<network.size(); ++i) {
 471                 //cout << "Name = " << network[i]->name << '\n';
 472                 if(network[i]->name.size()) {
 473                         string s = network[i]->name;
 474                         // Warning - the next bit is fragile and dependent on my current naming scheme
 475                         //cout << "substr = " << s.substr(0,3) << '\n';
 476                         //cout << "size of s = " << s.size() << '\n';
 477                         if(s.substr(0,3) == "rwy") {
 478                                 //cout << "subsubstr = " << s.substr(4, s.size() - 4) << '\n';
 479                                 if(s.substr(4, s.size() - 4) == rwyID) {
 480                                         return network[i];
 481                                 }
 482                         }
 483                 }
 484         }
 485         return NULL;
 486 }
 487
 488
 489 // Get a path from a point on a runway to a gate
 490 // TODO !!
 491
 492 // Get a path from a node to another node
 493 // Eventually we will need complex algorithms for this taking other traffic,
 494 // shortest path and suitable paths into accout.
 495 // For now we'll just call the shortest path algorithm.
 496 ground_network_path_type FGGround::GetPath(node* A, node* B) {
 497         return(GetShortestPath(A, B));
 498 };
 499
 500 // Get a path from a node to a runway threshold
 501 ground_network_path_type FGGround::GetPath(node* A, const string& rwyID) {
 502         node* b = GetThresholdNode(rwyID);
 503         if(b == NULL) {
 504                 SG_LOG(SG_ATC, SG_ALERT, "ERROR - unable to find path to runway theshold in ground.cxx for airport " << ident << '\n');
 505                 ground_network_path_type emptyPath;
 506                 emptyPath.erase(emptyPath.begin(), emptyPath.end());
 507                 return(emptyPath);
 508         }
 509         return GetShortestPath(A, b);
 510 }
 511
 512 // Get a path from a node to a runway hold short point
 513 // Bit of a hack this at the moment!
 514 ground_network_path_type FGGround::GetPathToHoldShort(node* A, const string& rwyID) {
 515         ground_network_path_type path = GetPath(A, rwyID);
 516         path.pop_back();        // That should be the threshold stripped of
 517         path.pop_back();        // and that should be the arc from hold short to threshold
 518         // This isn't robust though - TODO - implement properly!
 519         return(path);
 520 }
 521
 522 // A shortest path algorithm from memory (ie. I can't find the bl&*dy book again!)
 523 // I'm sure there must be enchancements that we can make to this, such as biasing the
 524 // order in which the nodes are searched out from in favour of those geographically
 525 // closer to the destination.
 526 // Note that we are working with the master set of nodes and arcs so we mustn't change
 527 // or delete them -  we only delete the paths that we create during the algorithm.
 528 ground_network_path_type FGGround::GetShortestPath(node* A, node* B) {
 529         a_path* pathPtr;
 530         shortest_path_map_type pathMap;
 531         node_array_type nodesLeft;
 532
 533         // Debugging check
 534         int pathsCreated = 0;
 535
 536         // Initialise the algorithm
 537         nodesLeft.push_back(A);
 538         pathPtr = new a_path;
 539         pathsCreated++;
 540         pathPtr->path.push_back(A);
 541         pathPtr->cost = 0;
 542         pathMap[A->nodeID] = pathPtr;
 543         bool solution_found = false;    // Flag to indicate that at least one candidate path has been found
 544         int solution_cost = -1;                 // Cost of current best cost solution.  -1 indicates no solution found yet.
 545         a_path solution_path;
 546
 547         node* nPtr;     // nPtr is used to point to the node we are currently working with
 548
 549         while(nodesLeft.size()) {
 550                 //cout << "\n*****nodesLeft*****\n";
 551                 //for(unsigned int i=0; i<nodesLeft.size(); ++i) {
 552                         //cout << nodesLeft[i]->nodeID << '\n';
 553                 //}
 554                 //cout << "*******************\n\n";
 555                 nPtr = *nodesLeft.begin();              // Thought - definate optimization possibilities here in the choice of which nodes we process first.
 556                 nodesLeft.erase(nodesLeft.begin());
 557                 //cout << "Walking out from node " << nPtr->nodeID << '\n';
 558                 for(unsigned int i=0; i<nPtr->arcs.size(); ++i) {
 559                         //cout << "ARC TO " << ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1) << '\n';
 560                 }
 561                 if((solution_found) && (solution_cost <= pathMap[nPtr->nodeID]->cost)) {
 562                         // Do nothing - we've already found a solution and this partial path is already more expensive
 563                 } else {
 564                         // This path could still be better than the current solution - check it out
 565                         for(unsigned int i=0; i<(nPtr->arcs.size()); i++) {
 566                                 // Map the new path against the end node, ie. *not* the one we just started with.
 567                                 unsigned int end_nodeID = ((nPtr->arcs[i]->n1 == nPtr->nodeID) ? nPtr->arcs[i]->n2 : nPtr->arcs[i]->n1);
 568                                 //cout << "end_nodeID = " << end_nodeID << '\n';
 569                                 //cout << "pathMap size is " << pathMap.size() << '\n';
 570                                 if(end_nodeID == nPtr->nodeID) {
 571                                         //cout << "Circular arc!\n";
 572                                         // Then its a circular arc - don't bother!!
 573                                         //nPtr->arcs.erase(nPtr->arcs.begin() + i);
 574                                 } else {
 575                                         // see if the end node is already in the map or not
 576                                         if(pathMap.find(end_nodeID) == pathMap.end()) {
 577                                                 //cout << "Not in the map" << endl;;
 578                                                 // Not in the map - easy!
 579                                                 pathPtr = new a_path;
 580                                                 pathsCreated++;
 581                                                 *pathPtr = *pathMap[nPtr->nodeID];      // *copy* the path
 582                                                 pathPtr->path.push_back(nPtr->arcs[i]);
 583                                                 pathPtr->path.push_back(network[end_nodeID]);
 584                                                 pathPtr->cost += nPtr->arcs[i]->distance;
 585                                                 pathMap[end_nodeID] = pathPtr;
 586                                                 nodesLeft.push_back(network[end_nodeID]);       // By definition this can't be in the list already, or
 587                                                 // it would also have been in the map and hence OR'd with this one.
 588                                                 if(end_nodeID == B->nodeID) {
 589                                                         //cout << "Solution found!!!" << endl;
 590                                                         // Since this node wasn't in the map this is by definition the first solution
 591                                                         solution_cost = pathPtr->cost;
 592                                                         solution_path = *pathPtr;
 593                                                         solution_found = true;
 594                                                 }
 595                                         } else {
 596                                                 //cout << "Already in the map" << endl;
 597                                                 // In the map - not so easy - need to get rid of an arc from the higher cost one.
 598                                                 //cout << "Current cost of node " << end_nodeID << " is " << pathMap[end_nodeID]->cost << endl;
 599                                                 int newCost = pathMap[nPtr->nodeID]->cost + nPtr->arcs[i]->distance;
 600                                                 //cout << "New cost is of node " << nPtr->nodeID << " is " << newCost << endl;
 601                                                 if(newCost >= pathMap[end_nodeID]->cost) {
 602                                                         // No need to do anything.
 603                                                         //cout << "Not doing anything!" << endl;
 604                                                 } else {
 605                                                         delete pathMap[end_nodeID];
 606                                                         pathsCreated--;
 607
 608                                                         pathPtr = new a_path;
 609                                                         pathsCreated++;
 610                                                         *pathPtr = *pathMap[nPtr->nodeID];      // *copy* the path
 611                                                         pathPtr->path.push_back(nPtr->arcs[i]);
 612                                                         pathPtr->path.push_back(network[end_nodeID]);
 613                                                         pathPtr->cost += nPtr->arcs[i]->distance;
 614                                                         pathMap[end_nodeID] = pathPtr;
 615
 616                                                         // We need to add this node to the list-to-do again to force a recalculation
 617                                                         // onwards from this node with the new lower cost to node cost.
 618                                                         nodesLeft.push_back(network[end_nodeID]);
 619
 620                                                         if(end_nodeID == B->nodeID) {
 621                                                                 //cout << "Solution found!!!" << endl;
 622                                                                 // Need to check if there is a previous better solution
 623                                                                 if((solution_cost < 0) || (pathPtr->cost < solution_cost)) {
 624                                                                         solution_cost = pathPtr->cost;
 625                                                                         solution_path = *pathPtr;
 626                                                                         solution_found = true;
 627                                                                 }
 628                                                         }
 629                                                 }
 630                                         }
 631                                 }
 632                         }
 633                 }
 634         }
 635
 636         // delete all the paths before returning
 637         shortest_path_map_iterator spItr = pathMap.begin();
 638         while(spItr != pathMap.end()) {
 639                 if(spItr->second != NULL) {
 640                         delete spItr->second;
 641                         --pathsCreated;
 642                 }
 643                 ++spItr;
 644         }
 645
 646         //cout << "pathsCreated = " << pathsCreated << '\n';
 647         if(pathsCreated > 0) {
 648                 SG_LOG(SG_ATC, SG_ALERT, "WARNING - Possible memory leak in FGGround::GetShortestPath\n\
 649                                                                           Please report to flightgear-devel@flightgear.org\n");
 650         }
 651
 652         //cout << (solution_found ? "Result: solution found\n" : "Result: no solution found\n");
 653         return(solution_path.path);             // TODO - we really ought to have a fallback position incase a solution isn't found.
 654 }
 655
 656
 657
 658 // Randomly or otherwise populate some of the gates with parked planes
 659 // (might eventually be done by the AIMgr if and when lots of AI traffic is generated)
 660
 661 // Return a list of exits from a given runway
 662 // It is up to the calling function to check for non-zero size of returned array before use
 663 node_array_type FGGround::GetExits(const string& rwyID) {
 664         // FIXME - get a 07L or similar in here and we're stuffed!!!
 665         return(runways[atoi(rwyID.c_str())].exits);
 666 }
 667
 668 void FGGround::RequestDeparture(const PlaneRec& plane, FGAIEntity* requestee) {
 669         // For now we'll just automatically clear all planes to the runway hold.
 670         // This communication needs to be delayed 20 sec or so from receiving the request.
 671         // Even if display=false we still need to start the timer in case display=true when communication starts.
 672         // We also need to bear in mind we also might have other outstanding communications, although for now we'll punt that issue!
 673         // FIXME - sort the above!
 674
 675         // HACK - assume that anything requesting departure is new for now - FIXME LATER
 676         GroundRec* g = new GroundRec;
 677         g->plane = plane;
 678         g->planePtr = requestee;
 679         g->taxiRequestOutstanding = true;
 680         g->clearanceCounter = 0;
 681         g->cleared = false;
 682         g->incoming = false;
 683         // TODO - need to handle the next 3 as well
 684     //Point3D current_pos;
 685     //node* destination;
 686     //node* last_clearance;
 687
 688         ground_traffic.push_back(g);
 689 }
 690
 691 #if 0
 692 void FGGround::NewArrival(plane_rec plane) {
 693         // What are we going to do here?
 694         // We need to start a new ground_rec and add the plane_rec to it
 695         // We need to decide what gate we are going to clear it to.
 696         // Then we need to add clearing it to that gate to the pending transmissions queue? - or simply transmit?
 697         // Probably simply transmit for now and think about a transmission queue later if we need one.
 698         // We might need one though in order to add a little delay for response time.
 699         ground_rec* g = new ground_rec;
 700         g->plane_rec = plane;
 701         g->current_pos = ConvertWGS84ToXY(plane.pos);
 702         g->node = GetNode(g->current_pos);  // TODO - might need to sort out node/arc here
 703         AssignGate(g);
 704         g->cleared = false;
 705         ground_traffic.push_back(g);
 706         NextClearance(g);
 707 }
 708
 709 void FGGround::NewContact(plane_rec plane) {
 710         // This is a bit of a convienience function at the moment and is likely to change.
 711         if(at a gate or apron)
 712                 NewDeparture(plane);
 713         else
 714                 NewArrival(plane);
 715 }
 716
 717 void FGGround::NextClearance(ground_rec &g) {
 718         // Need to work out where we can clear g to.
 719         // Assume the pilot doesn't need progressive instructions
 720         // We *should* already have a gate or holding point assigned by the time we get here
 721         // but it wouldn't do any harm to check.
 722
 723         // For now though we will hardwire it to clear to the final destination.
 724 }
 725
 726 void FGGround::AssignGate(ground_rec &g) {
 727         // We'll cheat for now - since we only have the user's aircraft and a couple of airports implemented
 728         // we'll hardwire the gate!
 729         // In the long run the logic of which gate or area to send the plane to could be somewhat non-trivial.
 730 }
 731 #endif //0
 732