From e65f53b571794fc4c4f3746699f4e2e47f08e808 Mon Sep 17 00:00:00 2001 From: Durk Talsma Date: Sat, 3 Sep 2011 11:26:17 +0200 Subject: [PATCH] Read groundnet node information from the scenery, and (optionally) write it to a cache file. Note that this mechanism should be extended, so that the cache files will be updated whenever the terrain files are changing. The current patch also uncovers a bug, in the sense that the groundnetwork destructor is never called. Presumably this is due to the way airports are allocated in flightgear. Because the ground networks are never saved, I am periodically saving the cache files; but this behavior should probably be changed later on. The current patch incorporates work contributed by Adrian Musceac. --- src/ATC/trafficcontrol.cxx | 80 +++++++++++++- src/ATC/trafficcontrol.hxx | 3 +- src/Airports/gnnode.cxx | 3 + src/Airports/gnnode.hxx | 18 +-- src/Airports/groundnetwork.cxx | 194 ++++++++++++++++++++++++++++++++- src/Airports/groundnetwork.hxx | 4 + src/Airports/simple.cxx | 1 + 7 files changed, 283 insertions(+), 20 deletions(-) diff --git a/src/ATC/trafficcontrol.cxx b/src/ATC/trafficcontrol.cxx index 8025fe6f7..47f5acc8a 100644 --- 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 62e87007e..6aae0ab9e 100644 --- a/src/ATC/trafficcontrol.hxx +++ b/src/ATC/trafficcontrol.hxx @@ -241,9 +241,10 @@ typedef vector::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 ae37d5959..cdf29f640 100644 --- a/src/Airports/gnnode.cxx +++ b/src/Airports/gnnode.cxx @@ -1,6 +1,7 @@ #include "gnnode.hxx" #include "groundnetwork.hxx" +#include #include 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 a7627715e..d7a90038f 100644 --- 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 a642a7768..86beec118 100644 --- 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 3de47c2ba..a49342687 100644 --- 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 7354db28b..92b79005a 100644 --- 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; } -- 2.39.5