1 /******************************************************************************
2 * AIFlightPlanCreateCruise.cxx
3 * Written by Durk Talsma, started February, 2006.
5 * This program is free software; you can redistribute it and/or
6 * modify it under the terms of the GNU General Public License as
7 * published by the Free Software Foundation; either version 2 of the
8 * License, or (at your option) any later version.
10 * This program is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 * General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
20 **************************************************************************/
29 #include <Airports/airport.hxx>
30 #include <Airports/runways.hxx>
31 #include <Airports/dynamics.hxx>
33 #include <Environment/environment_mgr.hxx>
34 #include <Environment/environment.hxx>
36 #include <Traffic/Schedule.hxx>
38 #include "AIFlightPlan.hxx"
39 #include "AIAircraft.hxx"
40 #include "performancedata.hxx"
46 void FGAIFlightPlan::evaluateRoutePart(double deplat,
51 // First do a prescan of all the waypoints that are within a reasonable distance of the
54 int tmpNode, prevNode;
56 SGGeoc dep(SGGeoc::fromDegM(deplon, deplat, 100.0));
57 SGGeoc arr(SGGeoc::fromDegM(arrlon, arrlat, 100.0));
59 SGVec3d a = SGVec3d::fromGeoc(dep);
60 SGVec3d nb = normalize(SGVec3d::fromGeoc(arr));
61 SGVec3d na = normalize(a);
63 SGVec3d _cross = cross(nb, na);
65 double angle = acos(dot(na, nb));
66 const double angleStep = 0.05 * SG_DEGREES_TO_RADIANS;
68 for (double ang = 0.0; ang < angle; ang += angleStep)
70 SGQuatd q = SGQuatd::fromAngleAxis(ang, _cross);
71 SGGeod geod = SGGeod::fromCart(q.transform(a));
74 tmpNode = globals->get_airwaynet()->findNearestNode(geod);
76 FGNode* node = globals->get_airwaynet()->findNode(tmpNode);
78 if ((tmpNode != prevNode) && (SGGeodesy::distanceM(geod, node->getPosition()) < 25000)) {
79 nodes.push_back(tmpNode);
83 intVecIterator i = nodes.begin();
84 intVecIterator j = nodes.end();
85 while (i != nodes.end())
91 FGAirRoute routePart = globals->get_airwaynet()->findShortestRoute(*i, *j);
92 if (!(routePart.empty()))
94 airRoute.add(routePart);
105 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
106 FGAirport *arr, double latitude,
107 double longitude, double speed, double alt)
109 bool useInitialWayPoint = true;
110 bool useCurrentWayPoint = false;
113 double lat2, lon2, az2;
116 waypoint *init_waypoint;
119 SGPath routefile = globals->get_fg_root();
120 init_waypoint = new waypoint;
121 init_waypoint->name = "Initial waypoint";
122 init_waypoint->latitude = latitude;
123 init_waypoint->longitude = longitude;;
124 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
125 init_waypoint->altitude = alt;
126 init_waypoint->speed = 450; //speed;
127 init_waypoint->crossat = -10000;
128 init_waypoint->gear_down = false;
129 init_waypoint->flaps_down= false;
130 init_waypoint->finished = false;
131 init_waypoint->on_ground = false;
132 pushBackWaypoint(init_waypoint);
133 routefile.append("Data/AI/FlightPlans");
134 snprintf(buffer, 32, "%s-%s.txt",
135 dep->getId().c_str(),
136 arr->getId().c_str());
137 routefile.append(buffer);
138 cerr << "trying to read " << routefile.c_str()<< endl;
140 if (routefile.exists())
142 sg_gzifstream in( routefile.str() );
145 } while (!(in.eof()));
148 //int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2);
149 //int startId = globals->get_airwaynet()->findNearestNode(dep->getLatitude(), dep->getLongitude());
150 //int endId = globals->get_airwaynet()->findNearestNode(arr->getLatitude(), arr->getLongitude());
152 evaluateRoutePart(dep->getLatitude(), dep->getLongitude(),
153 arr->getLatitude(), arr->getLongitude());
159 // if no route could be found, create a direct gps route...
160 cerr << "still no route found from " << dep->getName() << " to << " << arr->getName() <<endl;
164 while(route.next(&node))
166 FGNode *fn = globals->get_airwaynet()->findNode(node);
167 //cerr << "Checking status of each waypoint: " << fn->getIdent();
169 SGWayPoint first(init_waypoint->longitude,
170 init_waypoint->latitude,
172 SGWayPoint curr (fn->getLongitude(),
175 SGWayPoint arr (arr->getLongitude(),
179 double crse, crsDiff;
181 first.CourseAndDistance(arr, &course, &distance);
182 first.CourseAndDistance(curr, &crse, &dist);
184 dist *= SG_METER_TO_NM;
186 // We're only interested in the absolute value of crsDiff
187 // wich should fall in the 0-180 deg range.
188 crsDiff = fabs(crse-course);
190 crsDiff = 360-crsDiff;
191 // These are the three conditions that we consider including
192 // in our flight plan:
193 // 1) current waypoint is less then 100 miles away OR
194 // 2) curren waypoint is ahead of us, at any distance
195 //cerr << " Distance : " << dist << " : Course diff " << crsDiff
196 // << " crs to dest : " << course
197 // << " crs to wpt : " << crse;
198 if ((dist > 20.0) && (crsDiff > 90.0))
201 // Once we start including waypoints, we have to continue, even though
202 // one of the following way point would suffice.
203 // so once is the useWpt flag is set to true, we cannot reset it to false.
204 //cerr << " discarding " << endl;
205 // << ": Course difference = " << crsDiff
206 // << "Course = " << course
207 // << "crse = " << crse << endl;
211 //cerr << " accepting " << endl;
215 wpt->name = "Airway"; // fixme: should be the name of the waypoint
216 wpt->latitude = fn->getLatitude();
217 wpt->longitude = fn->getLongitude();
218 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
220 wpt->speed = 450; //speed;
221 wpt->crossat = -10000;
222 wpt->gear_down = false;
223 wpt->flaps_down= false;
224 wpt->finished = false;
225 wpt->on_ground = false;
226 pushBackWaypoint(wpt);
229 if (!(routefile.exists()))
232 fstream outf( routefile.c_str(), fstream::out );
233 while (route.next(&node))
234 outf << node << endl;
238 arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
239 if (!(globals->get_runways()->search(arr->getId(),
243 cout << "Failed to find runway for " << arr->getId() << endl;
244 // Hmm, how do we handle a potential error like this?
248 //arr->getActiveRunway(string("com"), 1, test);
251 //cerr << "Altitude = " << alt << endl;
252 //cerr << "Done" << endl;
253 //if (arr->getId() == "EHAM")
255 // cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
257 heading = rwy._heading;
258 azimuth = heading + 180.0;
259 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
262 // Note: This places us at the location of the active
263 // runway during initial cruise. This needs to be
265 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
267 &lat2, &lon2, &az2 );
269 wpt->name = "BOD"; //wpt_node->getStringValue("name", "END");
270 wpt->latitude = lat2;
271 wpt->longitude = lon2;
275 wpt->gear_down = false;
276 wpt->flaps_down= false;
277 wpt->finished = false;
278 wpt->on_ground = false;
279 pushBackWaypoint(wpt);
284 /*******************************************************************
286 * initialize the Aircraft at the parking location
288 * Note that this is the original version that does not
289 * do any dynamic route computation.
290 ******************************************************************/
291 bool FGAIFlightPlan::createCruise(FGAIAircraft *ac, bool firstFlight, FGAirport *dep,
292 FGAirport *arr, double latitude,
293 double longitude, double speed,
294 double alt, const string& fltType)
296 double vCruise = ac->getPerformance()->vCruise();
298 wpt = createInAir(ac, "Cruise", SGGeod::fromDeg(longitude, latitude), alt, vCruise);
299 pushBackWaypoint(wpt);
301 const string& rwyClass = getRunwayClassFromTrafficType(fltType);
302 double heading = ac->getTrafficRef()->getCourse();
303 arr->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway, heading);
304 FGRunway* rwy = arr->getRunwayByIdent(activeRunway);
305 assert( rwy != NULL );
306 // begin descent 110km out
307 SGGeod beginDescentPoint = rwy->pointOnCenterline(0);
308 SGGeod secondaryDescentPoint = rwy->pointOnCenterline(-10000);
310 wpt = createInAir(ac, "BOD", beginDescentPoint, alt, vCruise);
311 pushBackWaypoint(wpt);
312 wpt = createInAir(ac, "BOD2", secondaryDescentPoint, alt, vCruise);
313 pushBackWaypoint(wpt);