X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAIFlightPlanCreateCruise.cxx;h=269acda945bec5bc919609a8d28f085ac319ec86;hb=a2ffbba23e9dddef1f821b4d222923cfd87a71fb;hp=d2fc62473be08e4e2b5caf54b63f29cc018a007d;hpb=8ad7f4eb70d307bfb6f7b14a1a63ab2331d27183;p=flightgear.git

diff --git a/src/AIModel/AIFlightPlanCreateCruise.cxx b/src/AIModel/AIFlightPlanCreateCruise.cxx
old mode 100755
new mode 100644
index d2fc62473..269acda94
--- a/src/AIModel/AIFlightPlanCreateCruise.cxx
+++ b/src/AIModel/AIFlightPlanCreateCruise.cxx
@@ -25,9 +25,6 @@
 
 #include <fstream>
 #include <iostream>
-#include "AIFlightPlan.hxx"
-#include <simgear/math/polar3d.hxx>
-#include <simgear/math/sg_geodesy.hxx>
 #include <simgear/route/waypoint.hxx>
 
 #include <Navaids/awynet.hxx>
@@ -37,6 +34,10 @@
 #include <Environment/environment_mgr.hxx>
 #include <Environment/environment.hxx>
 
+#include "AIFlightPlan.hxx"
+#include "AIAircraft.hxx"
+#include "performancedata.hxx"
+
 using std::iostream;
 
 void FGAIFlightPlan::evaluateRoutePart(double deplat,
@@ -49,57 +50,31 @@ void FGAIFlightPlan::evaluateRoutePart(double deplat,
   intVec nodes;
   int tmpNode, prevNode;
 
+  SGGeoc dep(SGGeoc::fromDegM(deplon, deplat, 100.0));
+  SGGeoc arr(SGGeoc::fromDegM(arrlon, arrlat, 100.0));
+  
+  SGVec3d a = SGVec3d::fromGeoc(dep);
+  SGVec3d nb = normalize(SGVec3d::fromGeoc(arr));
+  SGVec3d na = normalize(a);
+  
+  SGVec3d _cross = cross(nb, na);
 
-  SGWayPoint first (deplon,
-		    deplat,
-		    100);
-  SGWayPoint sec (arrlon,
-		  arrlat,
-		  100);
-  double course, distance;
-  first.CourseAndDistance(sec, &course, &distance);
-  distance *= SG_METER_TO_NM;
-
-  SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(deplon, deplat, 1));
-  SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arrlon, arrlat, 1));
-  SGVec3d _cross = cross(b, a);
-
-  double angle = sgACos(dot(a, b));
+  double angle = acos(dot(na, nb));
+  const double angleStep = 0.05 * SG_DEGREES_TO_RADIANS;
   tmpNode = 0;
-  for (double ang = 0.0; ang < angle; ang += 0.05)
-    {
-      sgdVec3 newPos;
-      sgdMat4 matrix;
-      //cerr << "Angle = " << ang << endl;
-      sgdMakeRotMat4(matrix, ang, _cross.sg());
-      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];
-	    }
-	}
-      //cerr << "1"<< endl;
-      SGGeoc geoc = SGGeoc::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
-
-      double midlat = geoc.getLatitudeDeg();
-      double midlon = geoc.getLongitudeDeg();
+  for (double ang = 0.0; ang < angle; ang += angleStep)
+  {  
+      SGQuatd q = SGQuatd::fromAngleAxis(ang, _cross);
+      SGGeod geod = SGGeod::fromCart(q.transform(a));
 
       prevNode = tmpNode;
-      tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
+      tmpNode = globals->get_airwaynet()->findNearestNode(geod);
 
-      double nodelat = globals->get_airwaynet()->findNode(tmpNode)->getLatitude  ();
-      double nodelon = globals->get_airwaynet()->findNode(tmpNode)->getLongitude ();
-      SGWayPoint curr(midlat,
-      	              midlon,
-      	              100);
-      SGWayPoint node(nodelat,
-      	              nodelon,
-      	              100);
-      curr.CourseAndDistance(node, &course, &distance);
-      if ((distance < 25000) && (tmpNode != prevNode))
-      	nodes.push_back(tmpNode);
+      FGNode* node = globals->get_airwaynet()->findNode(tmpNode);
+    
+      if ((tmpNode != prevNode) && (SGGeodesy::distanceM(geod, node->getPosition()) < 25000)) {
+        nodes.push_back(tmpNode);
+      }
     }
 
     intVecIterator i = nodes.begin();
@@ -310,21 +285,26 @@ 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, const string& fltType)
 {
+  double vCruise = ac->getPerformance()->vCruise();
   waypoint *wpt;
-  wpt = createInAir("Cruise", SGGeod::fromDeg(longitude, latitude), alt, speed);
+  wpt = createInAir(ac, "Cruise", SGGeod::fromDeg(longitude, latitude), alt, vCruise);
   waypoints.push_back(wpt); 
   
   string rwyClass = getRunwayClassFromTrafficType(fltType);
-  arr->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway);
+  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);
+  SGGeod beginDescentPoint     = rwy->pointOnCenterline(0);
+  SGGeod secondaryDescentPoint = rwy->pointOnCenterline(-10000);
   
-  wpt = createInAir("BOD", beginDescentPoint, alt, speed);
+  wpt = createInAir(ac, "BOD", beginDescentPoint,  alt, vCruise);
+  waypoints.push_back(wpt); 
+  wpt = createInAir(ac, "BOD2", secondaryDescentPoint, alt, vCruise);
   waypoints.push_back(wpt); 
 }