*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
*
*
**************************************************************************/
#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>
+#include <Airports/airport.hxx>
#include <Airports/runways.hxx>
#include <Airports/dynamics.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,
double deplon,
double arrlat,
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();
}
}
-
+*/
/*
void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
FGAirport *arr, double latitude,
init_waypoint->flaps_down= false;
init_waypoint->finished = false;
init_waypoint->on_ground = false;
- waypoints.push_back(init_waypoint);
+ pushBackWaypoint(init_waypoint);
routefile.append("Data/AI/FlightPlans");
snprintf(buffer, 32, "%s-%s.txt",
dep->getId().c_str(),
wpt->flaps_down= false;
wpt->finished = false;
wpt->on_ground = false;
- waypoints.push_back(wpt);
+ pushBackWaypoint(wpt);
}
if (!(routefile.exists()))
// {
// cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
// }
- heading = rwy->headingDeg();
+ heading = rwy._heading;
azimuth = heading + 180.0;
while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
wpt->flaps_down= false;
wpt->finished = false;
wpt->on_ground = false;
- waypoints.push_back(wpt);
+ pushBackWaypoint(wpt);
}
*/
* Note that this is the original version that does not
* do any dynamic route computation.
******************************************************************/
-void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
+bool FGAIFlightPlan::createCruise(FGAIAircraft *ac, bool firstFlight, FGAirport *dep,
FGAirport *arr, double latitude,
double longitude, double speed,
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;
- waypoints.push_back(wpt);
-
-
- string rwyClass = getRunwayClassFromTrafficType(fltType);
- arr->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway);
- rwy = arr->getRunwayByIdent(activeRunway);
-
- heading = rwy->headingDeg();
- azimuth = heading + 180.0;
- while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
+ double vCruise = ac->getPerformance()->vCruise();
+ FGAIWaypoint *wpt;
+ wpt = createInAir(ac, "Cruise", SGGeod::fromDeg(longitude, latitude), alt, vCruise);
+ pushBackWaypoint(wpt);
+ const string& rwyClass = getRunwayClassFromTrafficType(fltType);
+ double heading = ac->getTrafficRef()->getCourse();
+ arr->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway, heading);
+ FGRunway* rwy = arr->getRunwayByIdent(activeRunway);
+ assert( rwy != NULL );
+ // begin descent 110km out
+ SGGeod beginDescentPoint = rwy->pointOnCenterline(0);
+ SGGeod secondaryDescentPoint = rwy->pointOnCenterline(-10000);
- geo_direct_wgs_84 ( 0, rwy->latitude(), rwy->longitude(), 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;
- waypoints.push_back(wpt);
+ wpt = createInAir(ac, "BOD", beginDescentPoint, alt, vCruise);
+ pushBackWaypoint(wpt);
+ wpt = createInAir(ac, "BOD2", secondaryDescentPoint, alt, vCruise);
+ pushBackWaypoint(wpt);
+ return true;
}
projects / flightgear.git / blobdiff