src/Airports/groundnetwork.cxx

   1
   2 // groundnet.cxx - Implimentation of the FlightGear airport ground handling code
   3 //
   4 // Written by Durk Talsma, started June 2005.
   5 //
   6 // Copyright (C) 2004 Durk Talsma.
   7 //
   8 // This program is free software; you can redistribute it and/or
   9 // modify it under the terms of the GNU General Public License as
  10 // published by the Free Software Foundation; either version 2 of the
  11 // License, or (at your option) any later version.
  12 //
  13 // This program is distributed in the hope that it will be useful, but
  14 // WITHOUT ANY WARRANTY; without even the implied warranty of
  15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 // General Public License for more details.
  17 //
  18 // You should have received a copy of the GNU General Public License
  19 // along with this program; if not, write to the Free Software
  20 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  21 //
  22 // $Id$
  23
  24 #ifdef HAVE_CONFIG_H
  25 #  include <config.h>
  26 #endif
  27
  28 #include <math.h>
  29 #include <algorithm>
  30
  31
  32 #include <osg/Geode>
  33 #include <osg/Geometry>
  34 #include <osg/MatrixTransform>
  35 #include <osg/Shape>
  36
  37 #include <simgear/debug/logstream.hxx>
  38 #include <simgear/route/waypoint.hxx>
  39 #include <simgear/scene/material/EffectGeode.hxx>
  40 #include <simgear/scene/material/matlib.hxx>
  41 #include <simgear/scene/material/mat.hxx>
  42
  43 #include <Airports/simple.hxx>
  44 #include <Airports/dynamics.hxx>
  45
  46 #include <AIModel/AIAircraft.hxx>
  47 #include <AIModel/AIFlightPlan.hxx>
  48
  49 #include <ATC/atc_mgr.hxx>
  50
  51 #include <Scenery/scenery.hxx>
  52
  53 #include "groundnetwork.hxx"
  54
  55 /***************************************************************************
  56  * FGTaxiSegment
  57  **************************************************************************/
  58
  59 void FGTaxiSegment::setStart(FGTaxiNodeVector * nodes)
  60 {
  61     FGTaxiNodeVectorIterator i = nodes->begin();
  62     while (i != nodes->end()) {
  63         //cerr << "Scanning start node index" << (*i)->getIndex() << endl;
  64         if ((*i)->getIndex() == startNode) {
  65             start = (*i)->getAddress();
  66             (*i)->addSegment(this);
  67             return;
  68         }
  69         i++;
  70     }
  71     SG_LOG(SG_GENERAL, SG_ALERT,
  72            "Could not find start node " << startNode << endl);
  73 }
  74
  75 void FGTaxiSegment::setEnd(FGTaxiNodeVector * nodes)
  76 {
  77     FGTaxiNodeVectorIterator i = nodes->begin();
  78     while (i != nodes->end()) {
  79         //cerr << "Scanning end node index" << (*i)->getIndex() << endl;
  80         if ((*i)->getIndex() == endNode) {
  81             end = (*i)->getAddress();
  82             return;
  83         }
  84         i++;
  85     }
  86     SG_LOG(SG_GENERAL, SG_ALERT,
  87            "Could not find end node " << endNode << endl);
  88 }
  89
  90
  91
  92 // There is probably a computationally cheaper way of
  93 // doing this.
  94 void FGTaxiSegment::setDimensions(double elevation)
  95 {
  96     length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
  97     //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
  98
  99     double az2; //, distanceM;
 100     SGGeodesy::inverse(start->getGeod(), end->getGeod(), heading, az2, length);
 101     double coveredDistance = length * 0.5;
 102     SGGeodesy::direct(start->getGeod(), heading, coveredDistance, center, az2);
 103     //cerr << "Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
 104 }
 105
 106
 107 //void FGTaxiSegment::setCourseDiff(double crse)
 108 //{
 109 //    headingDiff = fabs(course - crse);
 110
 111 //    if (headingDiff > 180)
 112 //        headingDiff = fabs(headingDiff - 360);
 113 //}
 114
 115
 116 /***************************************************************************
 117  * FGTaxiRoute
 118  **************************************************************************/
 119 bool FGTaxiRoute::next(int *nde)
 120 {
 121     //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 122     //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 123     //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 124     //if (currNode != nodes.end())
 125     //  cerr << "true" << endl;
 126     //else
 127     //  cerr << "false" << endl;
 128     //if (nodes.size() != (routes.size()) +1)
 129     //  cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
 130
 131     if (currNode == nodes.end())
 132         return false;
 133     *nde = *(currNode);
 134     if (currNode != nodes.begin())      // make sure route corresponds to the end node
 135         currRoute++;
 136     currNode++;
 137     return true;
 138 };
 139
 140 bool FGTaxiRoute::next(int *nde, int *rte)
 141 {
 142     //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 143     //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 144     //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 145     //if (currNode != nodes.end())
 146     //  cerr << "true" << endl;
 147     //else
 148     //  cerr << "false" << endl;
 149     if (nodes.size() != (routes.size()) + 1) {
 150         SG_LOG(SG_GENERAL, SG_ALERT,
 151                "ALERT: Misconfigured TaxiRoute : " << nodes.
 152                size() << " " << routes.size());
 153         exit(1);
 154     }
 155     if (currNode == nodes.end())
 156         return false;
 157     *nde = *(currNode);
 158     //*rte = *(currRoute);
 159     if (currNode != nodes.begin())      // Make sure route corresponds to the end node
 160     {
 161         *rte = *(currRoute);
 162         currRoute++;
 163     } else {
 164         // If currNode points to the first node, this means the aircraft is not on the taxi node
 165         // yet. Make sure to return a unique identifyer in this situation though, because otherwise
 166         // the speed adjust AI code may be unable to resolve whether two aircraft are on the same
 167         // taxi route or not. the negative of the preceding route seems a logical choice, as it is
 168         // unique for any starting location.
 169         // Note that this is probably just a temporary fix until I get Parking / tower control working.
 170         *rte = -1 * *(currRoute);
 171     }
 172     currNode++;
 173     return true;
 174 };
 175
 176 void FGTaxiRoute::rewind(int route)
 177 {
 178     int currPoint;
 179     int currRoute;
 180     first();
 181     do {
 182         if (!(next(&currPoint, &currRoute))) {
 183             SG_LOG(SG_GENERAL, SG_ALERT,
 184                    "Error in rewinding TaxiRoute: current" << currRoute <<
 185                    " goal " << route);
 186         }
 187     } while (currRoute != route);
 188 }
 189
 190
 191
 192
 193 /***************************************************************************
 194  * FGGroundNetwork()
 195  **************************************************************************/
 196 bool compare_nodes(FGTaxiNode * a, FGTaxiNode * b)
 197 {
 198     return (*a) < (*b);
 199 }
 200
 201 bool compare_segments(FGTaxiSegment * a, FGTaxiSegment * b)
 202 {
 203     return (*a) < (*b);
 204 }
 205
 206 FGGroundNetwork::FGGroundNetwork()
 207 {
 208     hasNetwork = false;
 209     foundRoute = false;
 210     totalDistance = 0;
 211     maxDistance = 0;
 212     //maxDepth    = 1000;
 213     count = 0;
 214     currTraffic = activeTraffic.begin();
 215     group = 0;
 216
 217 }
 218
 219 FGGroundNetwork::~FGGroundNetwork()
 220 {
 221     for (FGTaxiNodeVectorIterator node = nodes.begin();
 222          node != nodes.end(); node++) {
 223         delete(*node);
 224     }
 225     nodes.clear();
 226     pushBackNodes.clear();
 227     for (FGTaxiSegmentVectorIterator seg = segments.begin();
 228          seg != segments.end(); seg++) {
 229         delete(*seg);
 230     }
 231     segments.clear();
 232 }
 233
 234 void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
 235 {
 236     segments.push_back(new FGTaxiSegment(seg));
 237 }
 238
 239 void FGGroundNetwork::addNode(const FGTaxiNode & node)
 240 {
 241     nodes.push_back(new FGTaxiNode(node));
 242 }
 243
 244 void FGGroundNetwork::addNodes(FGParkingVec * parkings)
 245 {
 246     FGTaxiNode n;
 247     FGParkingVecIterator i = parkings->begin();
 248     while (i != parkings->end()) {
 249         n.setIndex(i->getIndex());
 250         n.setLatitude(i->getLatitude());
 251         n.setLongitude(i->getLongitude());
 252         n.setElevation(parent->getElevation());
 253         nodes.push_back(new FGTaxiNode(n));
 254
 255         i++;
 256     }
 257 }
 258
 259
 260
 261 void FGGroundNetwork::init()
 262 {
 263     hasNetwork = true;
 264     int index = 1;
 265     sort(nodes.begin(), nodes.end(), compare_nodes);
 266     //sort(segments.begin(), segments.end(), compare_segments());
 267     FGTaxiSegmentVectorIterator i = segments.begin();
 268     while (i != segments.end()) {
 269         (*i)->setStart(&nodes);
 270         (*i)->setEnd(&nodes);
 271         (*i)->setDimensions(parent->getElevation());
 272         (*i)->setIndex(index);
 273         if ((*i)->isPushBack()) {
 274             pushBackNodes.push_back((*i)->getEnd());
 275         }
 276         //SG_LOG(SG_GENERAL, SG_BULK,  "initializing segment " << (*i)->getIndex() << endl);
 277         //SG_LOG(SG_GENERAL, SG_BULK, "Track distance = "     << (*i)->getLength() << endl);
 278         //SG_LOG(SG_GENERAL, SG_BULK, "Track runs from "      << (*i)->getStart()->getIndex() << " to "
 279         //                                                    << (*i)->getEnd()->getIndex() << endl);
 280         i++;
 281         index++;
 282     }
 283
 284     i = segments.begin();
 285     while (i != segments.end()) {
 286         FGTaxiSegmentVectorIterator j = (*i)->getEnd()->getBeginRoute();
 287         while (j != (*i)->getEnd()->getEndRoute()) {
 288             if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex()) {
 289 //          int start1 = (*i)->getStart()->getIndex();
 290 //          int end1   = (*i)->getEnd()  ->getIndex();
 291 //          int start2 = (*j)->getStart()->getIndex();
 292 //          int end2   = (*j)->getEnd()->getIndex();
 293 //          int oppIndex = (*j)->getIndex();
 294                 //cerr << "Opposite of  " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
 295                 //   << "happens to be " << oppIndex      << " (" << start2 << "," << end2 << ") " << endl;
 296                 (*i)->setOpposite(*j);
 297                 break;
 298             }
 299             j++;
 300         }
 301         i++;
 302     }
 303     //FGTaxiNodeVectorIterator j = nodes.begin();
 304     //while (j != nodes.end()) {
 305     //    if ((*j)->getHoldPointType() == 3) {
 306     //        pushBackNodes.push_back((*j));
 307     //    }
 308     //    j++;
 309     //}
 310     //cerr << "Done initializing ground network" << endl;
 311     //exit(1);
 312 }
 313
 314 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
 315 {
 316     double minDist = HUGE_VAL;
 317     int index = -1;
 318
 319     for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end();
 320          itr++) {
 321         double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
 322         if (d < minDist) {
 323             minDist = d;
 324             index = (*itr)->getIndex();
 325             //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
 326         }
 327     }
 328
 329     return index;
 330 }
 331
 332 int FGGroundNetwork::findNearestNode(double lat, double lon)
 333 {
 334     return findNearestNode(SGGeod::fromDeg(lon, lat));
 335 }
 336
 337 FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
 338 {                               /*
 339                                    for (FGTaxiNodeVectorIterator
 340                                    itr = nodes.begin();
 341                                    itr != nodes.end(); itr++)
 342                                    {
 343                                    if (itr->getIndex() == idx)
 344                                    return itr->getAddress();
 345                                    } */
 346
 347     if ((idx >= 0) && (idx < nodes.size()))
 348         return nodes[idx]->getAddress();
 349     else
 350         return 0;
 351 }
 352
 353 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
 354 {                               /*
 355                                    for (FGTaxiSegmentVectorIterator
 356                                    itr = segments.begin();
 357                                    itr != segments.end(); itr++)
 358                                    {
 359                                    if (itr->getIndex() == idx)
 360                                    return itr->getAddress();
 361                                    }
 362                                  */
 363     if ((idx > 0) && (idx <= segments.size()))
 364         return segments[idx - 1]->getAddress();
 365     else {
 366         //cerr << "Alert: trying to find invalid segment " << idx << endl;
 367         return 0;
 368     }
 369 }
 370
 371
 372 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
 373                                                bool fullSearch)
 374 {
 375 //implements Dijkstra's algorithm to find shortest distance route from start to end
 376 //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
 377
 378     //double INFINITE = 100000000000.0;
 379     // initialize scoring values
 380     int nParkings = parent->getDynamics()->getNrOfParkings();
 381     FGTaxiNodeVector *currNodesSet;
 382     if (fullSearch) {
 383         currNodesSet = &nodes;
 384     } else {
 385         currNodesSet = &pushBackNodes;
 386     }
 387
 388     for (FGTaxiNodeVectorIterator
 389          itr = currNodesSet->begin(); itr != currNodesSet->end(); itr++) {
 390         (*itr)->setPathScore(HUGE_VAL); //infinity by all practical means
 391         (*itr)->setPreviousNode(0);     //
 392         (*itr)->setPreviousSeg(0);      //
 393     }
 394
 395     FGTaxiNode *firstNode = findNode(start);
 396     firstNode->setPathScore(0);
 397
 398     FGTaxiNode *lastNode = findNode(end);
 399
 400     FGTaxiNodeVector unvisited(*currNodesSet);  // working copy
 401
 402     while (!unvisited.empty()) {
 403         FGTaxiNode *best = *(unvisited.begin());
 404         for (FGTaxiNodeVectorIterator
 405              itr = unvisited.begin(); itr != unvisited.end(); itr++) {
 406             if ((*itr)->getPathScore() < best->getPathScore())
 407                 best = (*itr);
 408         }
 409
 410         FGTaxiNodeVectorIterator newend =
 411             remove(unvisited.begin(), unvisited.end(), best);
 412         unvisited.erase(newend, unvisited.end());
 413
 414         if (best == lastNode) { // found route or best not connected
 415             break;
 416         } else {
 417             for (FGTaxiSegmentVectorIterator
 418                  seg = best->getBeginRoute();
 419                  seg != best->getEndRoute(); seg++) {
 420                 if (fullSearch || (*seg)->isPushBack()) {
 421                     FGTaxiNode *tgt = (*seg)->getEnd();
 422                     double alt =
 423                         best->getPathScore() + (*seg)->getLength() +
 424                         (*seg)->getPenalty(nParkings);
 425                     if (alt < tgt->getPathScore()) {    // Relax (u,v)
 426                         tgt->setPathScore(alt);
 427                         tgt->setPreviousNode(best);
 428                         tgt->setPreviousSeg(*seg);      //
 429                     }
 430                 } else {
 431                     //   // cerr << "Skipping TaxiSegment " << (*seg)->getIndex() << endl;
 432                 }
 433             }
 434         }
 435     }
 436
 437     if (lastNode->getPathScore() == HUGE_VAL) {
 438         // no valid route found
 439         if (fullSearch) {
 440             SG_LOG(SG_GENERAL, SG_ALERT,
 441                    "Failed to find route from waypoint " << start << " to "
 442                    << end << " at " << parent->getId());
 443         }
 444         FGTaxiRoute empty;
 445         return empty;
 446         //exit(1); //TODO exit more gracefully, no need to stall the whole sim with broken GN's
 447     } else {
 448         // assemble route from backtrace information
 449         intVec nodes, routes;
 450         FGTaxiNode *bt = lastNode;
 451         while (bt->getPreviousNode() != 0) {
 452             nodes.push_back(bt->getIndex());
 453             routes.push_back(bt->getPreviousSegment()->getIndex());
 454             bt = bt->getPreviousNode();
 455         }
 456         nodes.push_back(start);
 457         reverse(nodes.begin(), nodes.end());
 458         reverse(routes.begin(), routes.end());
 459
 460         return FGTaxiRoute(nodes, routes, lastNode->getPathScore(), 0);
 461     }
 462 }
 463
 464 int FGTaxiSegment::getPenalty(int nGates)
 465 {
 466     int penalty = 0;
 467     if (end->getIndex() < nGates) {
 468         penalty += 10000;
 469     }
 470     if (end->getIsOnRunway()) { // For now. In future versions, need to find out whether runway is active.
 471         penalty += 1000;
 472     }
 473     return penalty;
 474 }
 475
 476 /* ATC Related Functions */
 477
 478 void FGGroundNetwork::announcePosition(int id,
 479                                        FGAIFlightPlan * intendedRoute,
 480                                        int currentPosition, double lat,
 481                                        double lon, double heading,
 482                                        double speed, double alt,
 483                                        double radius, int leg,
 484                                        FGAIAircraft * aircraft)
 485 {
 486     init();
 487     TrafficVectorIterator i = activeTraffic.begin();
 488     // Search search if the current id alread has an entry
 489     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 490     if (activeTraffic.size()) {
 491         //while ((i->getId() != id) && i != activeTraffic.end()) {
 492         while (i != activeTraffic.end()) {
 493             if (i->getId() == id) {
 494                 break;
 495             }
 496             i++;
 497         }
 498     }
 499     // Add a new TrafficRecord if no one exsists for this aircraft.
 500     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 501         FGTrafficRecord rec;
 502         rec.setId(id);
 503         rec.setLeg(leg);
 504         rec.setPositionAndIntentions(currentPosition, intendedRoute);
 505         rec.setPositionAndHeading(lat, lon, heading, speed, alt);
 506         rec.setRadius(radius);  // only need to do this when creating the record.
 507         rec.setAircraft(aircraft);
 508         activeTraffic.push_back(rec);
 509     } else {
 510         i->setPositionAndIntentions(currentPosition, intendedRoute);
 511         i->setPositionAndHeading(lat, lon, heading, speed, alt);
 512     }
 513 }
 514
 515
 516 void FGGroundNetwork::signOff(int id)
 517 {
 518     TrafficVectorIterator i = activeTraffic.begin();
 519     // Search search if the current id alread has an entry
 520     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 521     if (activeTraffic.size()) {
 522         //while ((i->getId() != id) && i != activeTraffic.end()) {
 523         while (i != activeTraffic.end()) {
 524             if (i->getId() == id) {
 525                 break;
 526             }
 527             i++;
 528         }
 529     }
 530     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 531         SG_LOG(SG_GENERAL, SG_ALERT,
 532                "AI error: Aircraft without traffic record is signing off");
 533     } else {
 534         i = activeTraffic.erase(i);
 535     }
 536 }
 537
 538 bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, TrafficVectorIterator i, time_t now, AtcMsgId msgId,
 539                                AtcMsgDir msgDir)
 540 {
 541     int state = i->getState();
 542     if ((state >= minState) && (state <= maxState) && available) {
 543         if ((msgDir == ATC_AIR_TO_GROUND) && isUserAircraft(i->getAircraft())) {
 544             //cerr << "Checking state " << state << " for " << i->getAircraft()->getCallSign() << endl;
 545             static SGPropertyNode_ptr trans_num = globals->get_props()->getNode("/sim/atc/transmission-num", true);
 546             int n = trans_num->getIntValue();
 547             if (n == 0) {
 548                 trans_num->setIntValue(-1);
 549                  // PopupCallback(n);
 550                  cerr << "Selected transmission message " << n << endl;
 551                  FGATCManager *atc = (FGATCManager*) globals->get_subsystem("atc");
 552                  atc->getATCDialog()->removeEntry(1);
 553             } else {
 554                 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
 555                 transmit(&(*i), msgId, msgDir, false);
 556                 return false;
 557             }
 558         }
 559         transmit(&(*i), msgId, msgDir, true);
 560         i->updateState();
 561         lastTransmission = now;
 562         available = false;
 563         return true;
 564     }
 565     return false;
 566 }
 567
 568 void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
 569                                                 double heading, double speed, double alt,
 570                                                 double dt)
 571 {
 572     // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to
 573     // Transmit air-to-ground "Ready to taxi request:
 574     // Transmit ground to air approval / hold
 575     // Transmit confirmation ...
 576     // Probably use a status mechanism similar to the Engine start procedure in the startup controller.
 577
 578
 579     TrafficVectorIterator i = activeTraffic.begin();
 580     // Search search if the current id has an entry
 581     // This might be faster using a map instead of a vector, but let's start by taking a safe route
 582     TrafficVectorIterator current, closest;
 583     if (activeTraffic.size()) {
 584         //while ((i->getId() != id) && i != activeTraffic.end()) {
 585         while (i != activeTraffic.end()) {
 586             if (i->getId() == id) {
 587                 break;
 588             }
 589             i++;
 590         }
 591     }
 592     // update position of the current aircraft
 593     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 594         SG_LOG(SG_GENERAL, SG_ALERT,
 595                "AI error: updating aircraft without traffic record");
 596     } else {
 597         i->setPositionAndHeading(lat, lon, heading, speed, alt);
 598         current = i;
 599     }
 600
 601     setDt(getDt() + dt);
 602
 603     // Update every three secs, but add some randomness
 604     // to prevent all IA objects doing this in synchrony
 605     //if (getDt() < (3.0) + (rand() % 10))
 606     //  return;
 607     //else
 608     //  setDt(0);
 609     current->clearResolveCircularWait();
 610     current->setWaitsForId(0);
 611     checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
 612     bool needsTaxiClearance = current->getAircraft()->getTaxiClearanceRequest();
 613     if (!needsTaxiClearance) {
 614         checkHoldPosition(id, lat, lon, heading, speed, alt);
 615         if (checkForCircularWaits(id)) {
 616             i->setResolveCircularWait();
 617         }
 618     } else {
 619         current->setHoldPosition(true);
 620         int state = current->getState();
 621         time_t now = time(NULL) + fgGetLong("/sim/time/warp");
 622         if ((now - lastTransmission) > 15) {
 623             available = true;
 624         }
 625         if (checkTransmissionState(0,2, current, now, MSG_REQUEST_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
 626             current->setState(3);
 627         }
 628         if (checkTransmissionState(3,3, current, now, MSG_ISSUE_TAXI_CLEARANCE, ATC_GROUND_TO_AIR)) {
 629             current->setState(4);
 630         }
 631         if (checkTransmissionState(4,4, current, now, MSG_ACKNOWLEDGE_TAXI_CLEARANCE, ATC_AIR_TO_GROUND)) {
 632             current->setState(5);
 633         }
 634         if ((state == 5) && available) {
 635             current->setState(0);
 636             current->getAircraft()->setTaxiClearanceRequest(false);
 637             current->setHoldPosition(true);
 638             available = false;
 639         }
 640
 641     }
 642
 643 }
 644
 645 /**
 646    Scan for a speed adjustment change. Find the nearest aircraft that is in front
 647    and adjust speed when we get too close. Only do this when current position and/or
 648    intentions of the current aircraft match current taxiroute position of the proximate
 649    aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
 650    instruction. See below for the hold position instruction.
 651
 652    Note that there currently still is one flaw in the logic that needs to be addressed.
 653    There can be situations where one aircraft is in front of the current aircraft, on a separate
 654    route, but really close after an intersection coming off the current route. This
 655    aircraft is still close enough to block the current aircraft. This situation is currently
 656    not addressed yet, but should be.
 657 */
 658
 659 void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
 660                                            double lon, double heading,
 661                                            double speed, double alt)
 662 {
 663
 664     TrafficVectorIterator current, closest;
 665     TrafficVectorIterator i = activeTraffic.begin();
 666     bool otherReasonToSlowDown = false;
 667     bool previousInstruction;
 668     if (activeTraffic.size()) {
 669         //while ((i->getId() != id) && (i != activeTraffic.end()))
 670         while (i != activeTraffic.end()) {
 671             if (i->getId() == id) {
 672                 break;
 673             }
 674             i++;
 675         }
 676     } else {
 677         return;
 678     }
 679     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 680         SG_LOG(SG_GENERAL, SG_ALERT,
 681                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkSpeedAdjustment");
 682     }
 683     current = i;
 684     //closest = current;
 685
 686     previousInstruction = current->getSpeedAdjustment();
 687     double mindist = HUGE_VAL;
 688     if (activeTraffic.size()) {
 689         double course, dist, bearing, minbearing, az2;
 690         SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
 691         //TrafficVector iterator closest;
 692         closest = current;
 693         for (TrafficVectorIterator i = activeTraffic.begin();
 694              i != activeTraffic.end(); i++) {
 695             if (i == current) {
 696                 continue;
 697             }
 698
 699             SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 700                                           i->getLatitude(),
 701                                           i->getAltitude()));
 702             SGGeodesy::inverse(curr, other, course, az2, dist);
 703             bearing = fabs(heading - course);
 704             if (bearing > 180)
 705                 bearing = 360 - bearing;
 706             if ((dist < mindist) && (bearing < 60.0)) {
 707                 mindist = dist;
 708                 closest = i;
 709                 minbearing = bearing;
 710             }
 711         }
 712         //Check traffic at the tower controller
 713         if (towerController->hasActiveTraffic()) {
 714             for (TrafficVectorIterator i =
 715                  towerController->getActiveTraffic().begin();
 716                  i != towerController->getActiveTraffic().end(); i++) {
 717                 //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
 718                 SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 719                                               i->getLatitude(),
 720                                               i->getAltitude()));
 721                 SGGeodesy::inverse(curr, other, course, az2, dist);
 722                 bearing = fabs(heading - course);
 723                 if (bearing > 180)
 724                     bearing = 360 - bearing;
 725                 if ((dist < mindist) && (bearing < 60.0)) {
 726                     mindist = dist;
 727                     closest = i;
 728                     minbearing = bearing;
 729                     otherReasonToSlowDown = true;
 730                 }
 731             }
 732         }
 733         // Finally, check UserPosition
 734         // Note, as of 2011-08-01, this should no longer be necessecary.
 735         /*
 736         double userLatitude = fgGetDouble("/position/latitude-deg");
 737         double userLongitude = fgGetDouble("/position/longitude-deg");
 738         SGGeod user(SGGeod::fromDeg(userLongitude, userLatitude));
 739         SGGeodesy::inverse(curr, user, course, az2, dist);
 740
 741         bearing = fabs(heading - course);
 742         if (bearing > 180)
 743             bearing = 360 - bearing;
 744         if ((dist < mindist) && (bearing < 60.0)) {
 745             mindist = dist;
 746             //closest = i;
 747             minbearing = bearing;
 748             otherReasonToSlowDown = true;
 749         }
 750         */
 751         current->clearSpeedAdjustment();
 752
 753         if (current->checkPositionAndIntentions(*closest)
 754             || otherReasonToSlowDown) {
 755             double maxAllowableDistance =
 756                 (1.1 * current->getRadius()) +
 757                 (1.1 * closest->getRadius());
 758             if (mindist < 2 * maxAllowableDistance) {
 759                 if (current->getId() == closest->getWaitsForId())
 760                     return;
 761                 else
 762                     current->setWaitsForId(closest->getId());
 763                 if (closest->getId() != current->getId())
 764                     current->setSpeedAdjustment(closest->getSpeed() *
 765                                                 (mindist / 100));
 766                     if (closest->getAircraft()->isScheduledForTakeoff())
 767                         current->getAircraft()->scheduleForATCTowerDepartureControl();
 768                 else
 769                     current->setSpeedAdjustment(0);     // This can only happen when the user aircraft is the one closest
 770                 if (mindist < maxAllowableDistance) {
 771                     //double newSpeed = (maxAllowableDistance-mindist);
 772                     //current->setSpeedAdjustment(newSpeed);
 773                     //if (mindist < 0.5* maxAllowableDistance)
 774                     //  {
 775                     current->setSpeedAdjustment(0);
 776                     //  }
 777                 }
 778             }
 779         }
 780     }
 781 }
 782
 783 /**
 784    Check for "Hold position instruction".
 785    The hold position should be issued under the following conditions:
 786    1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
 787    2) For taxiing aircraft that use one taxiway in opposite directions
 788    3) For crossing or merging taxiroutes.
 789 */
 790
 791 void FGGroundNetwork::checkHoldPosition(int id, double lat,
 792                                         double lon, double heading,
 793                                         double speed, double alt)
 794 {
 795     TrafficVectorIterator current;
 796     TrafficVectorIterator i = activeTraffic.begin();
 797     if (activeTraffic.size()) {
 798         //while ((i->getId() != id) && i != activeTraffic.end())
 799         while (i != activeTraffic.end()) {
 800             if (i->getId() == id) {
 801                 break;
 802             }
 803             i++;
 804         }
 805     } else {
 806         return;
 807     }
 808     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
 809         SG_LOG(SG_GENERAL, SG_ALERT,
 810                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkHoldPosition");
 811     }
 812     current = i;
 813     bool origStatus = current->hasHoldPosition();
 814     current->setHoldPosition(false);
 815     SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
 816
 817     for (i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
 818         if (i->getId() != current->getId()) {
 819             int node = current->crosses(this, *i);
 820             if (node != -1) {
 821                 FGTaxiNode *taxiNode = findNode(node);
 822
 823                 // Determine whether it's save to continue or not.
 824                 // If we have a crossing route, there are two possibilities:
 825                 // 1) This is an interestion
 826                 // 2) This is oncoming two-way traffic, using the same taxiway.
 827                 //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
 828
 829                 SGGeod other(SGGeod::
 830                              fromDegM(i->getLongitude(), i->getLatitude(),
 831                                       i->getAltitude()));
 832                 bool needsToWait;
 833                 bool opposing;
 834                 if (current->isOpposing(this, *i, node)) {
 835                     needsToWait = true;
 836                     opposing = true;
 837                     //cerr << "Hold check 2 : " << node << "  has opposing segment " << endl;
 838                     // issue a "Hold Position" as soon as we're close to the offending node
 839                     // For now, I'm doing this as long as the other aircraft doesn't
 840                     // have a hold instruction as soon as we're within a reasonable
 841                     // distance from the offending node.
 842                     // This may be a bit of a conservative estimate though, as it may
 843                     // be well possible that both aircraft can both continue to taxi
 844                     // without crashing into each other.
 845                 } else {
 846                     opposing = false;
 847                     if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
 848                     {
 849                         needsToWait = false;
 850                         //cerr << "Hold check 3 : " << id <<"  Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
 851                         //           << endl;
 852                     } else {
 853                         needsToWait = true;
 854                         //cerr << "Hold check 4: " << id << "  Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
 855                     }
 856                 }
 857
 858                 double dist =
 859                     SGGeodesy::distanceM(curr, taxiNode->getGeod());
 860                 if (!(i->hasHoldPosition())) {
 861
 862                     if ((dist < 200) && //2.5*current->getRadius()) &&
 863                         (needsToWait) && (i->onRoute(this, *current)) &&
 864                         //((i->onRoute(this, *current)) || ((!(i->getSpeedAdjustment())))) &&
 865                         (!(current->getId() == i->getWaitsForId())))
 866                         //(!(i->getSpeedAdjustment()))) // &&
 867                         //(!(current->getSpeedAdjustment())))
 868
 869                     {
 870                         current->setHoldPosition(true);
 871                         current->setWaitsForId(i->getId());
 872                         //cerr << "Hold check 5: " << current->getCallSign() <<"  Setting Hold Position: distance to node ("  << node << ") "
 873                         //           << dist << " meters. Waiting for " << i->getCallSign();
 874                         //if (opposing)
 875                         //cerr <<" [opposing] " << endl;
 876                         //else
 877                         //        cerr << "[non-opposing] " << endl;
 878                         //if (i->hasSpeefAdjustment())
 879                         //        {
 880                         //          cerr << " (which in turn waits for ) " << i->
 881                     } else {
 882                         //cerr << "Hold check 6: " << id << "  No need to hold yet: Distance to node : " << dist << " nm"<< endl;
 883                     }
 884                 }
 885             }
 886         }
 887     }
 888     bool currStatus = current->hasHoldPosition();
 889
 890     // Either a Hold Position or a resume taxi transmission has been issued
 891     time_t now = time(NULL) + fgGetLong("/sim/time/warp");
 892     if ((now - lastTransmission) > 2) {
 893         available = true;
 894     }
 895     if ((origStatus != currStatus) && available) {
 896         //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
 897         if (currStatus == true) { // No has a hold short instruction
 898            transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
 899            //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
 900            current->setState(1);
 901         } else {
 902            transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
 903            //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
 904            current->setState(2);
 905         }
 906         lastTransmission = now;
 907         available = false;
 908         // Don't act on the changed instruction until the transmission is confirmed
 909         // So set back to original status
 910         current->setHoldPosition(origStatus);
 911         //cerr << "Current state " << current->getState() << endl;
 912     } else {
 913     }
 914     //int state = current->getState();
 915     if (checkTransmissionState(1,1, current, now, MSG_ACKNOWLEDGE_HOLD_POSITION, ATC_AIR_TO_GROUND)) {
 916             current->setState(0);
 917             current->setHoldPosition(true);
 918     }
 919     if (checkTransmissionState(2,2, current, now, MSG_ACKNOWLEDGE_RESUME_TAXI, ATC_AIR_TO_GROUND)) {
 920             current->setState(0);
 921             current->setHoldPosition(false);
 922     }
 923 }
 924
 925 /**
 926  * Check whether situations occur where the current aircraft is waiting for itself
 927  * due to higher order interactions.
 928  * A 'circular' wait is a situation where a waits for b, b waits for c, and c waits
 929  * for a. Ideally each aircraft only waits for one other aircraft, so by tracing
 930  * through this list of waiting aircraft, we can check if we'd eventually end back
 931  * at the current aircraft.
 932  *
 933  * Note that we should consider the situation where we are actually checking aircraft
 934  * d, which is waiting for aircraft a. d is not part of the loop, but is held back by
 935  * the looping aircraft. If we don't check for that, this function will get stuck into
 936  * endless loop.
 937  */
 938
 939 bool FGGroundNetwork::checkForCircularWaits(int id)
 940 {
 941     //cerr << "Performing Wait check " << id << endl;
 942     int target = 0;
 943     TrafficVectorIterator current, other;
 944     TrafficVectorIterator i = activeTraffic.begin();
 945     int trafficSize = activeTraffic.size();
 946     if (trafficSize) {
 947         while (i != activeTraffic.end()) {
 948             if (i->getId() == id) {
 949                 break;
 950             }
 951             i++;
 952         }
 953     } else {
 954         return false;
 955     }
 956     if (i == activeTraffic.end() || (trafficSize == 0)) {
 957         SG_LOG(SG_GENERAL, SG_ALERT,
 958                "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
 959     }
 960
 961     current = i;
 962     target = current->getWaitsForId();
 963     //bool printed = false; // Note that this variable is for debugging purposes only.
 964     int counter = 0;
 965
 966     if (id == target) {
 967         //cerr << "aircraft waits for user" << endl;
 968         return false;
 969     }
 970
 971
 972     while ((target > 0) && (target != id) && counter++ < trafficSize) {
 973         //printed = true;
 974         TrafficVectorIterator i = activeTraffic.begin();
 975         if (trafficSize) {
 976             //while ((i->getId() != id) && i != activeTraffic.end())
 977             while (i != activeTraffic.end()) {
 978                 if (i->getId() == target) {
 979                     break;
 980                 }
 981                 i++;
 982             }
 983         } else {
 984             return false;
 985         }
 986         if (i == activeTraffic.end() || (trafficSize == 0)) {
 987             //cerr << "[Waiting for traffic at Runway: DONE] " << endl << endl;;
 988             // The target id is not found on the current network, which means it's at the tower
 989             //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: Trying to access non-existing aircraft in FGGroundNetwork::checkForCircularWaits");
 990             return false;
 991         }
 992         other = i;
 993         target = other->getWaitsForId();
 994
 995         // actually this trap isn't as impossible as it first seemed:
 996         // the setWaitsForID(id) is set to current when the aircraft
 997         // is waiting for the user controlled aircraft.
 998         //if (current->getId() == other->getId()) {
 999         //    cerr << "Caught the impossible trap" << endl;
1000         //    cerr << "Current = " << current->getId() << endl;
1001         //    cerr << "Other   = " << other  ->getId() << endl;
1002         //    for (TrafficVectorIterator at = activeTraffic.begin();
1003         //          at != activeTraffic.end();
1004         //          at++) {
1005         //        cerr << "currently active aircraft : " << at->getCallSign() << " with Id " << at->getId() << " waits for " << at->getWaitsForId() << endl;
1006         //    }
1007         //    exit(1);
1008         if (current->getId() == other->getId())
1009             return false;
1010         //}
1011         //cerr << current->getCallSign() << " (" << current->getId()  << ") " << " -> " << other->getCallSign()
1012         //     << " (" << other->getId()  << "); " << endl;;
1013         //current = other;
1014     }
1015
1016
1017
1018
1019
1020
1021     //if (printed)
1022     //   cerr << "[done] " << endl << endl;;
1023     if (id == target) {
1024         SG_LOG(SG_GENERAL, SG_WARN,
1025                "Detected circular wait condition: Id = " << id <<
1026                "target = " << target);
1027         return true;
1028     } else {
1029         return false;
1030     }
1031 }
1032
1033 // Note that this function is probably obsolete...
1034 bool FGGroundNetwork::hasInstruction(int id)
1035 {
1036     TrafficVectorIterator i = activeTraffic.begin();
1037     // Search search if the current id has an entry
1038     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1039     if (activeTraffic.size()) {
1040         //while ((i->getId() != id) && i != activeTraffic.end()) {
1041         while (i != activeTraffic.end()) {
1042             if (i->getId() == id) {
1043                 break;
1044             }
1045             i++;
1046         }
1047     }
1048     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1049         SG_LOG(SG_GENERAL, SG_ALERT,
1050                "AI error: checking ATC instruction for aircraft without traffic record");
1051     } else {
1052         return i->hasInstruction();
1053     }
1054     return false;
1055 }
1056
1057 FGATCInstruction FGGroundNetwork::getInstruction(int id)
1058 {
1059     TrafficVectorIterator i = activeTraffic.begin();
1060     // Search search if the current id has an entry
1061     // This might be faster using a map instead of a vector, but let's start by taking a safe route
1062     if (activeTraffic.size()) {
1063         //while ((i->getId() != id) && i != activeTraffic.end()) {
1064         while (i != activeTraffic.end()) {
1065             if (i->getId() == id) {
1066                 break;
1067             }
1068             i++;
1069         }
1070     }
1071     if (i == activeTraffic.end() || (activeTraffic.size() == 0)) {
1072         SG_LOG(SG_GENERAL, SG_ALERT,
1073                "AI error: requesting ATC instruction for aircraft without traffic record");
1074     } else {
1075         return i->getInstruction();
1076     }
1077     return FGATCInstruction();
1078 }
1079
1080 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
1081 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
1082                             double lon, double elev, double hdg)
1083 {
1084     SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
1085     obj_pos = geod.makeZUpFrame();
1086     // hdg is not a compass heading, but a counter-clockwise rotation
1087     // around the Z axis
1088     obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
1089                                         0.0, 0.0, 1.0));
1090 }
1091
1092
1093
1094
1095 void FGGroundNetwork::render(bool visible)
1096 {
1097
1098     SGMaterialLib *matlib = globals->get_matlib();
1099     if (group) {
1100         //int nr = ;
1101         globals->get_scenery()->get_scene_graph()->removeChild(group);
1102         //while (group->getNumChildren()) {
1103         //  cerr << "Number of children: " << group->getNumChildren() << endl;
1104         simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
1105           //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
1106            //geode->releaseGLObjects();
1107            //group->removeChild(geode);
1108            //delete geode;
1109         group = 0;
1110     }
1111     if (visible) {
1112         group = new osg::Group;
1113
1114         //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
1115         double dx = 0;
1116         for   (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
1117             // Handle start point
1118             int pos = i->getCurrentPosition() - 1;
1119             if (pos >= 0) {
1120
1121                 SGGeod start(SGGeod::fromDeg((i->getLongitude()), (i->getLatitude())));
1122                 SGGeod end  (SGGeod::fromDeg(segments[pos]->getEnd()->getLongitude(), segments[pos]->getEnd()->getLatitude()));
1123
1124                 double length = SGGeodesy::distanceM(start, end);
1125                 //heading = SGGeodesy::headingDeg(start->getGeod(), end->getGeod());
1126
1127                 double az2, heading; //, distanceM;
1128                 SGGeodesy::inverse(start, end, heading, az2, length);
1129                 double coveredDistance = length * 0.5;
1130                 SGGeod center;
1131                 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
1132                 //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
1133             ///////////////////////////////////////////////////////////////////////////////
1134                 // Make a helper function out of this
1135                 osg::Matrix obj_pos;
1136                 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1137                 obj_trans->setDataVariance(osg::Object::STATIC);
1138
1139                 WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), parent->elevation()+8+dx, -(heading) );
1140
1141                 obj_trans->setMatrix( obj_pos );
1142                 //osg::Vec3 center(0, 0, 0)
1143
1144                 float width = length /2.0;
1145                 osg::Vec3 corner(-width, 0, 0.25f);
1146                 osg::Vec3 widthVec(2*width + 1, 0, 0);
1147                 osg::Vec3 heightVec(0, 1, 0);
1148                 osg::Geometry* geometry;
1149                 geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1150                 simgear::EffectGeode* geode = new simgear::EffectGeode;
1151                 geode->setName("test");
1152                 geode->addDrawable(geometry);
1153                 //osg::Node *custom_obj;
1154                 SGMaterial *mat = matlib->find("UnidirectionalTaper");
1155                 if (mat)
1156                     geode->setEffect(mat->get_effect());
1157                 obj_trans->addChild(geode);
1158                 // wire as much of the scene graph together as we can
1159                 //->addChild( obj_trans );
1160                 group->addChild( obj_trans );
1161             /////////////////////////////////////////////////////////////////////
1162             } else {
1163                 //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
1164             }
1165             for(intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
1166                 osg::Matrix obj_pos;
1167                 int k = (*j)-1;
1168                 if (k >= 0) {
1169                     osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
1170                     obj_trans->setDataVariance(osg::Object::STATIC);
1171
1172                     WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), parent->elevation()+8+dx, -(segments[k]->getHeading()) );
1173
1174                     obj_trans->setMatrix( obj_pos );
1175                     //osg::Vec3 center(0, 0, 0)
1176
1177                     float width = segments[k]->getLength() /2.0;
1178                     osg::Vec3 corner(-width, 0, 0.25f);
1179                     osg::Vec3 widthVec(2*width + 1, 0, 0);
1180                     osg::Vec3 heightVec(0, 1, 0);
1181                     osg::Geometry* geometry;
1182                     geometry = osg::createTexturedQuadGeometry(corner, widthVec, heightVec);
1183                     simgear::EffectGeode* geode = new simgear::EffectGeode;
1184                     geode->setName("test");
1185                     geode->addDrawable(geometry);
1186                     //osg::Node *custom_obj;
1187                     SGMaterial *mat = matlib->find("UnidirectionalTaper");
1188                     if (mat)
1189                         geode->setEffect(mat->get_effect());
1190                     obj_trans->addChild(geode);
1191                     // wire as much of the scene graph together as we can
1192                     //->addChild( obj_trans );
1193                     group->addChild( obj_trans );
1194                 }
1195             }
1196             //dx += 0.1;
1197         }
1198         globals->get_scenery()->get_scene_graph()->addChild(group);
1199     }
1200 }
1201
1202 string FGGroundNetwork::getName() {
1203     return string(parent->getId() + "-ground");
1204 }