Port over remaining Point3D usage to the more type and unit safe SG* classes.

[flightgear.git] / src / AIModel / AIFlightPlanCreateCruise.cxx

diff --git a/src/AIModel/AIFlightPlanCreateCruise.cxx b/src/AIModel/AIFlightPlanCreateCruise.cxx
index fc81c4dbd088700858beb4d46a86c842cd0cf947..4f714cc8cd3f13af267ceb0a976e5aa3d0108ded 100755 (executable)
--- a/src/AIModel/AIFlightPlanCreateCruise.cxx
+++ b/src/AIModel/AIFlightPlanCreateCruise.cxx
@@ -18,20 +18,25 @@
  *
  *
  **************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#  include <config.h>
+#endif
+
 #include <fstream>
 #include <iostream>
 #include "AIFlightPlan.hxx"
-#include <simgear/math/polar3d.hxx>
-#include <simgear/math/sg_geodesy.hxx>
+#include "AIAircraft.hxx"
 #include <simgear/route/waypoint.hxx>
 
 #include <Navaids/awynet.hxx>
 #include <Airports/runways.hxx>
+#include <Airports/dynamics.hxx>
 
 #include <Environment/environment_mgr.hxx>
 #include <Environment/environment.hxx>
 
-SG_USING_STD(iostream);
+using std::iostream;
 
 void FGAIFlightPlan::evaluateRoutePart(double deplat,
                                       double deplon,
@@ -50,35 +55,22 @@ void FGAIFlightPlan::evaluateRoutePart(double deplat,
   SGWayPoint sec (arrlon,
                  arrlat,
                  100);
+  double course, distance;
   first.CourseAndDistance(sec, &course, &distance);
   distance *= SG_METER_TO_NM;
-  temp = sgPolarToCart3d(Point3D(deplon *
-                                SG_DEGREES_TO_RADIANS,
-                                deplat  *
-                                SG_DEGREES_TO_RADIANS,
-                                1.0));
-  a[0] = temp.x();
-  a[1] = temp.y();
-  a[2] = temp.z();
 
-  temp = sgPolarToCart3d(Point3D(arrlon *
-                                SG_DEGREES_TO_RADIANS,
-                                arrlat  *
-                                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(deplon, deplat, 1));
+  SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arrlon, arrlat, 1));
+  SGVec3d _cross = cross(b, a);
 
-  angle = sgACos(sgdScalarProductVec3(a,b));
+  double angle = sgACos(dot(a, b));
   tmpNode = 0;
   for (double ang = 0.0; ang < angle; ang += 0.05)
     {
+      sgdVec3 newPos;
+      sgdMat4 matrix;
       //cerr << "Angle = " << ang << endl;
-      sgdMakeRotMat4(matrix, ang, cross);
+      sgdMakeRotMat4(matrix, ang, _cross.sg());
       for(int j = 0; j < 3; j++)
        {
          newPos[j] =0.0;
@@ -88,9 +80,10 @@ void FGAIFlightPlan::evaluateRoutePart(double deplat,
            }
        }
       //cerr << "1"<< endl;
-      temp = sgCartToPolar3d(Point3D(newPos[0], newPos[1],newPos[2]));
-      midlat = temp.lat() * SG_RADIANS_TO_DEGREES;
-      midlon = temp.lon() * SG_RADIANS_TO_DEGREES;
+      SGGeoc geoc = SGGeoc::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
+
+      double midlat = geoc.getLatitudeDeg();
+      double midlon = geoc.getLongitudeDeg();
 
       prevNode = tmpNode;
       tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
@@ -316,64 +309,22 @@ void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
  * Note that this is the original version that does not 
  * do any dynamic route computation.
  ******************************************************************/
-void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep, 
+void FGAIFlightPlan::createCruise(FGAIAircraft *ac, bool firstFlight, FGAirport *dep, 
                                  FGAirport *arr, double latitude, 
                                  double longitude, double speed, 
-                                 double alt)
+                                 double alt, const string& fltType)
 {
-  double wind_speed;
-  double wind_heading;
-  double heading;
-  double lat, lon, az;
-  double lat2, lon2, az2;
-  double azimuth;
   waypoint *wpt;
-
-  wpt = new waypoint;
-  wpt->name      = "Cruise"; //wpt_node->getStringValue("name", "END");
-  wpt->latitude  = latitude;
-  wpt->longitude = longitude;
-  wpt->altitude  = alt;
-  wpt->speed     = speed; 
-  wpt->crossat   = -10000;
-  wpt->gear_down = false;
-  wpt->flaps_down= false;
-  wpt->finished  = false;
-  wpt->on_ground = false;
-  wpt->routeIndex = 0;
+  wpt = createInAir(ac, "Cruise", SGGeod::fromDeg(longitude, latitude), alt, speed);
   waypoints.push_back(wpt); 
   
- 
-  // should be changed dynamically to allow "gen" and "mil"
-  arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
-  if (!(globals->get_runways()->search(arr->getId(), 
-                                      activeRunway, 
-                                      &rwy)))
-    {
-      SG_LOG(SG_INPUT, SG_ALERT, "Failed to find runway " << 
-            activeRunway << 
-            " at airport     " << arr->getId());
-      exit(1);
-    }
-  heading = rwy._heading;
-  azimuth = heading + 180.0;
-  while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
-  
+  string rwyClass = getRunwayClassFromTrafficType(fltType);
+  double heading = ac->getTrafficRef()->getCourse();
+  arr->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway, heading);
+  rwy = arr->getRunwayByIdent(activeRunway);
+  // begin descent 110km out
+  SGGeod beginDescentPoint = rwy->pointOnCenterline(-110000);
   
-  geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth, 
-                     110000,
-                     &lat2, &lon2, &az2 );
-  wpt = new waypoint;
-  wpt->name      = "BOD";
-  wpt->latitude  = lat2;
-  wpt->longitude = lon2;
-  wpt->altitude  = alt;
-  wpt->speed     = speed; 
-  wpt->crossat   = alt;
-  wpt->gear_down = false;
-  wpt->flaps_down= false;
-  wpt->finished  = false;
-  wpt->on_ground = false;
-  wpt->routeIndex = 0;
+  wpt = createInAir(ac, "BOD", beginDescentPoint, alt, speed);
   waypoints.push_back(wpt); 
 }