Merge commit 'c8115f516c47fd389b34f59055d3c2e9a6b697e2' into next

[flightgear.git] / src / Airports / groundnetwork.cxx

diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx
index 3ea86f979243adc66ece7db3a01d714e71a4045f..a6ad1f5c44a8c0f4653d3314814f1cb7e643581e 100644 (file)
--- a/src/Airports/groundnetwork.cxx
+++ b/src/Airports/groundnetwork.cxx
@@ -24,7 +24,7 @@
 #  include <config.h>
 #endif
 
-#include <math.h>
+#include <cmath>
 #include <algorithm>
 #include <fstream>
 #include <map>
@@ -82,14 +82,14 @@ SGGeod FGTaxiSegment::getCenter() const
   return SGGeodesy::direct(start->geod(), heading, length * 0.5);
 }
 
-FGTaxiNode* FGTaxiSegment::getEnd() const
+FGTaxiNodeRef FGTaxiSegment::getEnd() const
 {
-  return static_cast<FGTaxiNode*>(NavDataCache::instance()->loadById(endNode));
+  return FGPositioned::loadById<FGTaxiNode>(endNode);
 }
 
-FGTaxiNode* FGTaxiSegment::getStart() const
+FGTaxiNodeRef FGTaxiSegment::getStart() const
 {
-  return static_cast<FGTaxiNode*>(NavDataCache::instance()->loadById(startNode));
+  return FGPositioned::loadById<FGTaxiNode>(startNode);
 }
 
 double FGTaxiSegment::getLength() const
@@ -145,13 +145,22 @@ void FGTaxiSegment::unblock(time_t now)
 /***************************************************************************
  * FGTaxiRoute
  **************************************************************************/
-bool FGTaxiRoute::next(PositionedID *nde)
+bool FGTaxiRoute::next(PositionedID *nde, int *rte)
 {
+    if (nodes.size() != (routes.size()) + 1) {
+        SG_LOG(SG_GENERAL, SG_ALERT, "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size());
+        throw sg_range_exception("Misconfigured taxi route");
+    }
     if (currNode == nodes.end())
         return false;
-  
     *nde = *(currNode);
-
+    if (currNode != nodes.begin()) {
+        *rte = *(currRoute);
+        currRoute++;
+    } else {
+        // Handle special case for the first node. 
+        *rte = -1 * *(currRoute);
+    }
     currNode++;
     return true;
 };
@@ -212,7 +221,7 @@ void FGGroundNetwork::saveElevationCache()
                        airport[0], airport[1], airport[2]);
             cacheData.append(buffer);
             if (!cacheData.exists()) {
-                cacheData.create_dir(0777);
+                cacheData.create_dir(0755);
             }
             cacheData.append(airport + "-groundnet-cache.txt");
             cachefile.open(cacheData.str().c_str());
@@ -255,7 +264,7 @@ void FGGroundNetwork::init(FGAirport* pr)
       segment->setIndex(index++);
       
       if (segment->oppositeDirection) {
-        continue; // already establish
+        continue; // already established
       }
       
       FGTaxiSegment* opp = findSegment(segment->endNode, segment->startNode);
@@ -300,7 +309,7 @@ void FGGroundNetwork::parseCache()
              airport[0], airport[1], airport[2]);
   cacheData.append(buffer);
   if (!cacheData.exists()) {
-    cacheData.create_dir(0777);
+    cacheData.create_dir(0755);
   }
   int index;
   double elev;
@@ -343,10 +352,9 @@ int FGGroundNetwork::findNearestNodeOnRunway(const SGGeod & aGeod, FGRunway* aRu
   return NavDataCache::instance()->findGroundNetNode(parent->guid(), aGeod, onRunway, aRunway);
 }
 
-FGTaxiNode* FGGroundNetwork::findNode(PositionedID idx) const
+FGTaxiNodeRef FGGroundNetwork::findNode(PositionedID idx) const
 {
-
-  return static_cast<FGTaxiNode*>(NavDataCache::instance()->loadById(idx));
+  return FGPositioned::loadById<FGTaxiNode>(idx);
 }
 
 FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx) const
@@ -394,7 +402,7 @@ public:
   {}
   
   double score;
-  FGTaxiNode_ptr previousNode;
+  FGTaxiNodeRef previousNode;
 };
 
 FGTaxiRoute FGGroundNetwork::findShortestRoute(PositionedID start, PositionedID end,
@@ -447,7 +455,7 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(PositionedID start, PositionedID
         }
       
         BOOST_FOREACH(PositionedID targetId, cache->groundNetEdgesFrom(best->guid(), !fullSearch)) {
-            FGTaxiNode* tgt = (FGTaxiNode*) cache->loadById(targetId);
+            FGTaxiNodeRef tgt = FGPositioned::loadById<FGTaxiNode>(targetId);
             double edgeLength = dist(best->cart(), tgt->cart());          
             double alt = searchData[best].score + edgeLength + edgePenalty(tgt);
             if (alt < searchData[tgt].score) {    // Relax (u,v)
@@ -470,14 +478,21 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(PositionedID start, PositionedID
   
     // assemble route from backtrace information
     PositionedIDVec nodes;
+    intVec routes;
     FGTaxiNode *bt = lastNode;
+    
     while (searchData[bt].previousNode != 0) {
         nodes.push_back(bt->guid());
+        FGTaxiSegment *segment = findSegment(searchData[bt].previousNode->guid(), bt->guid());
+        int idx = segment->getIndex();
+        routes.push_back(idx);
         bt = searchData[bt].previousNode;
+        
     }
     nodes.push_back(start);
     reverse(nodes.begin(), nodes.end());
-    return FGTaxiRoute(nodes, searchData[lastNode].score, 0);
+    reverse(routes.begin(), routes.end());
+    return FGTaxiRoute(nodes, routes, searchData[lastNode].score, 0);
 }
 
 /* ATC Related Functions */
@@ -490,7 +505,9 @@ void FGGroundNetwork::announcePosition(int id,
                                        double radius, int leg,
                                        FGAIAircraft * aircraft)
 {
+    
     assert(parent);
+    init(parent);
   
     TrafficVectorIterator i = activeTraffic.begin();
     // Search search if the current id alread has an entry
@@ -1141,7 +1158,6 @@ static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
 
 void FGGroundNetwork::render(bool visible)
 {
-
     SGMaterialLib *matlib = globals->get_matlib();
     if (group) {
         //int nr = ;
@@ -1164,7 +1180,8 @@ void FGGroundNetwork::render(bool visible)
         //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
         //double dx = 0;
         for   (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
-            // Handle start point
+            // Handle start point i.e. the segment that is connected to the aircraft itself on the starting end
+            // and to the the first "real" taxi segment on the other end. 
             int pos = i->getCurrentPosition() - 1;
             if (pos >= 0) {
 
@@ -1179,7 +1196,7 @@ void FGGroundNetwork::render(bool visible)
                 double coveredDistance = length * 0.5;
                 SGGeod center;
                 SGGeodesy::direct(start, heading, coveredDistance, center, az2);
-                //cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
+                //std::cerr << "Active Aircraft : Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << std::endl;
                 ///////////////////////////////////////////////////////////////////////////////
                 // Make a helper function out of this
                 osg::Matrix obj_pos;
@@ -1231,9 +1248,9 @@ void FGGroundNetwork::render(bool visible)
                 //osg::Node *custom_obj;
                 SGMaterial *mat;
                 if (segments[pos]->hasBlock(now)) {
-                    mat = matlib->find("UnidirectionalTaperRed");
+                    mat = matlib->find("UnidirectionalTaperRed", center);
                 } else {
-                    mat = matlib->find("UnidirectionalTaperGreen");
+                    mat = matlib->find("UnidirectionalTaperGreen", center);
                 }
                 if (mat)
                     geode->setEffect(mat->get_effect());
@@ -1243,8 +1260,9 @@ void FGGroundNetwork::render(bool visible)
                 group->addChild( obj_trans );
                 /////////////////////////////////////////////////////////////////////
             } else {
-                //cerr << "BIG FAT WARNING: current position is here : " << pos << endl;
+                //std::cerr << "BIG FAT WARNING: current position is here : " << pos << std::endl;
             }
+            // Next: Draw the other taxi segments. 
             for (intVecIterator j = (i)->getIntentions().begin(); j != (i)->getIntentions().end(); j++) {
                 osg::Matrix obj_pos;
                 int k = (*j)-1;
@@ -1305,9 +1323,9 @@ void FGGroundNetwork::render(bool visible)
                     //osg::Node *custom_obj;
                     SGMaterial *mat;
                     if (segments[k]->hasBlock(now)) {
-                        mat = matlib->find("UnidirectionalTaperRed");
+                        mat = matlib->find("UnidirectionalTaperRed", segCenter);
                     } else {
-                        mat = matlib->find("UnidirectionalTaperGreen");
+                        mat = matlib->find("UnidirectionalTaperGreen", segCenter);
                     }
                     if (mat)
                         geode->setEffect(mat->get_effect());