Canvas: Add new element type map for geo mapping.

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

diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx
index f817e30351f6935370094579b5a87b42acc64f4c..a63002eac87ed95e4cb24aee6e6ef4886fa89270 100644 (file)
--- a/src/Airports/groundnetwork.cxx
+++ b/src/Airports/groundnetwork.cxx
@@ -26,6 +26,7 @@
 
 #include <math.h>
 #include <algorithm>
+#include <fstream>
 
 
 #include <osg/Geode>
@@ -34,7 +35,6 @@
 #include <osg/Shape>
 
 #include <simgear/debug/logstream.hxx>
-#include <simgear/route/waypoint.hxx>
 #include <simgear/scene/material/EffectGeode.hxx>
 #include <simgear/scene/material/matlib.hxx>
 #include <simgear/scene/material/mat.hxx>
@@ -530,7 +530,7 @@ FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
                                    return itr->getAddress();
                                    } */
 
-    if ((idx >= 0) && (idx < nodes.size()))
+    if (idx < nodes.size())
         return nodes[idx]->getAddress();
     else
         return 0;
@@ -579,9 +579,23 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
     }
 
     FGTaxiNode *firstNode = findNode(start);
+    if (!firstNode)
+    {
+        SG_LOG(SG_GENERAL, SG_ALERT,
+               "Error in ground network. Failed to find first waypoint: " << start
+               << " at " << ((parent) ? parent->getId() : "<unknown>"));
+        return FGTaxiRoute();
+    }
     firstNode->setPathScore(0);
 
     FGTaxiNode *lastNode = findNode(end);
+    if (!lastNode)
+    {
+        SG_LOG(SG_GENERAL, SG_ALERT,
+               "Error in ground network. Failed to find last waypoint: " << end
+               << " at " << ((parent) ? parent->getId() : "<unknown>"));
+        return FGTaxiRoute();
+    }
 
     FGTaxiNodeVector unvisited(*currNodesSet);  // working copy
 
@@ -605,6 +619,13 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end,
                     seg != best->getEndRoute(); seg++) {
                 if (fullSearch || (*seg)->isPushBack()) {
                     FGTaxiNode *tgt = (*seg)->getEnd();
+                    if (!tgt)
+                    {
+                        SG_LOG(SG_GENERAL, SG_ALERT,
+                               "Error in ground network. Found empty segment "
+                               << " at " << ((parent) ? parent->getId() : "<unknown>"));
+                        return FGTaxiRoute();
+                    }
                     double alt =
                         best->getPathScore() + (*seg)->getLength() +
                         (*seg)->getPenalty(nParkings);
@@ -755,11 +776,11 @@ bool FGGroundNetwork::checkTransmissionState(int minState, int maxState, Traffic
                 FGATCDialogNew::instance()->removeEntry(1);
             } else {
                 //cerr << "creating message for " << i->getAircraft()->getCallSign() << endl;
-                transmit(&(*i), msgId, msgDir, false);
+                transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, false);
                 return false;
             }
         }
-        transmit(&(*i), msgId, msgDir, true);
+        transmit(&(*i), &(*parent->getDynamics()), msgId, msgDir, true);
         i->updateState();
         lastTransmission = now;
         available = false;
@@ -871,7 +892,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
     TrafficVectorIterator current, closest, closestOnNetwork;
     TrafficVectorIterator i = activeTraffic.begin();
     bool otherReasonToSlowDown = false;
-    bool previousInstruction;
+//    bool previousInstruction;
     if (activeTraffic.size()) {
         //while ((i->getId() != id) && (i != activeTraffic.end()))
         while (i != activeTraffic.end()) {
@@ -890,10 +911,10 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
     current = i;
     //closest = current;
 
-    previousInstruction = current->getSpeedAdjustment();
+//    previousInstruction = current->getSpeedAdjustment();
     double mindist = HUGE_VAL;
     if (activeTraffic.size()) {
-        double course, dist, bearing, minbearing, az2;
+        double course, dist, bearing, az2; // minbearing,
         SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
         //TrafficVector iterator closest;
         closest = current;
@@ -915,7 +936,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
                 mindist = dist;
                 closest = i;
                 closestOnNetwork = i;
-                minbearing = bearing;
+//                minbearing = bearing;
                 
             }
         }
@@ -939,7 +960,7 @@ void FGGroundNetwork::checkSpeedAdjustment(int id, double lat,
                     //     << endl;
                     mindist = dist;
                     closest = i;
-                    minbearing = bearing;
+//                    minbearing = bearing;
                     otherReasonToSlowDown = true;
                 }
             }
@@ -1097,11 +1118,11 @@ void FGGroundNetwork::checkHoldPosition(int id, double lat,
         if ((origStatus != currStatus) && available) {
             //cerr << "Issueing hold short instrudtion " << currStatus << " " << available << endl;
             if (currStatus == true) { // No has a hold short instruction
-                transmit(&(*current), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
+                transmit(&(*current), &(*parent->getDynamics()), MSG_HOLD_POSITION, ATC_GROUND_TO_AIR, true);
                 //cerr << "Transmittin hold short instrudtion " << currStatus << " " << available << endl;
                 current->setState(1);
             } else {
-                transmit(&(*current), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
+                transmit(&(*current), &(*parent->getDynamics()), MSG_RESUME_TAXI, ATC_GROUND_TO_AIR, true);
                 //cerr << "Transmittig resume instrudtion " << currStatus << " " << available << endl;
                 current->setState(2);
             }
@@ -1305,7 +1326,7 @@ static void WorldCoordinate(osg::Matrix& obj_pos, double lat,
                             double lon, double elev, double hdg, double slope)
 {
     SGGeod geod = SGGeod::fromDegM(lon, lat, elev);
-    obj_pos = geod.makeZUpFrame();
+    obj_pos = makeZUpFrame(geod);
     // hdg is not a compass heading, but a counter-clockwise rotation
     // around the Z axis
     obj_pos.preMult(osg::Matrix::rotate(hdg * SGD_DEGREES_TO_RADIANS,
@@ -1336,11 +1357,11 @@ 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;
+        // double elevation_meters = 0.0;
+//        double elevation_feet = 0.0;
         time_t now = time(NULL) + fgGetLong("/sim/time/warp");
         //for ( FGTaxiSegmentVectorIterator i = segments.begin(); i != segments.end(); i++) {
-        double dx = 0;
+        //double dx = 0;
         for   (TrafficVectorIterator i = activeTraffic.begin(); i != activeTraffic.end(); i++) {
             // Handle start point
             int pos = i->getCurrentPosition() - 1;
@@ -1377,7 +1398,7 @@ void FGGroundNetwork::render(bool visible)
                     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_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
                         //elevation_meters += 0.5;
                     }
                     else {
@@ -1437,7 +1458,7 @@ void FGGroundNetwork::render(bool visible)
                         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_feet = elevationStart * SG_METER_TO_FEET + 0.5;
                             //elevation_meters += 0.5;
                         }
                         else {
@@ -1449,7 +1470,7 @@ void FGGroundNetwork::render(bool visible)
                         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_feet = elevationEnd * SG_METER_TO_FEET + 0.5;
                             //elevation_meters += 0.5;
                         }
                         else {
@@ -1521,7 +1542,7 @@ void FGGroundNetwork::update(double dt)
         i->setPriority(priority++);
         // in meters per second;
         double vTaxi = (i->getAircraft()->getPerformance()->vTaxi() * SG_NM_TO_METER) / 3600;
-        if (i->isActive(60)) {
+        if (i->isActive(0)) {
 
             // Check for all active aircraft whether it's current pos segment is
             // an opposite of one of the departing aircraft's intentions