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 }
  65
  66 void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes)
  67 {
  68   FGTaxiNodeVectorIterator i = nodes->begin();
  69   while (i != nodes->end())
  70     {
  71       if (i->getIndex() == startNode)
  72         {
  73           start = i->getAddress();
  74           i->addSegment(this);
  75           return;
  76         }
  77       i++;
  78     }
  79 }
  80
  81 void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes)
  82 {
  83   FGTaxiNodeVectorIterator i = nodes->begin();
  84   while (i != nodes->end())
  85     {
  86       if (i->getIndex() == endNode)
  87         {
  88           end = i->getAddress();
  89           return;
  90         }
  91       i++;
  92     }
  93 }
  94
  95 // There is probably a computationally cheaper way of
  96 // doing this.
  97 void FGTaxiSegment::setTrackDistance()
  98 {
  99   double course;
 100   SGWayPoint first  (start->getLongitude(),
 101                      start->getLatitude(),
 102                      0);
 103   SGWayPoint second (end->getLongitude(),
 104                      end->getLatitude(),
 105                      0);
 106   first.CourseAndDistance(second, &course, &length);
 107 }
 108 /***************************************************************************
 109  * FGTaxiRoute
 110  **************************************************************************/
 111 bool FGTaxiRoute::next(int *nde)
 112 {
 113   //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 114   //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 115   //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 116   //if (currNode != nodes.end())
 117   //  cerr << "true" << endl;
 118   //else
 119   //  cerr << "false" << endl;
 120   //if (nodes.size() != (routes.size()) +1)
 121   //  cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
 122
 123   if (currNode == nodes.end())
 124     return false;
 125   *nde = *(currNode);
 126   if (currNode != nodes.begin()) // make sure route corresponds to the end node
 127     currRoute++;
 128   currNode++;
 129   return true;
 130 };
 131
 132 bool FGTaxiRoute::next(int *nde, int *rte)
 133 {
 134   //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 135   //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 136   //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 137   //if (currNode != nodes.end())
 138   //  cerr << "true" << endl;
 139   //else
 140   //  cerr << "false" << endl;
 141   if (nodes.size() != (routes.size()) +1) {
 142     SG_LOG(SG_GENERAL, SG_ALERT, "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size());
 143     exit(1);
 144   }
 145   if (currNode == nodes.end())
 146     return false;
 147   *nde = *(currNode);
 148   //*rte = *(currRoute);
 149   if (currNode != nodes.begin()) // Make sure route corresponds to the end node
 150     {
 151       *rte = *(currRoute);
 152       currRoute++;
 153     }
 154   else
 155     {
 156       // If currNode points to the first node, this means the aircraft is not on the taxi node
 157       // yet. Make sure to return a unique identifyer in this situation though, because otherwise
 158       // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
 159       // taxi route or not. the negative of the preceding route seems a logical choice, as it is
 160       // unique for any starting location.
 161       // Note that this is probably just a temporary fix until I get Parking / tower control working.
 162       *rte = -1 * *(currRoute);
 163     }
 164   currNode++;
 165   return true;
 166 };
 167
 168 void FGTaxiRoute::rewind(int route)
 169 {
 170   int currPoint;
 171   int currRoute;
 172   first();
 173   do {
 174     if (!(next(&currPoint, &currRoute))) {
 175       SG_LOG(SG_GENERAL,SG_ALERT, "Error in rewinding TaxiRoute: current" << currRoute
 176              << " goal " << route);
 177     }
 178   } while (currRoute != route);
 179 }
 180
 181 /***************************************************************************
 182  * FGTrafficRecord
 183  **************************************************************************/
 184 void FGTrafficRecord::setPositionAndIntentions(int pos, FGAIFlightPlan *route)
 185 {
 186
 187   currentPos = pos;
 188   if (intentions.size()) {
 189     if (*intentions.begin() != pos) {
 190       SG_LOG(SG_GENERAL, SG_ALERT, "Error in FGTrafficRecord::setPositionAndIntentions");
 191       cerr << "Pos : " << pos << " Curr " << *(intentions.begin())  << endl;
 192       for (intVecIterator i = intentions.begin(); i != intentions.end() ; i++) {
 193         cerr << (*i) << " ";
 194       }
 195       cerr << endl;
 196     }
 197     intentions.erase(intentions.begin());
 198   } else {
 199     //int legNr, routeNr;
 200     //FGAIFlightPlan::waypoint* const wpt= route->getCurrentWaypoint();
 201     int size = route->getNrOfWayPoints();
 202     cerr << "Setting pos" << pos << " ";
 203     cerr << "setting intentions ";
 204     for (int i = 0; i < size; i++) {
 205       int val = route->getRouteIndex(i);
 206
 207       if ((val) && (val != pos))
 208         {
 209           intentions.push_back(val);
 210           cerr << val<< " ";
 211         }
 212     }
 213     cerr << endl;
 214     //while (route->next(&legNr, &routeNr)) {
 215     //intentions.push_back(routeNr);
 216     //}
 217     //route->rewind(currentPos);
 218   }
 219   //exit(1);
 220 }
 221
 222 bool FGTrafficRecord::checkPositionAndIntentions(FGTrafficRecord &other)
 223 {
 224   bool result = false;
 225   //cerr << "Start check 1" << endl;
 226   if (currentPos == other.currentPos)
 227     {
 228       //cerr << "Check Position and intentions: current matches" << endl;
 229       result = true;
 230     }
 231  //  else if (other.intentions.size())
 232 //     {
 233 //       cerr << "Start check 2" << endl;
 234 //       intVecIterator i = other.intentions.begin();
 235 //       while (!((i == other.intentions.end()) || ((*i) == currentPos)))
 236 //      i++;
 237 //       if (i != other.intentions.end()) {
 238 //      cerr << "Check Position and intentions: current matches other.intentions" << endl;
 239 //      result = true;
 240 //       }
 241   else if (intentions.size()) {
 242     //cerr << "Start check 3" << endl;
 243     intVecIterator i = intentions.begin();
 244     while (!((i == intentions.end()) || ((*i) == other.currentPos)))
 245       i++;
 246     if (i != intentions.end()) {
 247       //cerr << "Check Position and intentions: .other.current matches" << endl;
 248       result = true;
 249     }
 250   }
 251   //cerr << "Done !!" << endl;
 252   return result;
 253 }
 254
 255 void FGTrafficRecord::setPositionAndHeading(double lat, double lon, double hdg,
 256                                             double spd, double alt)
 257 {
 258   latitude = lat;
 259   longitude = lon;
 260   heading = hdg;
 261   speed = spd;
 262   altitude = alt;
 263 }
 264
 265 /***************************************************************************
 266  * FGGroundNetwork()
 267  **************************************************************************/
 268
 269 FGGroundNetwork::FGGroundNetwork()
 270 {
 271   hasNetwork = false;
 272   foundRoute = false;
 273   totalDistance = 0;
 274   maxDistance = 0;
 275   currTraffic = activeTraffic.begin();
 276 }
 277
 278 void FGGroundNetwork::addSegment(const FGTaxiSegment &seg)
 279 {
 280   segments.push_back(seg);
 281 }
 282
 283 void FGGroundNetwork::addNode(const FGTaxiNode &node)
 284 {
 285   nodes.push_back(node);
 286 }
 287
 288 void FGGroundNetwork::addNodes(FGParkingVec *parkings)
 289 {
 290   FGTaxiNode n;
 291   FGParkingVecIterator i = parkings->begin();
 292   while (i != parkings->end())
 293     {
 294       n.setIndex(i->getIndex());
 295       n.setLatitude(i->getLatitude());
 296       n.setLongitude(i->getLongitude());
 297       nodes.push_back(n);
 298
 299       i++;
 300     }
 301 }
 302
 303
 304
 305 void FGGroundNetwork::init()
 306 {
 307   hasNetwork = true;
 308   int index = 1;
 309   FGTaxiSegmentVectorIterator i = segments.begin();
 310   while(i != segments.end()) {
 311     //cerr << "initializing node " << i->getIndex() << endl;
 312     i->setStart(&nodes);
 313     i->setEnd  (&nodes);
 314     i->setTrackDistance();
 315     i->setIndex(index++);
 316     //cerr << "Track distance = " << i->getLength() << endl;
 317     //cerr << "Track ends at"      << i->getEnd()->getIndex() << endl;
 318     i++;
 319   }
 320   //exit(1);
 321 }
 322
 323 int FGGroundNetwork::findNearestNode(double lat, double lon)
 324 {
 325   double minDist = HUGE_VAL;
 326   double course, dist;
 327   int index;
 328   SGWayPoint first  (lon,
 329                      lat,
 330                      0);
 331
 332   for (FGTaxiNodeVectorIterator
 333          itr = nodes.begin();
 334        itr != nodes.end(); itr++)
 335     {
 336       double course;
 337       SGWayPoint second (itr->getLongitude(),
 338                          itr->getLatitude(),
 339                          0);
 340       first.CourseAndDistance(second, &course, &dist);
 341       if (dist < minDist)
 342         {
 343           minDist = dist;
 344           index = itr->getIndex();
 345           //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
 346         }
 347     }
 348   return index;
 349 }
 350
 351 FGTaxiNode *FGGroundNetwork::findNode(int idx)
 352 {
 353   for (FGTaxiNodeVectorIterator
 354          itr = nodes.begin();
 355        itr != nodes.end(); itr++)
 356     {
 357       if (itr->getIndex() == idx)
 358         return itr->getAddress();
 359     }
 360   return 0;
 361 }
 362
 363 FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
 364 {
 365   for (FGTaxiSegmentVectorIterator
 366          itr = segments.begin();
 367        itr != segments.end(); itr++)
 368     {
 369       if (itr->getIndex() == idx)
 370         return itr->getAddress();
 371     }
 372   return 0;
 373 }
 374
 375 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
 376 {
 377   foundRoute = false;
 378   totalDistance = 0;
 379   FGTaxiNode *firstNode = findNode(start);
 380   FGTaxiNode *lastNode  = findNode(end);
 381   //prevNode = prevPrevNode = -1;
 382   //prevNode = start;
 383   routes.clear();
 384   nodesStack.clear();
 385   routesStack.clear();
 386
 387   trace(firstNode, end, 0, 0);
 388   FGTaxiRoute empty;
 389
 390   if (!foundRoute)
 391     {
 392       SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
 393       exit(1);
 394     }
 395   sort(routes.begin(), routes.end());
 396   //for (intVecIterator i = route.begin(); i != route.end(); i++)
 397   //  {
 398   //    rte->push_back(*i);
 399   //  }
 400
 401   if (routes.begin() != routes.end())
 402     return *(routes.begin());
 403   else
 404     return empty;
 405 }
 406
 407
 408 void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance)
 409 {
 410   // Just check some preconditions of the trace algorithm
 411   if (nodesStack.size() != routesStack.size())
 412     {
 413       SG_LOG(SG_GENERAL, SG_ALERT, "size of nodesStack and routesStack is not equal. NodesStack :"
 414              << nodesStack.size() << ". RoutesStack : " << routesStack.size());
 415     }
 416   nodesStack.push_back(currNode->getIndex());
 417   totalDistance += distance;
 418   //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl;
 419   //<< currNode->getIndex() << endl;
 420
 421   // If the current route matches the required end point we found a valid route
 422   // So we can add this to the routing table
 423   if (currNode->getIndex() == end)
 424     {
 425       //cerr << "Found route : " <<  totalDistance << "" << " " << *(nodesStack.end()-1) << endl;
 426       routes.push_back(FGTaxiRoute(nodesStack,routesStack,totalDistance));
 427       if (nodesStack.empty() || routesStack.empty())
 428         {
 429           printRoutingError(string("while finishing route"));
 430         }
 431       nodesStack.pop_back();
 432       routesStack.pop_back();
 433       if (!(foundRoute))
 434         maxDistance = totalDistance;
 435       else
 436         if (totalDistance < maxDistance)
 437           maxDistance = totalDistance;
 438       foundRoute = true;
 439       totalDistance -= distance;
 440       return;
 441     }
 442
 443
 444   // search if the currentNode has been encountered before
 445   // if so, we should step back one level, because it is
 446   // rather rediculous to proceed further from here.
 447   // if the current node has not been encountered before,
 448   // i should point to nodesStack.end()-1; and we can continue
 449   // if i is not nodesStack.end, the previous node was found,
 450   // and we should return.
 451   // This only works at trace levels of 1 or higher though
 452   if (depth > 0) {
 453     intVecIterator i = nodesStack.begin();
 454     while ((*i) != currNode->getIndex()) {
 455       //cerr << "Route so far : " << (*i) << endl;
 456       i++;
 457     }
 458     if (i != nodesStack.end()-1) {
 459       if (nodesStack.empty() || routesStack.empty())
 460         {
 461           printRoutingError(string("while returning from an already encountered node"));
 462         }
 463       nodesStack.pop_back();
 464       routesStack.pop_back();
 465       totalDistance -= distance;
 466       return;
 467     }
 468     // If the total distance from start to the current waypoint
 469     // is longer than that of a route we can also stop this trace
 470     // and go back one level.
 471     if ((totalDistance > maxDistance) && foundRoute)
 472       {
 473         //cerr << "Stopping rediculously long trace: " << totalDistance << endl;
 474         if (nodesStack.empty() || routesStack.empty())
 475         {
 476           printRoutingError(string("while returning from finding a rediculously long route"));
 477         }
 478         nodesStack.pop_back();
 479         routesStack.pop_back();
 480         totalDistance -= distance;
 481         return;
 482       }
 483   }
 484
 485   //cerr << "2" << endl;
 486   if (currNode->getBeginRoute() != currNode->getEndRoute())
 487     {
 488       //cerr << "3" << endl;
 489       for (FGTaxiSegmentPointerVectorIterator
 490              i = currNode->getBeginRoute();
 491            i != currNode->getEndRoute();
 492            i++)
 493         {
 494           //cerr << (*i)->getLength() << endl;
 495           //cerr << (*i)->getIndex() << endl;
 496           int idx = (*i)->getIndex();
 497           routesStack.push_back((*i)->getIndex());
 498           trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
 499         //  {
 500         //      // cerr << currNode -> getIndex() << " ";
 501         //      route.push_back(currNode->getIndex());
 502         //      return true;
 503         //    }
 504         }
 505     }
 506   else
 507     {
 508       //SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
 509     }
 510   if (nodesStack.empty())
 511     {
 512       printRoutingError(string("while finishing trace"));
 513     }
 514   nodesStack.pop_back();
 515   // Make sure not to dump the level-zero routesStack entry, because that was never created.
 516   if (depth)
 517     {
 518       routesStack.pop_back();
 519       //cerr << "leaving trace " << routesStack.size() << endl;
 520     }
 521   totalDistance -= distance;
 522   return;
 523 }
 524
 525 void FGGroundNetwork::printRoutingError(string mess)
 526 {
 527   SG_LOG(SG_GENERAL, SG_ALERT,  "Error in ground network trace algorithm " << mess);
 528   if (nodesStack.empty())
 529     {
 530       SG_LOG(SG_GENERAL, SG_ALERT, " nodesStack is empty. Dumping routesStack");
 531       for (intVecIterator i = routesStack.begin() ; i != routesStack.end(); i++)
 532         SG_LOG(SG_GENERAL, SG_ALERT, "Route " << (*i));
 533     }
 534   if (routesStack.empty())
 535     {
 536       SG_LOG(SG_GENERAL, SG_ALERT, " routesStack is empty. Dumping nodesStack");
 537       for (intVecIterator i = nodesStack.begin() ; i != nodesStack.end(); i++)
 538         SG_LOG(SG_GENERAL, SG_ALERT, "Node " << (*i));
 539     }
 540   //exit(1);
 541 }
 542
 543
 544 void FGGroundNetwork::announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentPosition,
 545                                        double lat, double lon, double heading,
 546                                        double speed, double alt, double radius)
 547 {
 548   TrafficVectorIterator i = activeTraffic.begin();
 549   // Search search if the current id alread has an entry
 550   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 551   if (activeTraffic.size()) {
 552     while ((i->getId() != id) && i != activeTraffic.end()) {
 553       i++;
 554     }
 555   }
 556   // Add a new TrafficRecord if no one exsists for this aircraft.
 557   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 558     FGTrafficRecord rec;
 559     rec.setId(id);
 560     rec.setPositionAndIntentions(currentPosition, intendedRoute);
 561     rec.setPositionAndHeading(lat, lon, heading, speed, alt);
 562     rec.setRadius(radius); // only need to do this when creating the record.
 563     activeTraffic.push_back(rec);
 564   } else {
 565     i->setPositionAndIntentions(currentPosition, intendedRoute);
 566     i->setPositionAndHeading(lat, lon, heading, speed, alt);
 567   }
 568 }
 569
 570 void FGGroundNetwork::signOff(int id) {
 571  TrafficVectorIterator i = activeTraffic.begin();
 572   // Search search if the current id alread has an entry
 573   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 574   if (activeTraffic.size()) {
 575     while ((i->getId() != id) && i != activeTraffic.end()) {
 576       i++;
 577     }
 578   }
 579   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 580     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Aircraft without traffic record is signing off");
 581   } else {
 582       i = activeTraffic.erase(i);
 583   }
 584 }
 585
 586 void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt) {
 587   TrafficVectorIterator i = activeTraffic.begin();
 588   // Search search if the current id has an entry
 589   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 590   TrafficVectorIterator current, closest;
 591   if (activeTraffic.size()) {
 592     while ((i->getId() != id) && i != activeTraffic.end()) {
 593       i++;
 594     }
 595   }
 596   // update position of the current aircraft
 597   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 598     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: updating aircraft without traffic record");
 599   } else {
 600     i->setPositionAndHeading(lat, lon, heading, speed, alt);
 601     current = i;
 602   }
 603
 604   // Scan for a speed adjustment change. Find the nearest aircraft that is in front
 605   // and adjust speed when we get too close. Only do this when current position and/or
 606   // intentions of the current aircraft match current taxiroute position of the proximate
 607   // aircraft.
 608   double mindist = HUGE;
 609   if (activeTraffic.size())
 610     {
 611       double course, dist, bearing, minbearing;
 612
 613       //TrafficVector iterator closest;
 614       for (TrafficVectorIterator i = activeTraffic.begin();
 615            i != activeTraffic.end(); i++)
 616         {
 617           if (i != current) {
 618             SGWayPoint curr  (lon,
 619                               lat,
 620                               alt);
 621             SGWayPoint other    (i->getLongitude  (),
 622                                  i->getLatitude (),
 623                                  i->getAltitude  ());
 624             other.CourseAndDistance(curr, &course, &dist);
 625              bearing = fabs(heading-course);
 626           if (bearing > 180)
 627             bearing = 360-bearing;
 628           if ((dist < mindist) && (bearing < 60.0))
 629             {
 630               mindist = dist;
 631               closest = i;
 632               minbearing = bearing;
 633             }
 634           }
 635         }
 636           //cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading
 637           //   << " course : " << course << endl;
 638       current->clearSpeedAdjustment();
 639       // Only clear the heading adjustment at positive speeds, otherwise the waypoint following
 640       // code wreaks havoc
 641       if (speed > 0.2)
 642         current->clearHeadingAdjustment();
 643       // All clear
 644       if (mindist > 100)
 645         {
 646           //current->clearSpeedAdjustment();
 647           //current->clearHeadingAdjustment();
 648         }
 649       else
 650         {
 651           if (current->getId() == closest->getWaitsForId())
 652             return;
 653           else
 654             current->setWaitsForId(closest->getId());
 655
 656           // Getting close: Slow down to a bit less than the other aircraft
 657           double maxAllowableDistance = (1.1*current->getRadius()) + (1.1*closest->getRadius());
 658             if (mindist > maxAllowableDistance)
 659             {
 660               if (current->checkPositionAndIntentions(*closest))
 661                 {
 662                   // Adjust speed, but don't let it drop to below 1 knots
 663                   //if (fabs(speed) > 1)
 664                   if (!(current->hasHeadingAdjustment()))
 665                     {
 666                       current->setSpeedAdjustment(closest->getSpeed()* (mindist/100));
 667                       //cerr << "Adjusting speed to " << closest->getSpeed() * (mindist / 100) << " "
 668                       //         << "Bearing = " << minbearing << " Distance = " << mindist
 669                       //         << " Latitude = " <<lat << " longitude = " << lon << endl;
 670                       //<< " Latitude = " <<closest->getLatitude()
 671                       //<< " longitude = " << closest->getLongitude()
 672                       //  << endl;
 673                     }
 674                   else
 675                     {
 676                        double newSpeed = (maxAllowableDistance-mindist);
 677                        current->setSpeedAdjustment(newSpeed);
 678                     }
 679                 }
 680             }
 681           else
 682             {
 683               if (!(current->hasHeadingAdjustment()))
 684                 {
 685                   double newSpeed;
 686                   if (mindist > 10) {
 687                     newSpeed = 0.01;
 688                       current->setSpeedAdjustment(newSpeed);
 689                   } else {
 690                     newSpeed = -1 * (maxAllowableDistance-mindist);
 691                     current->setSpeedAdjustment(newSpeed);
 692                     current->setHeadingAdjustment(heading);
 693                     //        if (mindist < 5) {
 694                     //          double bank_sense = 0;
 695                     //          current->setSpeedAdjustment(-0.1);
 696                     //          // Do a heading adjustment
 697                     //          double diff = fabs(heading - bearing);
 698                     //          if (diff > 180)
 699                     //            diff = fabs(diff - 360);
 700
 701                     //          double sum = heading + diff;
 702                     //          if (sum > 360.0)
 703                     //            sum -= 360.0;
 704                     //          if (fabs(sum - bearing) < 1.0) {
 705                     //            bank_sense = -1.0;   // turn left for evasive action
 706                     //          } else {
 707                     //            bank_sense = 1.0;  // turn right for evasive action
 708                     //          }
 709                     //          double newHeading = heading + bank_sense;
 710                     //          if (newHeading < 0) newHeading   += 360;
 711                     //          if (newHeading > 360) newHeading -= 360;
 712                     //          current->setHeadingAdjustment(newHeading);
 713                     //          //cerr << "Yikes: TOOOO close. backing up and turning to heading " << newHeading
 714                     //          //  << endl;
 715                     //          cerr << "Troubleshooting: " << current->getId() << " Closest : " << closest->getId()
 716                     //               << endl;
 717                     //       }
 718                   }
 719                 }
 720             }
 721         }
 722     }
 723 }
 724
 725 bool FGGroundNetwork::hasInstruction(int id)
 726 {
 727    TrafficVectorIterator i = activeTraffic.begin();
 728   // Search search if the current id has an entry
 729   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 730   if (activeTraffic.size()) {
 731     while ((i->getId() != id) && i != activeTraffic.end()) {
 732       i++;
 733     }
 734   }
 735   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 736     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: checking ATC instruction for aircraft without traffic record");
 737   } else {
 738     return i->hasInstruction();
 739   }
 740 }
 741
 742 FGATCInstruction FGGroundNetwork::getInstruction(int id)
 743 {
 744   TrafficVectorIterator i = activeTraffic.begin();
 745   // Search search if the current id has an entry
 746   // This might be faster using a map instead of a vector, but let's start by taking a safe route
 747   if (activeTraffic.size()) {
 748     while ((i->getId() != id) && i != activeTraffic.end()) {
 749       i++;
 750     }
 751   }
 752   if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 753     SG_LOG(SG_GENERAL, SG_ALERT, "AI error: requesting ATC instruction for aircraft without traffic record");
 754   } else {
 755     return i->getInstruction();
 756   }
 757 }
 758
 759
 760
 761
 762 /***************************************************************************
 763  * FGATCInstruction
 764  *
 765  * This class is really out of place here, and should be combined with
 766  * FGATC controller and go into it's own file / directory
 767  * I'm leaving it for now though, because I'm testing this stuff quite
 768  * heavily.
 769  **************************************************************************/
 770 FGATCInstruction::FGATCInstruction()
 771 {
 772   holdPattern    = false;
 773   holdPosition   = false;
 774   changeSpeed    = false;
 775   changeHeading  = false;
 776   changeAltitude = false;
 777
 778   double speed   = 0;
 779   double heading = 0;
 780   double alt     = 0;
 781 }
 782
 783 bool FGATCInstruction::hasInstruction()
 784 {
 785   return (holdPattern || holdPosition || changeSpeed || changeHeading || changeAltitude);
 786 }
 787
 788