Read groundnet node information from the scenery, and (optionally) write it to a...

The current patch incorporates work contributed by Adrian Musceac.

diff --git a/src/ATC/trafficcontrol.cxx b/src/ATC/trafficcontrol.cxx
index 8025fe6f7ba104d96a7f44df0968a2486965302a..47f5acc8a620e7842b07f1e7be2bc4cca823ecaf 100644 (file)
--- a/src/ATC/trafficcontrol.cxx
+++ b/src/ATC/trafficcontrol.cxx
@@ -1249,7 +1249,7 @@ void FGStartupController::updateAircraftInformation(int id, double lat, double l
 
 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
-                            double lon, double elev, double hdg)
+                            double lon, double elev, double hdg, double slope)
 {
     SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
     obj_pos = geod.makeZUpFrame();
@@ -1257,6 +1257,8 @@ static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
     // around the Z axis
     obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
                                         0.0, 0.0, 1.0));
+    obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
+                                        0.0, 1.0, 0.0));
 }
 
 
@@ -1269,7 +1271,7 @@ void FGStartupController::render(bool visible)
         globals->get_scenery()->get_scene_graph()->removeChild(group);
         //while (group->getNumChildren()) {
         //  cerr << "Number of children: " << group->getNumChildren() << endl;
-        simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
+        //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
           //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
            //geode->releaseGLObjects();
            //group->removeChild(geode);
@@ -1278,6 +1280,10 @@ void FGStartupController::render(bool visible)
     }
     if (visible) {
         group = new osg::Group;
+        FGScenery * local_scenery = globals->get_scenery();
+        double elevation_meters = 0.0;
+        double elevation_feet = 0.0;
+
 
         //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
         double dx = 0;
@@ -1304,8 +1310,36 @@ void FGStartupController::render(bool visible)
                 osg::Matrix obj_pos;
                 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
                 obj_trans->setDataVariance(osg::Object::STATIC);
+                // Experimental: Calculate slope here, based on length, and the individual elevations
+                double elevationStart;
+                if (isUserAircraft((i)->getAircraft())) {
+                    elevationStart = fgGetDouble("/position/ground-elev-m");
+                } else {
+                    elevationStart = ((i)->getAircraft()->_getAltitude()); 
+                }
+                double elevationEnd   = segment->getEnd()->getElevation();
+                if (elevationEnd == 0) {
+                    SGGeod center2 = end;
+                    center2.setElevationM(SG_MAX_ELEVATION_M);
+                    if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
+                        elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
+                            //elevation_meters += 0.5;
+                    }
+                    else { 
+                        elevationEnd = parent->getElevation()+8+dx;
+                    }
+                    segment->getEnd()->setElevation(elevationEnd);
+                }
+
+                double elevationMean  = (elevationStart + elevationEnd) / 2.0;
+                double elevDiff       = elevationEnd - elevationStart;
+               
+               double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
+                
+               //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
 
-                WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), parent->getElevation()+8+dx, -(heading) );
+                WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean + 0.5, -(heading), slope );
+;
 
                 obj_trans->setMatrix( obj_pos );
                 //osg::Vec3 center(0, 0, 0)
@@ -1339,7 +1373,45 @@ void FGStartupController::render(bool visible)
                     osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
                     obj_trans->setDataVariance(osg::Object::STATIC);
                     FGTaxiSegment *segment  = parent->getGroundNetwork()->findSegment(k);
-                    WorldCoordinate( obj_pos, segment->getLatitude(), segment->getLongitude(), parent->getElevation()+8+dx, -(segment->getHeading()) );
+
+                    double elevationStart = segment->getStart()->getElevation();
+                    double elevationEnd   = segment->getEnd  ()->getElevation();
+                    if (elevationStart == 0) {
+                        SGGeod center2 = segment->getStart()->getGeod();
+                        center2.setElevationM(SG_MAX_ELEVATION_M);
+                        if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
+                            elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
+                            //elevation_meters += 0.5;
+                        }
+                        else { 
+                            elevationStart = parent->getElevation()+8+dx;
+                        }
+                        segment->getStart()->setElevation(elevationStart);
+                    }
+                    if (elevationEnd == 0) {
+                        SGGeod center2 = segment->getEnd()->getGeod();
+                        center2.setElevationM(SG_MAX_ELEVATION_M);
+                        if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
+                            elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
+                            //elevation_meters += 0.5;
+                        }
+                        else { 
+                            elevationEnd = parent->getElevation()+8+dx;
+                        }
+                        segment->getEnd()->setElevation(elevationEnd);
+                    }
+ 
+                    double elevationMean  = (elevationStart + elevationEnd) / 2.0;
+                    double elevDiff       = elevationEnd - elevationStart;
+                    double length         = segment->getLength();
+                    double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
+                
+                    //cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
+
+
+                    WorldCoordinate( obj_pos, segment->getLatitude(), segment->getLongitude(), elevationMean + 0.5, -(segment->getHeading()), slope );
+
+                    //WorldCoordinate( obj_pos, segment->getLatitude(), segment->getLongitude(), parent->getElevation()+8+dx, -(segment->getHeading()) );
 
                     obj_trans->setMatrix( obj_pos );
                     //osg::Vec3 center(0, 0, 0)

diff --git a/src/ATC/trafficcontrol.hxx b/src/ATC/trafficcontrol.hxx
index 62e87007ef3b8f1ad8c470a53f7bdb8f9aef0dd9..6aae0ab9e38769ab4283370b1f172beaf2963f68 100644 (file)
--- a/src/ATC/trafficcontrol.hxx
+++ b/src/ATC/trafficcontrol.hxx
@@ -241,9 +241,10 @@ typedef vector<ActiveRunway>::iterator ActiveRunwayVecIterator;
 class FGATCController
 {
 private:
-     bool initialized;
+    
 
 protected:
+  bool initialized;
   bool available;
   time_t lastTransmission;
 

diff --git a/src/Airports/gnnode.cxx b/src/Airports/gnnode.cxx
index ae37d59594a03b65fc86847563820028659507ba..cdf29f64074f53b3ef7443662e353bed5873cc55 100644 (file)
--- a/src/Airports/gnnode.cxx
+++ b/src/Airports/gnnode.cxx
@@ -1,6 +1,7 @@
 #include "gnnode.hxx"
 #include "groundnetwork.hxx"
 
+#include <iostream>
 #include <algorithm>
 using std::sort;
 
@@ -74,3 +75,5 @@ void FGTaxiNode::setLongitude(const string& val)
 //  else
 //    sort(next.begin(), next.end(), sortByHeadingDiff);
 //}
+
+

diff --git a/src/Airports/gnnode.hxx b/src/Airports/gnnode.hxx
index a7627715e8e55190add131526e69edc09bcda447..d7a90038fe27643bd7860681f3bd1b3f703d2758 100644 (file)
--- a/src/Airports/gnnode.hxx
+++ b/src/Airports/gnnode.hxx
@@ -44,6 +44,7 @@ private:
   FGTaxiNode* previousNode;
   FGTaxiSegment* previousSeg;
 
+
 public:
   FGTaxiNode() :
       index(0),
@@ -69,14 +70,14 @@ public:
 
 FGTaxiNode &operator =(const FGTaxiNode &other)
 {
-   geod          = other.geod;
-   index        = other.index;
-   isOnRunway   = other.isOnRunway;
-   holdType     = other.holdType;
-   next         = other.next;
-   pathScore    = other.pathScore;
-   previousNode = other.previousNode;
-   previousSeg  = other.previousSeg;
+   geod                               = other.geod;
+   index                              = other.index;
+   isOnRunway                         = other.isOnRunway;
+   holdType                           = other.holdType;
+   next                               = other.next;
+   pathScore                          = other.pathScore;
+   previousNode                       = other.previousNode;
+   previousSeg                        = other.previousSeg;
    return *this;
 };
 
@@ -113,7 +114,6 @@ FGTaxiNode &operator =(const FGTaxiNode &other)
   FGTaxiSegmentVectorIterator getEndRoute()   { return next.end();   }; 
   bool operator<(const FGTaxiNode &other) const { return index < other.index; };
 
-  //void sortEndSegments(bool);
 
 };
 

diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx
index a642a7768bedc6584a366618837dea9ffce40351..86beec118568cd64b26cfe9dfc99d2398abf1532 100644 (file)
--- a/src/Airports/groundnetwork.cxx
+++ b/src/Airports/groundnetwork.cxx
@@ -100,6 +100,8 @@ void FGTaxiSegment::setDimensions(double elevation)
     SGGeodesy::inverse(start->getGeod(), end->getGeod(), heading, az2, length);
     double coveredDistance = length * 0.5;
     SGGeodesy::direct(start->getGeod(), heading, coveredDistance, center, az2);
+    //start->setElevation(elevation);
+    //end->setElevation(elevation);
     //cerr << "Centerpoint = (" << center.getLatitudeDeg() << ", " << center.getLongitudeDeg() << "). Heading = " << heading << endl;
 }
 
@@ -214,13 +216,40 @@ FGGroundNetwork::FGGroundNetwork()
     count = 0;
     currTraffic = activeTraffic.begin();
     group = 0;
+    networkInitialized = false;
 
 }
 
 FGGroundNetwork::~FGGroundNetwork()
 {
+    //cerr << "Running Groundnetwork Destructor " << endl;
+    bool saveData = false;
+    ofstream cachefile;
+    if (fgGetBool("/sim/ai/groundnet-cache")) {
+        SGPath cacheData(fgGetString("/sim/fg-home"));
+        cacheData.append("ai");
+        string airport = parent->getId();
+
+        if ((airport) != "") {
+            char buffer[128];
+            ::snprintf(buffer, 128, "%c/%c/%c/",
+                       airport[0], airport[1], airport[2]);
+            cacheData.append(buffer);
+            if (!cacheData.exists()) {
+                cacheData.create_dir(0777);
+            }
+            cacheData.append(airport + "-groundnet-cache.txt");
+            cachefile.open(cacheData.str().c_str());
+            saveData = true;
+        }
+    }
     for (FGTaxiNodeVectorIterator node = nodes.begin();
          node != nodes.end(); node++) {
+         if (saveData) {
+            cachefile << (*node)->getIndex     () << " "
+                      << (*node)->getElevation ()   << " " 
+                      << endl;
+         }
         delete(*node);
     }
     nodes.clear();
@@ -230,6 +259,44 @@ FGGroundNetwork::~FGGroundNetwork()
         delete(*seg);
     }
     segments.clear();
+    if (saveData) {
+        cachefile.close();
+    }
+}
+
+void FGGroundNetwork::saveElevationCache() {
+    //cerr << "Running Groundnetwork Destructor " << endl;
+    bool saveData = false;
+    ofstream cachefile;
+    if (fgGetBool("/sim/ai/groundnet-cache")) {
+        SGPath cacheData(fgGetString("/sim/fg-home"));
+        cacheData.append("ai");
+        string airport = parent->getId();
+
+        if ((airport) != "") {
+            char buffer[128];
+            ::snprintf(buffer, 128, "%c/%c/%c/",
+                       airport[0], airport[1], airport[2]);
+            cacheData.append(buffer);
+            if (!cacheData.exists()) {
+                cacheData.create_dir(0777);
+            }
+            cacheData.append(airport + "-groundnet-cache.txt");
+            cachefile.open(cacheData.str().c_str());
+            saveData = true;
+        }
+    }
+    for (FGTaxiNodeVectorIterator node = nodes.begin();
+         node != nodes.end(); node++) {
+         if (saveData) {
+            cachefile << (*node)->getIndex     () << " "
+                      << (*node)->getElevation () << " " 
+                      << endl;
+         }
+    }
+    if (saveData) {
+        cachefile.close();
+    }
 }
 
 void FGGroundNetwork::addSegment(const FGTaxiSegment & seg)
@@ -261,7 +328,13 @@ void FGGroundNetwork::addNodes(FGParkingVec * parkings)
 
 void FGGroundNetwork::init()
 {
+    if (networkInitialized) {
+        FGATCController::init();
+        //cerr << "FGground network already initialized" << endl;
+        return;
+    }
     hasNetwork = true;
+    nextSave = 0;
     int index = 1;
     sort(nodes.begin(), nodes.end(), compare_nodes);
     //sort(segments.begin(), segments.end(), compare_segments());
@@ -269,7 +342,7 @@ void FGGroundNetwork::init()
     while (i != segments.end()) {
         (*i)->setStart(&nodes);
         (*i)->setEnd(&nodes);
-        (*i)->setDimensions(parent->getElevation());
+        (*i)->setDimensions(parent->getElevation() * SG_FEET_TO_METER);
         (*i)->setIndex(index);
         if ((*i)->isPushBack()) {
             pushBackNodes.push_back((*i)->getEnd());
@@ -310,6 +383,42 @@ void FGGroundNetwork::init()
     //}
     //cerr << "Done initializing ground network" << endl;
     //exit(1);
+    if (fgGetBool("/sim/ai/groundnet-cache")) {
+        SGPath cacheData(fgGetString("/sim/fg-home"));
+        cacheData.append("ai");
+        string airport = parent->getId();
+
+        if ((airport) != "") {
+            char buffer[128];
+            ::snprintf(buffer, 128, "%c/%c/%c/",
+                       airport[0], airport[1], airport[2]);
+            cacheData.append(buffer);
+            if (!cacheData.exists()) {
+                cacheData.create_dir(0777);
+            }
+            int index;
+            double elev;
+            cacheData.append(airport + "-groundnet-cache.txt");
+            if (cacheData.exists()) {
+                ifstream data(cacheData.c_str());
+                for (FGTaxiNodeVectorIterator i = nodes.begin();
+                     i != nodes.end(); 
+                     i++) {
+                    (*i)->setElevation(parent->getElevation() * SG_FEET_TO_METER);
+                    data >> index >> elev;
+                    if (data.eof())
+                        break;
+                    if (index != (*i)->getIndex()) {
+                        SG_LOG(SG_GENERAL, SG_ALERT, "Index read from ground network cache at airport " << airport << " does not match index in the network itself");
+                    } else {
+                        (*i)->setElevation(elev);
+                    }
+                }
+            }
+        }
+    }
+    //cerr << "Finished initializing " << parent->getId() << " groundnetwork " << endl;
+    networkInitialized = true;
 }
 
 int FGGroundNetwork::findNearestNode(const SGGeod & aGeod)
@@ -582,6 +691,11 @@ void FGGroundNetwork::updateAircraftInformation(int id, double lat, double lon,
                                                 double heading, double speed, double alt,
                                                 double dt)
 {
+    time_t currentTime = time(NULL);
+    if (nextSave < currentTime) {
+        saveElevationCache();
+        nextSave = currentTime + 100 + rand() % 200;
+    }
     // Check whether aircraft are on hold due to a preceding pushback. If so, make sure to 
     // Transmit air-to-ground "Ready to taxi request:
     // Transmit ground to air approval / hold
@@ -1124,7 +1238,7 @@ FGATCInstruction FGGroundNetwork::getInstruction(int id)
 
 // Note that this function is copied from simgear. for maintanance purposes, it's probabtl better to make a general function out of that.
 static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
-                            double lon, double elev, double hdg)
+                            double lon, double elev, double hdg, double slope)
 {
     SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
     obj_pos = geod.makeZUpFrame();
@@ -1132,6 +1246,8 @@ static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
     // around the Z axis
     obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
                                         0.0, 0.0, 1.0));
+    obj_pos.preMult(osg::Matrix::rotate(slope * SGD_DEGREES_TO_RADIANS,
+                                        0.0, 1.0, 0.0));
 }
 
 
@@ -1146,7 +1262,7 @@ void FGGroundNetwork::render(bool visible)
         globals->get_scenery()->get_scene_graph()->removeChild(group);
         //while (group->getNumChildren()) {
         //  cerr << "Number of children: " << group->getNumChildren() << endl;
-        simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
+        //simgear::EffectGeode* geode = (simgear::EffectGeode*) group->getChild(0);
           //osg::MatrixTransform *obj_trans = (osg::MatrixTransform*) group->getChild(0);
            //geode->releaseGLObjects();
            //group->removeChild(geode);
@@ -1155,7 +1271,9 @@ void FGGroundNetwork::render(bool visible)
     }
     if (visible) {
         group = new osg::Group;
-
+        FGScenery * local_scenery = globals->get_scenery();
+        double elevation_meters = 0.0;
+        double elevation_feet = 0.0;
         //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
         double dx = 0;
         for   (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
@@ -1180,8 +1298,36 @@ void FGGroundNetwork::render(bool visible)
                 osg::Matrix obj_pos;
                 osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
                 obj_trans->setDataVariance(osg::Object::STATIC);
+                // Experimental: Calculate slope here, based on length, and the individual elevations
+                double elevationStart;
+                if (isUserAircraft((i)->getAircraft())) {
+                    elevationStart = fgGetDouble("/position/ground-elev-m");
+                } else {
+                    elevationStart = ((i)->getAircraft()->_getAltitude()); 
+                }
+                double elevationEnd   = segments[pos]->getEnd()->getElevation();
+                //cerr << "Using elevation " << elevationEnd << endl;
+
+                if (elevationEnd == 0) {
+                    SGGeod center2 = end;
+                    center2.setElevationM(SG_MAX_ELEVATION_M);
+                    if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
+                        elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
+                            //elevation_meters += 0.5;
+                    }
+                    else { 
+                        elevationEnd = parent->getElevation()+8+dx;
+                    }
+                    segments[pos]->getEnd()->setElevation(elevationEnd);
+                }
+                double elevationMean  = (elevationStart + elevationEnd) / 2.0;
+                double elevDiff       = elevationEnd - elevationStart;
+               
+               double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
+                
+               //cerr << "1. Using mean elevation : " << elevationMean << " and " << slope << endl;
 
-                WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), parent->elevation()+8+dx, -(heading) );
+                WorldCoordinate( obj_pos, center.getLatitudeDeg(), center.getLongitudeDeg(), elevationMean+ 0.5, -(heading), slope );
 
                 obj_trans->setMatrix( obj_pos );
                 //osg::Vec3 center(0, 0, 0)
@@ -1214,7 +1360,43 @@ void FGGroundNetwork::render(bool visible)
                     osg::MatrixTransform *obj_trans = new osg::MatrixTransform;
                     obj_trans->setDataVariance(osg::Object::STATIC);
 
-                    WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), parent->elevation()+8+dx, -(segments[k]->getHeading()) );
+                    // Experimental: Calculate slope here, based on length, and the individual elevations
+                    double elevationStart = segments[k]->getStart()->getElevation();
+                    double elevationEnd   = segments[k]->getEnd  ()->getElevation();
+                    if (elevationStart == 0) {
+                        SGGeod center2 = segments[k]->getStart()->getGeod();
+                        center2.setElevationM(SG_MAX_ELEVATION_M);
+                        if (local_scenery->get_elevation_m( center2, elevationStart, NULL )) {
+                            elevation_feet = elevationStart * SG_METER_TO_FEET + 0.5;
+                            //elevation_meters += 0.5;
+                        }
+                        else { 
+                            elevationStart = parent->getElevation()+8+dx;
+                        }
+                        segments[k]->getStart()->setElevation(elevationStart);
+                    }
+                    if (elevationEnd == 0) {
+                        SGGeod center2 = segments[k]->getEnd()->getGeod();
+                        center2.setElevationM(SG_MAX_ELEVATION_M);
+                        if (local_scenery->get_elevation_m( center2, elevationEnd, NULL )) {
+                            elevation_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
+                            //elevation_meters += 0.5;
+                        }
+                        else { 
+                            elevationEnd = parent->getElevation()+8+dx;
+                        }
+                        segments[k]->getEnd()->setElevation(elevationEnd);
+                    }
+
+                    double elevationMean  = (elevationStart + elevationEnd) / 2.0;
+                    double elevDiff       = elevationEnd - elevationStart;
+                    double length         = segments[k]->getLength();
+                    double slope = atan2(elevDiff, length) * SGD_RADIANS_TO_DEGREES;
+                
+                   // cerr << "2. Using mean elevation : " << elevationMean << " and " << slope << endl;
+
+
+                    WorldCoordinate( obj_pos, segments[k]->getLatitude(), segments[k]->getLongitude(), elevationMean+ 0.5, -(segments[k]->getHeading()), slope );
 
                     obj_trans->setMatrix( obj_pos );
                     //osg::Vec3 center(0, 0, 0)

diff --git a/src/Airports/groundnetwork.hxx b/src/Airports/groundnetwork.hxx
index 3de47c2bafb2b1cfc4f7929ba7d616356a315a16..a493426872df45c5803f8082e8a376f0ec75c7f9 100644 (file)
--- a/src/Airports/groundnetwork.hxx
+++ b/src/Airports/groundnetwork.hxx
@@ -150,6 +150,7 @@ public:
   void setCourseDiff(double crse);
 
 
+
   
 };
 
@@ -227,6 +228,8 @@ class FGGroundNetwork : public FGATCController
 {
 private:
   bool hasNetwork;
+  bool networkInitialized;
+  time_t nextSave;
   //int maxDepth;
   int count;
   FGTaxiNodeVector    nodes;
@@ -292,6 +295,7 @@ public:
   virtual void render(bool);
   virtual string getName();
 
+  void saveElevationCache();
 };
 
 

diff --git a/src/Airports/simple.cxx b/src/Airports/simple.cxx
index 7354db28be12753f00648058241645d9b77f4b7c..92b79005ad756d5a8b3945a8dd2316f5bb534a76 100644 (file)
--- a/src/Airports/simple.cxx
+++ b/src/Airports/simple.cxx
@@ -76,6 +76,7 @@ FGAirport::FGAirport(const string &id, const SGGeod& location, const SGGeod& tow
 
 FGAirport::~FGAirport()
 {
+    cerr << "Deleting Airport" << endl;
     delete _dynamics;
 }