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., 675 Mass Ave, Cambridge, MA 02139, USA.
20 **************************************************************************/
23 #include "AIFlightPlan.hxx"
24 #include <simgear/math/polar3d.hxx>
25 #include <simgear/math/sg_geodesy.hxx>
26 #include <simgear/route/waypoint.hxx>
28 #include <Navaids/awynet.hxx>
29 #include <Airports/runways.hxx>
31 #include <Environment/environment_mgr.hxx>
32 #include <Environment/environment.hxx>
34 SG_USING_STD(iostream);
36 void FGAIFlightPlan::evaluateRoutePart(double deplat,
41 // First do a prescan of all the waypoints that are within a reasonable distance of the
44 int tmpNode, prevNode;
47 SGWayPoint first (deplon,
50 SGWayPoint sec (arrlon,
53 first.CourseAndDistance(sec, &course, &distance);
54 distance *= SG_METER_TO_NM;
55 temp = sgPolarToCart3d(Point3D(deplon *
56 SG_DEGREES_TO_RADIANS,
58 SG_DEGREES_TO_RADIANS,
64 temp = sgPolarToCart3d(Point3D(arrlon *
65 SG_DEGREES_TO_RADIANS,
67 SG_DEGREES_TO_RADIANS,
74 sgdVectorProductVec3(cross,b,a);
76 angle = sgACos(sgdScalarProductVec3(a,b));
78 for (double ang = 0.0; ang < angle; ang += 0.05)
80 //cerr << "Angle = " << ang << endl;
81 sgdMakeRotMat4(matrix, ang, cross);
82 for(int j = 0; j < 3; j++)
85 for (int k = 0; k<3; k++)
87 newPos[j] += matrix[j][k]*a[k];
91 temp = sgCartToPolar3d(Point3D(newPos[0], newPos[1],newPos[2]));
92 midlat = temp.lat() * SG_RADIANS_TO_DEGREES;
93 midlon = temp.lon() * SG_RADIANS_TO_DEGREES;
96 tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
98 double nodelat = globals->get_airwaynet()->findNode(tmpNode)->getLatitude ();
99 double nodelon = globals->get_airwaynet()->findNode(tmpNode)->getLongitude ();
100 SGWayPoint curr(midlat,
103 SGWayPoint node(nodelat,
106 curr.CourseAndDistance(node, &course, &distance);
107 if ((distance < 25000) && (tmpNode != prevNode))
108 nodes.push_back(tmpNode);
111 intVecIterator i = nodes.begin();
112 intVecIterator j = nodes.end();
113 while (i != nodes.end())
119 FGAirRoute routePart = globals->get_airwaynet()->findShortestRoute(*i, *j);
120 if (!(routePart.empty()))
122 airRoute.add(routePart);
133 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
134 FGAirport *arr, double latitude,
135 double longitude, double speed, double alt)
137 bool useInitialWayPoint = true;
138 bool useCurrentWayPoint = false;
141 double lat2, lon2, az2;
144 waypoint *init_waypoint;
147 SGPath routefile = globals->get_fg_root();
148 init_waypoint = new waypoint;
149 init_waypoint->name = "Initial waypoint";
150 init_waypoint->latitude = latitude;
151 init_waypoint->longitude = longitude;;
152 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
153 init_waypoint->altitude = alt;
154 init_waypoint->speed = 450; //speed;
155 init_waypoint->crossat = -10000;
156 init_waypoint->gear_down = false;
157 init_waypoint->flaps_down= false;
158 init_waypoint->finished = false;
159 init_waypoint->on_ground = false;
160 waypoints.push_back(init_waypoint);
161 routefile.append("Data/AI/FlightPlans");
162 snprintf(buffer, 32, "%s-%s.txt",
163 dep->getId().c_str(),
164 arr->getId().c_str());
165 routefile.append(buffer);
166 cerr << "trying to read " << routefile.c_str()<< endl;
168 if (routefile.exists())
170 sg_gzifstream in( routefile.str() );
173 } while (!(in.eof()));
176 //int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2);
177 //int startId = globals->get_airwaynet()->findNearestNode(dep->getLatitude(), dep->getLongitude());
178 //int endId = globals->get_airwaynet()->findNearestNode(arr->getLatitude(), arr->getLongitude());
180 evaluateRoutePart(dep->getLatitude(), dep->getLongitude(),
181 arr->getLatitude(), arr->getLongitude());
187 // if no route could be found, create a direct gps route...
188 cerr << "still no route found from " << dep->getName() << " to << " << arr->getName() <<endl;
192 while(route.next(&node))
194 FGNode *fn = globals->get_airwaynet()->findNode(node);
195 //cerr << "Checking status of each waypoint: " << fn->getIdent();
197 SGWayPoint first(init_waypoint->longitude,
198 init_waypoint->latitude,
200 SGWayPoint curr (fn->getLongitude(),
203 SGWayPoint arr (arr->getLongitude(),
207 double crse, crsDiff;
209 first.CourseAndDistance(arr, &course, &distance);
210 first.CourseAndDistance(curr, &crse, &dist);
212 dist *= SG_METER_TO_NM;
214 // We're only interested in the absolute value of crsDiff
215 // wich should fall in the 0-180 deg range.
216 crsDiff = fabs(crse-course);
218 crsDiff = 360-crsDiff;
219 // These are the three conditions that we consider including
220 // in our flight plan:
221 // 1) current waypoint is less then 100 miles away OR
222 // 2) curren waypoint is ahead of us, at any distance
223 //cerr << " Distance : " << dist << " : Course diff " << crsDiff
224 // << " crs to dest : " << course
225 // << " crs to wpt : " << crse;
226 if ((dist > 20.0) && (crsDiff > 90.0))
229 // Once we start including waypoints, we have to continue, even though
230 // one of the following way point would suffice.
231 // so once is the useWpt flag is set to true, we cannot reset it to false.
232 //cerr << " discarding " << endl;
233 // << ": Course difference = " << crsDiff
234 // << "Course = " << course
235 // << "crse = " << crse << endl;
239 //cerr << " accepting " << endl;
243 wpt->name = "Airway"; // fixme: should be the name of the waypoint
244 wpt->latitude = fn->getLatitude();
245 wpt->longitude = fn->getLongitude();
246 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
248 wpt->speed = 450; //speed;
249 wpt->crossat = -10000;
250 wpt->gear_down = false;
251 wpt->flaps_down= false;
252 wpt->finished = false;
253 wpt->on_ground = false;
254 waypoints.push_back(wpt);
257 if (!(routefile.exists()))
260 fstream outf( routefile.c_str(), fstream::out );
261 while (route.next(&node))
262 outf << node << endl;
266 arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
267 if (!(globals->get_runways()->search(arr->getId(),
271 cout << "Failed to find runway for " << arr->getId() << endl;
272 // Hmm, how do we handle a potential error like this?
276 //arr->getActiveRunway(string("com"), 1, test);
279 //cerr << "Altitude = " << alt << endl;
280 //cerr << "Done" << endl;
281 //if (arr->getId() == "EHAM")
283 // cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
285 heading = rwy._heading;
286 azimuth = heading + 180.0;
287 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
290 // Note: This places us at the location of the active
291 // runway during initial cruise. This needs to be
293 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
295 &lat2, &lon2, &az2 );
297 wpt->name = "BOD"; //wpt_node->getStringValue("name", "END");
298 wpt->latitude = lat2;
299 wpt->longitude = lon2;
303 wpt->gear_down = false;
304 wpt->flaps_down= false;
305 wpt->finished = false;
306 wpt->on_ground = false;
307 waypoints.push_back(wpt);
312 /*******************************************************************
314 * initialize the Aircraft at the parking location
316 * Note that this is the original version that does not
317 * do any dynamic route computation.
318 ******************************************************************/
319 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
320 FGAirport *arr, double latitude,
321 double longitude, double speed,
328 double lat2, lon2, az2;
333 wpt->name = "Cruise"; //wpt_node->getStringValue("name", "END");
334 wpt->latitude = latitude;
335 wpt->longitude = longitude;
338 wpt->crossat = -10000;
339 wpt->gear_down = false;
340 wpt->flaps_down= false;
341 wpt->finished = false;
342 wpt->on_ground = false;
344 waypoints.push_back(wpt);
347 // should be changed dynamically to allow "gen" and "mil"
348 arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
349 if (!(globals->get_runways()->search(arr->getId(),
353 SG_LOG(SG_INPUT, SG_ALERT, "Failed to find runway " <<
355 " at airport " << arr->getId());
358 heading = rwy._heading;
359 azimuth = heading + 180.0;
360 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
363 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
365 &lat2, &lon2, &az2 );
368 wpt->latitude = lat2;
369 wpt->longitude = lon2;
373 wpt->gear_down = false;
374 wpt->flaps_down= false;
375 wpt->finished = false;
376 wpt->on_ground = false;
378 waypoints.push_back(wpt);