heading = rwy._heading;
double azimuth = heading + 180.0;
while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
+ cerr << "Creating climb at : " << rwy._id << " " << rwy._rwy_no << endl;
geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, heading,
10*SG_NM_TO_METER,
&lat2, &lon2, &az2 );
}
-/*******************************************************************
- * CreateCruise
- * initialize the Aircraft at the parking location
- ******************************************************************/
-void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
- FGAirport *arr, double latitude,
- double longitude, double speed,
- double alt)
-{
- double wind_speed;
- double wind_heading;
- double heading;
- double lat, lon, az;
- double lat2, lon2, az2;
- double azimuth;
- waypoint *wpt;
+// /*******************************************************************
+// * CreateCruise
+// * initialize the Aircraft at the parking location
+// ******************************************************************/
+// void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
+// FGAirport *arr, double latitude,
+// double longitude, double speed,
+// double alt)
+// {
+// 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;
- waypoints.push_back(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;
+// 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; }
+// // 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; }
- 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;
- waypoints.push_back(wpt);
-}
+// 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;
+// waypoints.push_back(wpt);
+// }
/*******************************************************************
* CreateDecent
--- /dev/null
+/******************************************************************************
+ * AIFlightPlanCreateCruise.cxx
+ * Written by Durk Talsma, started February, 2006.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * 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.
+ *
+ *
+ **************************************************************************/
+#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/runways.hxx>
+
+#include <Environment/environment_mgr.hxx>
+#include <Environment/environment.hxx>
+
+SG_USING_STD(iostream);
+
+void FGAIFlightPlan::evaluateRoutePart(double deplat,
+ double deplon,
+ double arrlat,
+ double arrlon)
+{
+ // First do a prescan of all the waypoints that are within a reasonable distance of the
+ // ideal route.
+ intVec nodes;
+ int tmpNode, prevNode;
+
+
+ SGWayPoint first (deplon,
+ deplat,
+ 100);
+ SGWayPoint sec (arrlon,
+ arrlat,
+ 100);
+ 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);
+
+ angle = sgACos(sgdScalarProductVec3(a,b));
+ tmpNode = 0;
+ for (double ang = 0.0; ang < angle; ang += 0.05)
+ {
+ //cerr << "Angle = " << ang << endl;
+ sgdMakeRotMat4(matrix, ang, cross);
+ 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;
+ temp = sgCartToPolar3d(Point3D(newPos[0], newPos[1],newPos[2]));
+ midlat = temp.lat() * SG_RADIANS_TO_DEGREES;
+ midlon = temp.lon() * SG_RADIANS_TO_DEGREES;
+
+ prevNode = tmpNode;
+ tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
+
+ 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);
+ }
+
+ intVecIterator i = nodes.begin();
+ intVecIterator j = nodes.end();
+ while (i != nodes.end())
+ {
+ j = nodes.end();
+ while (j != i)
+ {
+ j--;
+ FGAirRoute routePart = globals->get_airwaynet()->findShortestRoute(*i, *j);
+ if (!(routePart.empty()))
+ {
+ route.add(routePart);
+ i = j;
+ break;
+ }
+ }
+ i++;
+ }
+}
+
+
+/*
+void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
+ FGAirport *arr, double latitude,
+ double longitude, double speed, double alt)
+{
+ bool useInitialWayPoint = true;
+ bool useCurrentWayPoint = false;
+ double heading;
+ double lat, lon, az;
+ double lat2, lon2, az2;
+ double azimuth;
+ waypoint *wpt;
+ waypoint *init_waypoint;
+ intVec ids;
+ char buffer[32];
+ SGPath routefile = globals->get_fg_root();
+ init_waypoint = new waypoint;
+ init_waypoint->name = "Initial waypoint";
+ init_waypoint->latitude = latitude;
+ init_waypoint->longitude = longitude;;
+ //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
+ init_waypoint->altitude = alt;
+ init_waypoint->speed = 450; //speed;
+ init_waypoint->crossat = -10000;
+ init_waypoint->gear_down = false;
+ init_waypoint->flaps_down= false;
+ init_waypoint->finished = false;
+ init_waypoint->on_ground = false;
+ waypoints.push_back(init_waypoint);
+ routefile.append("Data/AI/FlightPlans");
+ snprintf(buffer, 32, "%s-%s.txt",
+ dep->getId().c_str(),
+ arr->getId().c_str());
+ routefile.append(buffer);
+ cerr << "trying to read " << routefile.c_str()<< endl;
+ //exit(1);
+ if (routefile.exists())
+ {
+ sg_gzifstream in( routefile.str() );
+ do {
+ in >> route;
+ } while (!(in.eof()));
+ }
+ else {
+ //int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2);
+ //int startId = globals->get_airwaynet()->findNearestNode(dep->getLatitude(), dep->getLongitude());
+ //int endId = globals->get_airwaynet()->findNearestNode(arr->getLatitude(), arr->getLongitude());
+ //FGAirRoute route;
+ evaluateRoutePart(dep->getLatitude(), dep->getLongitude(),
+ arr->getLatitude(), arr->getLongitude());
+ //exit(1);
+ }
+ route.first();
+ int node;
+ if (route.empty()) {
+ // if no route could be found, create a direct gps route...
+ cerr << "still no route found from " << dep->getName() << " to << " << arr->getName() <<endl;
+
+ //exit(1);
+ } else {
+ while(route.next(&node))
+ {
+ FGNode *fn = globals->get_airwaynet()->findNode(node);
+ //cerr << "Checking status of each waypoint: " << fn->getIdent();
+
+ SGWayPoint first(init_waypoint->longitude,
+ init_waypoint->latitude,
+ alt);
+ SGWayPoint curr (fn->getLongitude(),
+ fn->getLatitude(),
+ alt);
+ SGWayPoint arr (arr->getLongitude(),
+ arr->getLatitude(),
+ alt);
+
+ double crse, crsDiff;
+ double dist;
+ first.CourseAndDistance(arr, &course, &distance);
+ first.CourseAndDistance(curr, &crse, &dist);
+
+ dist *= SG_METER_TO_NM;
+
+ // We're only interested in the absolute value of crsDiff
+ // wich should fall in the 0-180 deg range.
+ crsDiff = fabs(crse-course);
+ if (crsDiff > 180)
+ crsDiff = 360-crsDiff;
+ // These are the three conditions that we consider including
+ // in our flight plan:
+ // 1) current waypoint is less then 100 miles away OR
+ // 2) curren waypoint is ahead of us, at any distance
+ //cerr << " Distance : " << dist << " : Course diff " << crsDiff
+ // << " crs to dest : " << course
+ // << " crs to wpt : " << crse;
+ if ((dist > 20.0) && (crsDiff > 90.0))
+ {
+ //useWpt = false;
+ // Once we start including waypoints, we have to continue, even though
+ // one of the following way point would suffice.
+ // so once is the useWpt flag is set to true, we cannot reset it to false.
+ //cerr << " discarding " << endl;
+ // << ": Course difference = " << crsDiff
+ // << "Course = " << course
+ // << "crse = " << crse << endl;
+ }
+ else {
+ //i = ids.end()-1;
+ //cerr << " accepting " << endl;
+
+ //ids.pop_back();
+ wpt = new waypoint;
+ wpt->name = "Airway"; // fixme: should be the name of the waypoint
+ wpt->latitude = fn->getLatitude();
+ wpt->longitude = fn->getLongitude();
+ //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
+ wpt->altitude = alt;
+ wpt->speed = 450; //speed;
+ wpt->crossat = -10000;
+ wpt->gear_down = false;
+ wpt->flaps_down= false;
+ wpt->finished = false;
+ wpt->on_ground = false;
+ waypoints.push_back(wpt);
+ }
+
+ if (!(routefile.exists()))
+ {
+ route.first();
+ fstream outf( routefile.c_str(), fstream::out );
+ while (route.next(&node))
+ outf << node << endl;
+ }
+ }
+ }
+ arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
+ if (!(globals->get_runways()->search(arr->getId(),
+ activeRunway,
+ &rwy)))
+ {
+ cout << "Failed to find runway for " << arr->getId() << endl;
+ // Hmm, how do we handle a potential error like this?
+ exit(1);
+ }
+ //string test;
+ //arr->getActiveRunway(string("com"), 1, test);
+ //exit(1);
+
+ //cerr << "Altitude = " << alt << endl;
+ //cerr << "Done" << endl;
+ //if (arr->getId() == "EHAM")
+ // {
+ // cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
+ // }
+ heading = rwy._heading;
+ azimuth = heading + 180.0;
+ while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
+
+
+ // Note: This places us at the location of the active
+ // runway during initial cruise. This needs to be
+ // fixed later.
+ geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
+ 110000,
+ &lat2, &lon2, &az2 );
+ wpt = new waypoint;
+ wpt->name = "BOD"; //wpt_node->getStringValue("name", "END");
+ 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;
+ waypoints.push_back(wpt);
+}
+*/
+
+
+/*******************************************************************
+ * CreateCruise
+ * initialize the Aircraft at the parking location
+ *
+ * Note that this is the original version that does not
+ * do any dynamic route computation.
+ ******************************************************************/
+void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
+ FGAirport *arr, double latitude,
+ double longitude, double speed,
+ double alt)
+{
+ 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;
+ 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; }
+
+
+ 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;
+ waypoints.push_back(wpt);
+}
projects / flightgear.git / commitdiff