X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAIModel%2FAIFlightPlanCreateCruise.cxx;h=13df4b6072425b93218866be5b0b6420e5f52fe8;hb=ae50c054a9007b98f1a8dafe6d589d0b4cab8873;hp=d20a2134be25c086e7ac21b538d03a0744f5b61f;hpb=3836abcf4b60844d4942e6c5da237fb3a1f93d24;p=flightgear.git

diff --git a/src/AIModel/AIFlightPlanCreateCruise.cxx b/src/AIModel/AIFlightPlanCreateCruise.cxx
index d20a2134b..13df4b607 100644
--- a/src/AIModel/AIFlightPlanCreateCruise.cxx
+++ b/src/AIModel/AIFlightPlanCreateCruise.cxx
@@ -38,7 +38,6 @@
 #include "AIAircraft.hxx"
 #include "performancedata.hxx"
 
-
 using std::iostream;
 
 void FGAIFlightPlan::evaluateRoutePart(double deplat,
@@ -55,28 +54,18 @@ void FGAIFlightPlan::evaluateRoutePart(double deplat,
   SGGeoc arr(SGGeoc::fromDegM(arrlon, arrlat, 100.0));
   
   SGVec3d a = SGVec3d::fromGeoc(dep);
-  SGVec3d b = SGVec3d::fromGeoc(arr);
-  SGVec3d _cross = cross(b, a);
+  SGVec3d nb = normalize(SGVec3d::fromGeoc(arr));
+  SGVec3d na = normalize(a);
+  
+  SGVec3d _cross = cross(nb, na);
 
-  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.data());
-      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;
-      SGGeod geod = SGGeod::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
+  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(geod);