- rename fgcommand "set-mouse" to "set-cursor"

[flightgear.git] / src / Traffic / Schedule.cxx

diff --git a/src/Traffic/Schedule.cxx b/src/Traffic/Schedule.cxx
index 6692c8cc89d42dc0784ba67b84d0d325591a363a..557e4496349b90c985fe1d96067af5e11294de3f 100644 (file)
--- a/src/Traffic/Schedule.cxx
+++ b/src/Traffic/Schedule.cxx
@@ -173,9 +173,6 @@ bool FGAISchedule::update(time_t now)
 {
   FGAirport *dep;
   FGAirport *arr;
-  sgdVec3 a, b, cross;
-  sgdVec3 newPos;
-  sgdMat4 matrix;
   double angle;
 
   FGAIManager *aimgr;
@@ -185,7 +182,6 @@ bool FGAISchedule::update(time_t now)
   double distanceToDest;
   double speed;
 
-  Point3D temp;
   time_t 
     totalTimeEnroute, 
     elapsedTimeEnroute,
@@ -235,6 +231,7 @@ bool FGAISchedule::update(time_t now)
   if (!deptime)
     deptime = (*flights.begin())->getDepartureTime();
   FGScheduledFlightVecIterator i = flights.begin();
+  SG_LOG (SG_GENERAL, SG_DEBUG,"Traffic Manager: Processing registration " << registration << " with callsign " << (*i)->getCallSign());
   if (AIManagerRef)
     {
       // Check if this aircraft has been released. 
@@ -253,6 +250,7 @@ bool FGAISchedule::update(time_t now)
       // push it forward in time to the next scheduled departure. 
       if (((*i)->getDepartureTime() < now) && ((*i)->getArrivalTime() < now))
        {
+          SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic Manager:      Flight is in the Past");
          (*i)->update();
          return true;
        }
@@ -263,37 +261,22 @@ bool FGAISchedule::update(time_t now)
       // object for it. 
       //if ((i->getDepartureTime() < now) && (i->getArrivalTime() > now))
       
-
       // Part of this flight is in the future.
       if ((*i)->getArrivalTime() > now)
        {
+          
          dep = (*i)->getDepartureAirport();
          arr = (*i)->getArrivalAirport  ();
          if (!(dep && arr))
            return false;
          
-         temp = sgPolarToCart3d(Point3D(dep->getLongitude() * 
-                                        SG_DEGREES_TO_RADIANS, 
-                                        dep->getLatitude()  * 
-                                        SG_DEGREES_TO_RADIANS, 
-                                        1.0));
-         a[0] = temp.x();
-         a[1] = temp.y();
-         a[2] = temp.z();
-         
-         temp = sgPolarToCart3d(Point3D(arr->getLongitude() *
-                                        SG_DEGREES_TO_RADIANS,
-                                        arr->getLatitude()  *
-                                        SG_DEGREES_TO_RADIANS, 
-                                        1.0));
-         b[0] = temp.x();
-         b[1] = temp.y();
-         b[2] = temp.z();
-         sgdNormaliseVec3(a);
-         sgdNormaliseVec3(b);
-         sgdVectorProductVec3(cross,b,a);
+          SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(dep->getLongitude(),
+                                                dep->getLatitude(), 1));
+          SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arr->getLongitude(),
+                                                arr->getLatitude(), 1));
+          SGVec3d _cross = cross(b, a);
          
-         angle = sgACos(sgdScalarProductVec3(a,b));
+         angle = sgACos(dot(a, b));
          
          // Okay, at this point we have the angle between departure and 
          // arrival airport, in degrees. From here we can interpolate the
@@ -307,6 +290,7 @@ bool FGAISchedule::update(time_t now)
              //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
              elapsedTimeEnroute   = now - (*i)->getDepartureTime();
              remainingTimeEnroute = (*i)->getArrivalTime()   - now;  
+              SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic Manager:      Flight is in progress.");
            }
          else
            {
@@ -314,6 +298,7 @@ bool FGAISchedule::update(time_t now)
              lon = dep->getLongitude();
              elapsedTimeEnroute = 0;
              remainingTimeEnroute = totalTimeEnroute;
+              SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic Manager:      Flight is pending.");
            }
                  
          angle *= ( (double) elapsedTimeEnroute/ (double) totalTimeEnroute);
@@ -321,23 +306,22 @@ bool FGAISchedule::update(time_t now)
          
          //cout << "a = " << a[0] << " " << a[1] << " " << a[2] 
          //     << "b = " << b[0] << " " << b[1] << " " << b[2] << endl;  
-         sgdMakeRotMat4(matrix, angle, cross); 
+          sgdMat4 matrix;
+         sgdMakeRotMat4(matrix, angle, _cross.sg()); 
+          SGVec3d newPos(0, 0, 0);
          for(int j = 0; j < 3; j++)
            {
-             newPos[j] =0.0;
              for (int k = 0; k<3; k++)
                {
                  newPos[j] += matrix[j][k]*a[k];
                }
            }
          
-         temp = sgCartToPolar3d(Point3D(newPos[0], newPos[1],newPos[2]));
          if (now > (*i)->getDepartureTime())
            {
-             //cerr << "Lat = " << lat << ", lon = " << lon << endl;
-             //cerr << "Time diff: " << now-i->getDepartureTime() << endl;
-             lat = temp.lat() * SG_RADIANS_TO_DEGREES;
-             lon = temp.lon() * SG_RADIANS_TO_DEGREES; 
+              SGGeoc geoc = SGGeoc::fromCart(newPos);
+             lat = geoc.getLatitudeDeg();
+             lon = geoc.getLongitudeDeg(); 
            }
          else
            {
@@ -370,13 +354,14 @@ bool FGAISchedule::update(time_t now)
          // then 500nm, create this flight. At jet speeds 500 nm is roughly
          // one hour flight time, so that would be a good approximate point
          // to start a more detailed simulation of this aircraft.
-         //cerr << registration << " is currently enroute from " 
-         //   << dep->_id << " to " << arr->_id << "distance : " 
-          //   << distanceToUser*SG_METER_TO_NM << endl;
+         SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic manager: " << registration << " is scheduled for a flight from " 
+            << dep->getId() << " to " << arr->getId() << ". Current distance to user: " 
+             << distanceToUser*SG_METER_TO_NM);
          if ((distanceToUser*SG_METER_TO_NM) < TRAFFICTOAIDIST)
            {
              string flightPlanName = dep->getId() + string("-") + arr->getId() + 
                string(".xml");
+              SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic manager: Creating AIModel");
              //int alt;
              //if  ((i->getDepartureTime() < now))
              //{