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