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 <math.h>
  28 #include <algorithm>
  29
  30 //#include <plib/sg.h>
  31 //#include <plib/ul.h>
  32
  33 //#include <Environment/environment_mgr.hxx>
  34 //#include <Environment/environment.hxx>
  35 //#include <simgear/misc/sg_path.hxx>
  36 //#include <simgear/props/props.hxx>
  37 //#include <simgear/structure/subsystem_mgr.hxx>
  38 #include <simgear/debug/logstream.hxx>
  39 #include <simgear/route/waypoint.hxx>
  40 //#include <Main/globals.hxx>
  41 //#include <Main/fg_props.hxx>
  42 //#include <Airports/runways.hxx>
  43
  44 #include <AIModel/AIFlightPlan.hxx>
  45
  46 //#include STL_STRING
  47
  48 #include "groundnetwork.hxx"
  49
  50 SG_USING_STD(sort);
  51
  52 /**************************************************************************
  53  * FGTaxiNode
  54  *************************************************************************/
  55 FGTaxiNode::FGTaxiNode()
  56 {
  57 }
  58
  59 /***************************************************************************
  60  * FGTaxiSegment
  61  **************************************************************************/
  62 FGTaxiSegment::FGTaxiSegment()
  63 {
  64   oppositeDirection = 0;
  65 }
  66
  67 void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes)
  68 {
  69   FGTaxiNodeVectorIterator i = nodes->begin();
  70   while (i != nodes->end())
  71     {
  72       if (i->getIndex() == startNode)
  73         {
  74           start = i->getAddress();
  75           i->addSegment(this);
  76           return;
  77         }
  78       i++;
  79     }
  80 }
  81
  82 void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes)
  83 {
  84   FGTaxiNodeVectorIterator i = nodes->begin();
  85   while (i != nodes->end())
  86     {
  87       if (i->getIndex() == endNode)
  88         {
  89           end = i->getAddress();
  90           return;
  91         }
  92       i++;
  93     }
  94 }
  95
  96 // There is probably a computationally cheaper way of
  97 // doing this.
  98 void FGTaxiSegment::setTrackDistance()
  99 {
 100   double course;
 101   SGWayPoint first  (start->getLongitude(),
 102                      start->getLatitude(),
 103                      0);
 104   SGWayPoint second (end->getLongitude(),
 105                      end->getLatitude(),
 106                      0);
 107   first.CourseAndDistance(second, &course, &length);
 108 }
 109 /***************************************************************************
 110  * FGTaxiRoute
 111  **************************************************************************/
 112 bool FGTaxiRoute::next(int *nde)
 113 {
 114   //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 115   //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 116   //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 117   //if (currNode != nodes.end())
 118   //  cerr << "true" << endl;
 119   //else
 120   //  cerr << "false" << endl;
 121   //if (nodes.size() != (routes.size()) +1)
 122   //  cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
 123
 124   if (currNode == nodes.end())
 125     return false;
 126   *nde = *(currNode);
 127   if (currNode != nodes.begin()) // make sure route corresponds to the end node
 128     currRoute++;
 129   currNode++;
 130   return true;
 131 };
 132
 133 bool FGTaxiRoute::next(int *nde, int *rte)
 134 {
 135   //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 136   //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 137   //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 138   //if (currNode != nodes.end())
 139   //  cerr << "true" << endl;
 140   //else
 141   //  cerr << "false" << endl;
 142   if (nodes.size() != (routes.size()) +1) {
 143     SG_LOG(SG_GENERAL, SG_ALERT, "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size());
 144     exit(1);
 145   }
 146   if (currNode == nodes.end())
 147     return false;
 148   *nde = *(currNode);
 149   //*rte = *(currRoute);
 150   if (currNode != nodes.begin()) // Make sure route corresponds to the end node
 151     {
 152       *rte = *(currRoute);
 153       currRoute++;
 154     }
 155   else
 156     {
 157       // If currNode points to the first node, this means the aircraft is not on the taxi node
 158       // yet. Make sure to return a unique identifyer in this situation though, because otherwise
 159       // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
 160       // taxi route or not. the negative of the preceding route seems a logical choice, as it is
 161       // unique for any starting location.
 162       // Note that this is probably just a temporary fix until I get Parking / tower control working.
 163       *rte = -1 * *(currRoute);
 164     }
 165   currNode++;
 166   return true;
 167 };
 168
 169 void FGTaxiRoute::rewind(int route)
 170 {
 171   int currPoint;
 172   int currRoute;
 173   first();
 174   do {
 175     if (!(next(&currPoint, &currRoute))) {
 176       SG_LOG(SG_GENERAL,SG_ALERT, "Error in rewinding TaxiRoute: current" << currRoute
 177              << " goal " << route);
 178     }
 179   } while (currRoute != route);
 180 }
 181
 182 /***************************************************************************
 183  * FGTrafficRecord
 184  **************************************************************************/
 185 void FGTrafficRecord::setPositionAndIntentions(int pos, FGAIFlightPlan *route)
 186 {
 187
 188   currentPos = pos;
 189   if (intentions.size()) {
 190     if (*intentions.begin() != pos) {
 191       SG_LOG(SG_GENERAL, SG_ALERT, "Error in FGTrafficRecord::setPositionAndIntentions");
 192       cerr << "Pos : " << pos << " Curr " << *(intentions.begin())  << endl;
 193       for (intVecIterator i = intentions.begin(); i != intentions.end() ; i++) {
 194         cerr << (*i) << " ";
 195       }
 196       cerr << endl;
 197     }
 198     intentions.erase(intentions.begin());
 199   } else {
 200     //int legNr, routeNr;
 201     //FGAIFlightPlan::waypoint* const wpt= route->getCurrentWaypoint();
 202     int size = route->getNrOfWayPoints();
 203     cerr << "Setting pos" << pos << " ";
 204     cerr << "setting intentions ";
 205     for (int i = 0; i < size; i++) {
 206       int val = route->getRouteIndex(i);
 207
 208       if ((val) && (val != pos))
 209         {
 210           intentions.push_back(val);
 211           cerr << val<< " ";
 212         }
 213     }
 214     cerr << endl;
 215     //while (route->next(&legNr, &routeNr)) {
 216     //intentions.push_back(routeNr);
 217     //}
 218     //route->rewind(currentPos);
 219   }
 220   //exit(1);
 221 }
 222
 223 bool FGTrafficRecord::checkPositionAndIntentions(FGTrafficRecord &other)
 224 {
 225   bool result = false;
 226   //cerr << "Start check 1" << endl;
 227   if (currentPos == other.currentPos)
 228     {
 229       //cerr << "Check Position and intentions: current matches" << endl;
 230       result = true;
 231     }
 232  //  else if (other.intentions.size())
 233 //     {
 234 //       cerr << "Start check 2" << endl;
 235 //       intVecIterator i = other.intentions.begin();
 236 //       while (!((i == other.intentions.end()) || ((*i) == currentPos)))
 237 //      i++;
 238 //       if (i != other.intentions.end()) {
 239 //      cerr << "Check Position and intentions: current matches other.intentions" << endl;
 240 //      result = true;
 241 //       }
 242   else if (intentions.size()) {
 243     //cerr << "Start check 3" << endl;
 244     intVecIterator i = intentions.begin();
 245     while (!((i == intentions.end()) || ((*i) == other.currentPos)))
 246       i++;
 247     if (i != intentions.end()) {
 248       //cerr << "Check Position and intentions: .other.current matches" << endl;
 249       result = true;
 250     }
 251   }
 252   //cerr << "Done !!" << endl;
 253   return result;
 254 }
 255
 256 void FGTrafficRecord::setPositionAndHeading(double lat, double lon, double hdg,
 257                                             double spd, double alt)
 258 {
 259   latitude = lat;
 260   longitude = lon;
 261   heading = hdg;
 262   speed = spd;
 263   altitude = alt;
 264 }
 265
 266 int FGTrafficRecord::crosses(FGGroundNetwork *net, FGTrafficRecord &other)
 267 {
 268   if (checkPositionAndIntentions(other) || (other.checkPositionAndIntentions(*this)))
 269     return -1;
 270   intVecIterator i, j;
 271   int currentTargetNode = 0, otherTargetNode = 0;
 272   if (currentPos > 0)
 273     currentTargetNode = net->findSegment(currentPos      )->getEnd()->getIndex(); // OKAY,...
 274   if (other.currentPos > 0)
 275     otherTargetNode   = net->findSegment(other.currentPos)->getEnd()->getIndex(); // OKAY,...
 276   if ((currentTargetNode == otherTargetNode) && currentTargetNode > 0)
 277     return currentTargetNode;
 278   if (intentions.size())
 279     {
 280       for (i = intentions.begin(); i != intentions.end(); i++)
 281         {
 282           if (currentTargetNode == net->findSegment(*i)->getEnd()->getIndex())
 283             {
 284               cerr << "Current crosses at " << currentTargetNode <<endl;
 285               return currentTargetNode;
 286             }
 287
 288         }
 289     }
 290   if (other.intentions.size())
 291     {
 292       for (i = other.intentions.begin(); i != other.intentions.end(); i++)
 293         {
 294           if (otherTargetNode == net->findSegment(*i)->getEnd()->getIndex())
 295             {
 296               cerr << "Other crosses at " << currentTargetNode <<endl;
 297               return otherTargetNode;
 298             }
 299         }
 300     }
 301   if (intentions.size() && other.intentions.size())
 302     {
 303       for (i = intentions.begin(); i != intentions.end(); i++)
 304         {
 305           for (j = other.intentions.begin(); j != other.intentions.end(); j++)
 306             {
 307               //cerr << "finding segment " << *i << " and " << *j << endl;
 308               currentTargetNode = net->findSegment(*i)->getEnd()->getIndex();
 309               otherTargetNode   = net->findSegment(*j)->getEnd()->getIndex();
 310               if (currentTargetNode == otherTargetNode)
 311                 {
 312                   //cerr << "Routes will cross at " << currentTargetNode << endl;
 313                   return currentTargetNode;
 314                 }
 315             }
 316         }
 317     }
 318   return -1;
 319 }
 320
 321 bool FGTrafficRecord::isOpposing (FGGroundNetwork *net, FGTrafficRecord &other, int node)
 322 {
 323   // Check if current segment is the reverse segment for the other aircraft
 324   FGTaxiSegment *opp;
 325   //cerr << "Current segment " << currentPos << endl;
 326   if ((currentPos > 0) && (other.currentPos > 0))
 327     {
 328       opp = net->findSegment(currentPos)->opposite();
 329       if (opp) {
 330         if (opp->getIndex() == other.currentPos)
 331           return true;
 332       }
 333
 334       for (intVecIterator i = intentions.begin(); i != intentions.end(); i++)
 335         {
 336           for (intVecIterator j = intentions.begin(); j != intentions.end(); j++)
 337             {
 338               // cerr << "Current segment 1 " << (*i) << endl;
 339               if (opp = net->findSegment(*i)->opposite())
 340                 {
 341                   if (opp->getIndex() ==
 342                       net->findSegment(*j)->getIndex())
 343                     {
 344                       cerr << "Nodes " << net->findSegment(*i)->getIndex()
 345                            << " and  " << net->findSegment(*j)->getIndex()
 346                            << " are opposites " << endl;
 347                       if (net->findSegment(*i)->getStart()->getIndex() == node) {
 348                         {
 349                           cerr << "Found the node" << endl;
 350                           return true;
 351                         }
 352                       }
 353                     }
 354                 }
 355             }
 356         }
 357     }
 358   return false;
 359 }
 360
 361
 362 /***************************************************************************
 363  * FGGroundNetwork()
 364  **************************************************************************/
 365
 366 FGGroundNetwork::FGGroundNetwork()
 367 {
 368   hasNetwork = false;
 369   foundRoute = false;
 370   totalDistance = 0;
 371   maxDistance = 0;
 372   currTraffic = activeTraffic.begin();
 373
 374 }
 375
 376 void FGGroundNetwork::addSegment(const FGTaxiSegment &seg)
 377 {
 378   segments.push_back(seg);
 379 }
 380
 381 void FGGroundNetwork::addNode(const FGTaxiNode &node)
 382 {
 383   nodes.push_back(node);
 384 }
 385
 386 void FGGroundNetwork::addNodes(FGParkingVec *parkings)
 387 {
 388   FGTaxiNode n;
 389   FGParkingVecIterator i = parkings->begin();
 390   while (i != parkings->end())
 391     {
 392       n.setIndex(i->getIndex());
 393       n.setLatitude(i->getLatitude());
 394       n.setLongitude(i->getLongitude());
 395       nodes.push_back(n);
 396
 397       i++;
 398     }
 399 }
 400
 401
 402
 403 void FGGroundNetwork::init()
 404 {
 405   hasNetwork = true;
 406   int index = 1;
 407   sort(nodes.begin(), nodes.end());
 408   sort(segments.begin(), segments.end());
 409   FGTaxiSegmentVectorIterator i = segments.begin();
 410   while(i != segments.end()) {
 411     //cerr << "initializing node " << i->getIndex() << endl;
 412     i->setStart(&nodes);
 413     i->setEnd  (&nodes);
 414     i->setTrackDistance();
 415     i->setIndex(index);
 416     //cerr << "Track distance = " << i->getLength() << endl;
 417     //cerr << "Track ends at"      << i->getEnd()->getIndex() << endl;
 418     i++;
 419     index++;
 420   }
 421   i = segments.begin();
 422   while(i != segments.end()) {
 423     FGTaxiSegmentPointerVectorIterator j = i->getEnd()->getBeginRoute();
 424     while (j != i->getEnd()->getEndRoute())
 425       {
 426         if ((*j)->getEnd()->getIndex() == i->getStart()->getIndex())
 427           {
 428             int start1 = i->getStart()->getIndex();
 429             int end1   = i->getEnd()  ->getIndex();
 430             int start2 = (*j)->getStart()->getIndex();
 431             int end2   = (*j)->getEnd()->getIndex();
 432             int oppIndex = (*j)->getIndex();
 433             cerr << "Oppossite of  " << i->getIndex() << " (" << start1 << "," << end1 << ") "
 434                  << "happens to be " << oppIndex      << " (" << start2 << "," << end2 << ") " << endl;
 435             break;
 436           }
 437           j++;
 438       }
 439     i++;
 440   }
 441   //exit(1);
 442 }
 443
 444 int FGGroundNetwork::findNearestNode(double lat, double lon)
 445 {
 446   double minDist = HUGE_VAL;
 447   double dist;
 448   int index;
 449   SGWayPoint first  (lon,
 450                      lat,
 451                      0);
 452
 453   for (FGTaxiNodeVectorIterator
 454          itr = nodes.begin();
 455        itr != nodes.end(); itr++)
 456     {
 457       double course;
 458       SGWayPoint second (itr->getLongitude(),
 459                          itr->getLatitude(),
 460                          0);
 461       first.CourseAndDistance(second, &course, &dist);
 462       if (dist < minDist)
 463         {
 464           minDist = dist;
 465           index = itr->getIndex();
 466           //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
 467         }
 468     }
 469   return index;
 470 }
 471
 472 FGTaxiNode *FGGroundNetwork::findNode(int idx)
 473 { /*
 474     for (FGTaxiNodeVectorIterator
 475     itr = nodes.begin();
 476     itr != nodes.end(); itr++)
 477     {
 478     if (itr->getIndex() == idx)
 479     return itr->getAddress();
 480     }*/
 481
 482   if ((idx >= 0) && (idx < nodes.size()))
 483     return nodes[idx].getAddress();
 484   else
 485     return 0;
 486 }
 487
 488 FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
 489 {/*
 490   for (FGTaxiSegmentVectorIterator
 491          itr = segments.begin();
 492        itr != segments.end(); itr++)
 493     {
 494       if (itr->getIndex() == idx)
 495         return itr->getAddress();
 496     }
 497  */
 498   if ((idx > 0) && (idx <= segments.size()))
 499     return segments[idx-1].getAddress();
 500   else
 501     {
 502       cerr << "Alert: trying to find invalid segment " << idx << endl;
 503       return 0;
 504     }
 505 }
 506
 507 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
 508 {
 509   foundRoute = false;
 510   totalDistance = 0;
 511   FGTaxiNode *firstNode = findNode(start);
 512   FGTaxiNode *lastNode  = findNode(end);
 513   //prevNode = prevPrevNode = -1;
 514   //prevNode = start;
 515   routes.clear();
 516   nodesStack.clear();
 517   routesStack.clear();
 518
 519   trace(firstNode, end, 0, 0);
 520   FGTaxiRoute empty;
 521
 522   if (!foundRoute)
 523     {
 524       SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
 525       exit(1);
 526     }
 527   sort(routes.begin(), routes.end());
 528   //for (intVecIterator i = route.begin(); i != route.end(); i++)
 529   //  {
 530   //    rte->push_back(*i);
 531   //  }
 532
 533   if (routes.begin() != routes.end())
 534     return *(routes.begin());
 535   else
 536     return empty;
 537 }
 538
 539
 540 void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance)
 541 {
 542   // Just check some preconditions of the trace algorithm
 543   if (nodesStack.size() != routesStack.size())
 544     {
 545       SG_LOG(SG_GENERAL, SG_ALERT, "size of nodesStack and routesStack is not equal. NodesStack :"
 546              << nodesStack.size() << ". RoutesStack : " << routesStack.size());
 547     }
 548   nodesStack.push_back(currNode->getIndex());
 549   totalDistance += distance;
 550   //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl;
 551   //<< currNode->getIndex() << endl;
 552
 553   // If the current route matches the required end point we found a valid route
 554   // So we can add this to the routing table
 555   if (currNode->getIndex() == end)
 556     {
 557       //cerr << "Found route : " <<  totalDistance << "" << " " << *(nodesStack.end()-1) << endl;
 558       routes.push_back(FGTaxiRoute(nodesStack,routesStack,totalDistance));
 559       if (nodesStack.empty() || routesStack.empty())
 560         {
 561           printRoutingError(string("while finishing route"));
 562         }
 563       nodesStack.pop_back();
 564       routesStack.pop_back();
 565       if (!(foundRoute))
 566         maxDistance = totalDistance;
 567       else
 568         if (totalDistance < maxDistance)
 569           maxDistance = totalDistance;
 570       foundRoute = true;
 571       totalDistance -= distance;
 572       return;
 573     }
 574
 575
 576   // search if the currentNode has been encountered before
 577   // if so, we should step back one level, because it is
 578   // rather rediculous to proceed further from here.
 579   // if the current node has not been encountered before,
 580   // i should point to nodesStack.end()-1; and we can continue
 581   // if i is not nodesStack.end, the previous node was found,
 582   // and we should return.
 583   // This only works at trace levels of 1 or higher though
 584   if (depth > 0) {
 585     intVecIterator i = nodesStack.begin();
 586     while ((*i) != currNode->getIndex()) {
 587       //cerr << "Route so far : " << (*i) << endl;
 588       i++;
 589     }
 590     if (i != nodesStack.end()-1) {
 591       if (nodesStack.empty() || routesStack.empty())
 592         {
 593           printRoutingError(string("while returning from an already encountered node"));
 594         }
 595       nodesStack.pop_back();
 596       routesStack.pop_back();
 597       totalDistance -= distance;
 598       return;
 599     }
 600     // If the total distance from start to the current waypoint
 601     // is longer than that of a route we can also stop this trace
 602     // and go back one level.
 603     if ((totalDistance > maxDistance) && foundRoute)
 604       {
 605         //cerr << "Stopping rediculously long trace: " << totalDistance << endl;
 606         if (nodesStack.empty() || routesStack.empty())
 607         {
 608           printRoutingError(string("while returning from finding a rediculously long route"));
 609         }
 610         nodesStack.pop_back();
 611         routesStack.pop_back();
 612         totalDistance -= distance;
 613         return;
 614       }
 615   }
 616
 617   //cerr << "2" << endl;
 618   if (currNode->getBeginRoute() != currNode->getEndRoute())
 619     {
 620       //cerr << "3" << endl;
 621       for (FGTaxiSegmentPointerVectorIterator
 622              i = currNode->getBeginRoute();
 623            i != currNode->getEndRoute();
 624            i++)
 625         {
 626           //cerr << (*i)->getLength() << endl;
 627           //cerr << (*i)->getIndex() << endl;
 628           int idx = (*i)->getIndex();
 629           routesStack.push_back((*i)->getIndex());
 630           trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
 631         //  {
 632         //      // cerr << currNode -> getIndex() << " ";
 633         //      route.push_back(currNode->getIndex());
 634         //      return true;
 635         //    }
 636         }
 637     }
 638   else
 639     {
 640       //SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
 641     }
 642   if (nodesStack.empty())
 643     {
 644       printRoutingError(string("while finishing trace"));
 645     }
 646   nodesStack.pop_back();
 647   // Make sure not to dump the level-zero routesStack entry, because that was never created.
 648   if (depth)
 649     {
 650       routesStack.pop_back();
 651       //cerr << "leaving trace " << routesStack.size() << endl;
 652     }
 653   totalDistance -= distance;
 654   return;
 655 }
 656
 657 void FGGroundNetwork::printRoutingError(string mess)
 658 {
 659   SG_LOG(SG_GENERAL, SG_ALERT,  "Error in ground network trace algorithm " << mess);
 660   if (nodesStack.empty())
 661     {
 662       SG_LOG(SG_GENERAL, SG_ALERT, " nodesStack is empty. Dumping routesStack");
 663       for (intVecIterator i = routesStack.begin() ; i != routesStack.end(); i++)
 664         SG_LOG(SG_GENERAL, SG_ALERT, "Route " << (*i));
 665     }
 666   if (routesStack.empty())
 667     {
 668       SG_LOG(SG_GENERAL, SG_ALERT, " routesStack is empty. Dumping nodesStack");
 669       for (intVecIterator i = nodesStack.begin() ; i != nodesStack.end(); i++)
 670         SG_LOG(SG_GENERAL, SG_ALERT, "Node " << (*i));
 671     }
 672   //exit(1);
 673 }
 674
 675
 676 void FGGroundNetwork::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition,
 677                                        double lat, double lon, double heading,
 678                                        double speed, double alt, double radius)
 679 {
 680   TrafficVectorIterator i = activeTraffic.begin();
 681   // Search search if the current id alread has an entry
 682   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 683   if (activeTraffic.size()) {
 684     while ((i->getId() != id) && i != activeTraffic.end()) {
 685       i++;
 686     }
 687   }
 688   // Add a new TrafficRecord if no one exsists for this aircraft.
 689   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 690     FGTrafficRecord rec;
 691     rec.setId(id);
 692     rec.setPositionAndIntentions(currentPosition, intendedRoute);
 693     rec.setPositionAndHeading(lat, lon, heading, speed, alt);
 694     rec.setRadius(radius); // only need to do this when creating the record.
 695     activeTraffic.push_back(rec);
 696   } else {
 697     i->setPositionAndIntentions(currentPosition, intendedRoute);
 698     i->setPositionAndHeading(lat, lon, heading, speed, alt);
 699   }
 700 }
 701
 702 void FGGroundNetwork::signOff(int id) {
 703  TrafficVectorIterator i = activeTraffic.begin();
 704   // Search search if the current id alread has an entry
 705   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 706   if (activeTraffic.size()) {
 707     while ((i->getId() != id) && i != activeTraffic.end()) {
 708       i++;
 709     }
 710   }
 711   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 712     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Aircraft without traffic record is signing off");
 713   } else {
 714       i = activeTraffic.erase(i);
 715   }
 716 }
 717
 718 void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt,
 719                              double dt) {
 720   TrafficVectorIterator i = activeTraffic.begin();
 721   // Search search if the current id has an entry
 722   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 723   TrafficVectorIterator current, closest;
 724   if (activeTraffic.size()) {
 725     while ((i->getId() != id) && i != activeTraffic.end()) {
 726       i++;
 727     }
 728   }
 729   // update position of the current aircraft
 730   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 731     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: updating aircraft without traffic record");
 732   } else {
 733     i->setPositionAndHeading(lat, lon, heading, speed, alt);
 734     current = i;
 735   }
 736
 737   setDt(getDt() + dt);
 738
 739   // Update every three secs, but add some randomness
 740   // to prevent all IA objects doing this in synchrony
 741   //if (getDt() < (3.0) + (rand() % 10))
 742   //  return;
 743   //else
 744   //  setDt(0);
 745   checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
 746   checkHoldPosition   (id, lat, lon, heading, speed, alt);
 747 }
 748
 749 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
 750                                            double lon, double heading,
 751                                            double speed, double alt)
 752 {
 753
 754   // Scan for a speed adjustment change. Find the nearest aircraft that is in front
 755   // and adjust speed when we get too close. Only do this when current position and/or
 756   // intentions of the current aircraft match current taxiroute position of the proximate
 757   // aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
 758   // instruction. See below for the hold position instruction.
 759   TrafficVectorIterator current, closest;
 760   TrafficVectorIterator i = activeTraffic.begin();
 761   if (activeTraffic.size())
 762     {
 763       while ((i->getId() != id) && (i != activeTraffic.end()))
 764         {
 765           i++;
 766         }
 767     }
 768   else
 769     {
 770       return;
 771     }
 772   current = i;
 773   double mindist = HUGE;
 774   if (activeTraffic.size())
 775     {
 776       double course, dist, bearing, minbearing;
 777
 778       //TrafficVector iterator closest;
 779       for (TrafficVectorIterator i = activeTraffic.begin();
 780            i != activeTraffic.end(); i++)
 781         {
 782           if (i != current) {
 783             SGWayPoint curr  (lon,
 784                               lat,
 785                               alt);
 786             SGWayPoint other    (i->getLongitude  (),
 787                                  i->getLatitude (),
 788                                  i->getAltitude  ());
 789             other.CourseAndDistance(curr, &course, &dist);
 790             bearing = fabs(heading-course);
 791             if (bearing > 180)
 792               bearing = 360-bearing;
 793             if ((dist < mindist) && (bearing < 60.0))
 794               {
 795                 mindist = dist;
 796                 closest = i;
 797                 minbearing = bearing;
 798               }
 799           }
 800         }
 801       //cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading
 802       //   << " course : " << course << endl;
 803       current->clearSpeedAdjustment();
 804       // Only clear the heading adjustment at positive speeds, otherwise the waypoint following
 805       // code wreaks havoc
 806       if (speed > 0.2)
 807         current->clearHeadingAdjustment();
 808       // All clear
 809       if (mindist > 100)
 810         {
 811           //current->clearSpeedAdjustment();
 812           //current->clearHeadingAdjustment();
 813         }
 814       else
 815         {
 816           if (current->getId() == closest->getWaitsForId())
 817             return;
 818           else
 819             current->setWaitsForId(closest->getId());
 820
 821           // Getting close: Slow down to a bit less than the other aircraft
 822           double maxAllowableDistance = (1.1*current->getRadius()) + (1.1*closest->getRadius());
 823           if (mindist > maxAllowableDistance)
 824             {
 825               if (current->checkPositionAndIntentions(*closest))
 826                 {
 827                   // Adjust speed, but don't let it drop to below 1 knots
 828                   //if (fabs(speed) > 1)
 829                   if (!(current->hasHeadingAdjustment()))
 830                     {
 831                       current->setSpeedAdjustment(closest->getSpeed()* (mindist/100));
 832                       //cerr << "Adjusting speed to " << closest->getSpeed() * (mindist / 100) << " "
 833                       //         << "Bearing = " << minbearing << " Distance = " << mindist
 834                       //         << " Latitude = " <<lat << " longitude = " << lon << endl;
 835                       //<< " Latitude = " <<closest->getLatitude()
 836                       //<< " longitude = " << closest->getLongitude()
 837                       //  << endl;
 838                     }
 839                   else
 840                     {
 841                       double newSpeed = (maxAllowableDistance-mindist);
 842                       current->setSpeedAdjustment(newSpeed);
 843                     }
 844                 }
 845             }
 846           else
 847             {
 848               if (!(current->hasHeadingAdjustment()))
 849                 {
 850                   double newSpeed;
 851                   if (mindist > 10) {
 852                     newSpeed = 0.01;
 853                     current->setSpeedAdjustment(newSpeed);
 854                   } else {
 855                     newSpeed = -1 * (maxAllowableDistance-mindist);
 856                     current->setSpeedAdjustment(newSpeed);
 857                     current->setHeadingAdjustment(heading);
 858                   }
 859                 }
 860             }
 861         }
 862     }
 863 }
 864
 865 void FGGroundNetwork::checkHoldPosition(int id, double lat,
 866                                         double lon, double heading,
 867                                         double speed, double alt)
 868 {
 869   // Check for "Hold position instruction".
 870   // The hold position should be issued under the following conditions:
 871   // 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
 872   // 2) For taxiing aircraft that use one taxiway in opposite directions
 873   // 3) For crossing or merging taxiroutes.
 874
 875   TrafficVectorIterator current, closest;
 876   TrafficVectorIterator i = activeTraffic.begin();
 877   if (activeTraffic.size())
 878     {
 879       while ((i->getId() != id) && i != activeTraffic.end())
 880         {
 881           i++;
 882         }
 883     }
 884   else
 885     {
 886       return ;
 887     }
 888   current = i;
 889   current->setHoldPosition(false);
 890   double course, dist, bearing, minbearing;
 891   for (i = activeTraffic.begin();
 892        i != activeTraffic.end(); i++)
 893     {
 894       if (i != current)
 895         {
 896           int node = current->crosses(this, *i);
 897           if (node != -1)
 898             {
 899               // Determine whether it's save to continue or not.
 900               // If we have a crossing route, there are two possibilities:
 901               // 1) This is an interestion
 902               // 2) This is oncoming two-way traffic, using the same taxiway.
 903               //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
 904               SGWayPoint nodePos(findNode(node)->getLongitude  (),
 905                                  findNode(node)->getLatitude   (),
 906                                  alt);
 907               SGWayPoint curr  (lon,
 908                                 lat,
 909                                 alt);
 910
 911               SGWayPoint other    (i->getLongitude  (),
 912                                    i->getLatitude (),
 913                                    i->getAltitude  ());
 914               //other.CourseAndDistance(curr, &course, &dist);
 915               bool needsToWait;
 916               if (current->isOpposing(this, *i, node))
 917                 {
 918                   needsToWait = true;
 919                   //cerr << "Hold check 2 : " << id << "  has opposing segment " << endl;
 920                   // issue a "Hold Position" as soon as we're close to the offending node
 921                   // For now, I'm doing this as long as the other aircraft doesn't
 922                   // have a hold instruction as soon as we're within a reasonable
 923                   // distance from the offending node.
 924                   // This may be a bit of a conservative estimate though, as it may
 925                   // be well possible that both aircraft can both continue to taxi
 926                   // without crashing into each other.
 927                 }
 928               else
 929                 {
 930                   other.CourseAndDistance(nodePos, &course, &dist);
 931                   if (dist > 2.0*i->getRadius())
 932                     {
 933                       needsToWait = false;
 934                       //cerr << "Hold check 3 : " << id <<"  Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
 935                       //           << endl;
 936                     }
 937                   else
 938                     {
 939                       needsToWait = true;
 940                       //cerr << "Hold check 4: " << id << "  Would need to wait for other aircraft : distance = " << dist << " nm" << endl;
 941                     }
 942                 }
 943               curr.CourseAndDistance(nodePos, &course, &dist);
 944               if (!(i->hasHoldPosition()))
 945                 {
 946
 947                   if ((dist < 2.5*current->getRadius()) &&
 948                       (needsToWait) &&
 949                       (!(current->getId() == i->getWaitsForId())) &&
 950                       (!(current->getSpeedAdjustment())))
 951
 952                     {
 953                       current->setHoldPosition(true);
 954                       //cerr << "Hold check 5: " << id <<"  Setting Hold Position: distance to node : " << dist << " nm"<< endl;
 955                     }
 956                   else
 957                     {
 958                       //cerr << "Hold check 6: " << id << "  No need to hold yet: Distance to node : " << dist << " nm"<< endl;
 959                     }
 960                 }
 961             }
 962         }
 963     }
 964 }
 965
 966
 967 bool FGGroundNetwork::hasInstruction(int id)
 968 {
 969    TrafficVectorIterator i = activeTraffic.begin();
 970   // Search search if the current id has an entry
 971   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 972   if (activeTraffic.size())
 973     {
 974       while ((i->getId() != id) && i != activeTraffic.end()) {
 975       i++;
 976     }
 977   }
 978   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 979     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: checking ATC instruction for aircraft without traffic record");
 980   } else {
 981     return i->hasInstruction();
 982   }
 983   return false;
 984 }
 985
 986 FGATCInstruction FGGroundNetwork::getInstruction(int id)
 987 {
 988   TrafficVectorIterator i = activeTraffic.begin();
 989   // Search search if the current id has an entry
 990   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 991   if (activeTraffic.size()) {
 992     while ((i->getId() != id) && i != activeTraffic.end()) {
 993       i++;
 994     }
 995   }
 996   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 997     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: requesting ATC instruction for aircraft without traffic record");
 998   } else {
 999     return i->getInstruction();
1000   }
1001   return FGATCInstruction();
1002 }
1003
1004
1005
1006
1007 /***************************************************************************
1008  * FGATCInstruction
1009  *
1010  * This class is really out of place here, and should be combined with
1011  * FGATC controller and go into it's own file / directory
1012  * I'm leaving it for now though, because I'm testing this stuff quite
1013  * heavily.
1014  **************************************************************************/
1015 FGATCInstruction::FGATCInstruction()
1016 {
1017   holdPattern    = false;
1018   holdPosition   = false;
1019   changeSpeed    = false;
1020   changeHeading  = false;
1021   changeAltitude = false;
1022
1023   double speed   = 0;
1024   double heading = 0;
1025   double alt     = 0;
1026 }
1027
1028 bool FGATCInstruction::hasInstruction()
1029 {
1030   return (holdPattern || holdPosition || changeSpeed || changeHeading || changeAltitude);
1031 }
1032
1033