#include <fstream>
#include <iostream>
-#include "AIFlightPlan.hxx"
-#include "AIAircraft.hxx"
-#include <simgear/math/polar3d.hxx>
-#include <simgear/math/sg_geodesy.hxx>
#include <simgear/route/waypoint.hxx>
#include <Navaids/awynet.hxx>
#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,
intVec nodes;
int tmpNode, prevNode;
-
- 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));
+ SGGeoc dep(SGGeoc::fromDegM(deplon, deplat, 100.0));
+ SGGeoc arr(SGGeoc::fromDegM(arrlon, arrlat, 100.0));
+
+ SGVec3d a = SGVec3d::fromGeoc(dep);
+ SGVec3d b = SGVec3d::fromGeoc(arr);
SGVec3d _cross = cross(b, a);
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.sg());
+ 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];
- }
- }
+ {
+ 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();
+ SGGeod geod = SGGeod::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
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();
double longitude, double speed,
double alt, const string& fltType)
{
+ double vCruise = ac->getPerformance()->vCruise();
waypoint *wpt;
- wpt = createInAir(ac, "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);
// begin descent 110km out
SGGeod beginDescentPoint = rwy->pointOnCenterline(-110000);
- wpt = createInAir(ac, "BOD", beginDescentPoint, alt, speed);
+ wpt = createInAir(ac, "BOD", beginDescentPoint, alt, vCruise);
waypoints.push_back(wpt);
}