- 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 039705ad201420f2dbb1744ad017af1352e1ded1..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
            {
@@ -348,13 +332,16 @@ bool FGAISchedule::update(time_t now)
          
          SGWayPoint current  (lon,
                               lat,
-                              (*i)->getCruiseAlt());
+                              (*i)->getCruiseAlt(), 
+                              SGWayPoint::SPHERICAL);
          SGWayPoint user (   userLongitude,
                              userLatitude,
-                             (*i)->getCruiseAlt());
+                             (*i)->getCruiseAlt(), 
+                             SGWayPoint::SPHERICAL);
          SGWayPoint dest (   arr->getLongitude(),
                              arr->getLatitude(),
-                             (*i)->getCruiseAlt());
+                             (*i)->getCruiseAlt(), 
+                             SGWayPoint::SPHERICAL);
          // We really only need distance to user
          // and course to destination 
          user.CourseAndDistance(current, &courseToUser, &distanceToUser);
@@ -367,14 +354,15 @@ 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");
-             int alt;
+              SG_LOG (SG_GENERAL, SG_DEBUG, "Traffic manager: Creating AIModel");
+             //int alt;
              //if  ((i->getDepartureTime() < now))
              //{
              //          alt = i->getCruiseAlt() *100;
@@ -461,8 +449,8 @@ void FGAISchedule::next()
 
 double FGAISchedule::getSpeed()
 {
-  double courseToUser,   courseToDest;
-  double distanceToUser, distanceToDest;
+  double courseToDest;
+  double distanceToDest;
   double speed, remainingTimeEnroute;
   FGAirport *dep, *arr;
 
@@ -474,10 +462,12 @@ double FGAISchedule::getSpeed()
  
   SGWayPoint dest (   dep->getLongitude(),
                      dep->getLatitude(),
-                     (*i)->getCruiseAlt()); 
+                     (*i)->getCruiseAlt(), 
+                     SGWayPoint::SPHERICAL); 
   SGWayPoint curr (    arr->getLongitude(),
                      arr->getLatitude(),
-                      (*i)->getCruiseAlt());
+                      (*i)->getCruiseAlt(), 
+                      SGWayPoint::SPHERICAL);
   remainingTimeEnroute     = (*i)->getArrivalTime() - (*i)->getDepartureTime();
   dest.CourseAndDistance(curr, &courseToDest, &distanceToDest);
   speed =  (distanceToDest*SG_METER_TO_NM) /