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 double course, distance;
54 first.CourseAndDistance(sec, &course, &distance);
55 distance *= SG_METER_TO_NM;
57 SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(deplon, deplat, 1));
58 SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arrlon, arrlat, 1));
59 SGVec3d _cross = cross(b, a);
61 double angle = sgACos(dot(a, b));
63 for (double ang = 0.0; ang < angle; ang += 0.05)
67 //cerr << "Angle = " << ang << endl;
68 sgdMakeRotMat4(matrix, ang, _cross.sg());
69 for(int j = 0; j < 3; j++)
72 for (int k = 0; k<3; k++)
74 newPos[j] += matrix[j][k]*a[k];
78 SGGeoc geoc = SGGeoc::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
79 double midlat = geoc.getLatitudeDeg();
80 double midlon = geoc.getLongitudeDeg();
83 tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
85 double nodelat = globals->get_airwaynet()->findNode(tmpNode)->getLatitude ();
86 double nodelon = globals->get_airwaynet()->findNode(tmpNode)->getLongitude ();
87 SGWayPoint curr(midlat,
90 SGWayPoint node(nodelat,
93 curr.CourseAndDistance(node, &course, &distance);
94 if ((distance < 25000) && (tmpNode != prevNode))
95 nodes.push_back(tmpNode);
98 intVecIterator i = nodes.begin();
99 intVecIterator j = nodes.end();
100 while (i != nodes.end())
106 FGAirRoute routePart = globals->get_airwaynet()->findShortestRoute(*i, *j);
107 if (!(routePart.empty()))
109 airRoute.add(routePart);
120 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
121 FGAirport *arr, double latitude,
122 double longitude, double speed, double alt)
124 bool useInitialWayPoint = true;
125 bool useCurrentWayPoint = false;
128 double lat2, lon2, az2;
131 waypoint *init_waypoint;
134 SGPath routefile = globals->get_fg_root();
135 init_waypoint = new waypoint;
136 init_waypoint->name = "Initial waypoint";
137 init_waypoint->latitude = latitude;
138 init_waypoint->longitude = longitude;;
139 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
140 init_waypoint->altitude = alt;
141 init_waypoint->speed = 450; //speed;
142 init_waypoint->crossat = -10000;
143 init_waypoint->gear_down = false;
144 init_waypoint->flaps_down= false;
145 init_waypoint->finished = false;
146 init_waypoint->on_ground = false;
147 waypoints.push_back(init_waypoint);
148 routefile.append("Data/AI/FlightPlans");
149 snprintf(buffer, 32, "%s-%s.txt",
150 dep->getId().c_str(),
151 arr->getId().c_str());
152 routefile.append(buffer);
153 cerr << "trying to read " << routefile.c_str()<< endl;
155 if (routefile.exists())
157 sg_gzifstream in( routefile.str() );
160 } while (!(in.eof()));
163 //int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2);
164 //int startId = globals->get_airwaynet()->findNearestNode(dep->getLatitude(), dep->getLongitude());
165 //int endId = globals->get_airwaynet()->findNearestNode(arr->getLatitude(), arr->getLongitude());
167 evaluateRoutePart(dep->getLatitude(), dep->getLongitude(),
168 arr->getLatitude(), arr->getLongitude());
174 // if no route could be found, create a direct gps route...
175 cerr << "still no route found from " << dep->getName() << " to << " << arr->getName() <<endl;
179 while(route.next(&node))
181 FGNode *fn = globals->get_airwaynet()->findNode(node);
182 //cerr << "Checking status of each waypoint: " << fn->getIdent();
184 SGWayPoint first(init_waypoint->longitude,
185 init_waypoint->latitude,
187 SGWayPoint curr (fn->getLongitude(),
190 SGWayPoint arr (arr->getLongitude(),
194 double crse, crsDiff;
196 first.CourseAndDistance(arr, &course, &distance);
197 first.CourseAndDistance(curr, &crse, &dist);
199 dist *= SG_METER_TO_NM;
201 // We're only interested in the absolute value of crsDiff
202 // wich should fall in the 0-180 deg range.
203 crsDiff = fabs(crse-course);
205 crsDiff = 360-crsDiff;
206 // These are the three conditions that we consider including
207 // in our flight plan:
208 // 1) current waypoint is less then 100 miles away OR
209 // 2) curren waypoint is ahead of us, at any distance
210 //cerr << " Distance : " << dist << " : Course diff " << crsDiff
211 // << " crs to dest : " << course
212 // << " crs to wpt : " << crse;
213 if ((dist > 20.0) && (crsDiff > 90.0))
216 // Once we start including waypoints, we have to continue, even though
217 // one of the following way point would suffice.
218 // so once is the useWpt flag is set to true, we cannot reset it to false.
219 //cerr << " discarding " << endl;
220 // << ": Course difference = " << crsDiff
221 // << "Course = " << course
222 // << "crse = " << crse << endl;
226 //cerr << " accepting " << endl;
230 wpt->name = "Airway"; // fixme: should be the name of the waypoint
231 wpt->latitude = fn->getLatitude();
232 wpt->longitude = fn->getLongitude();
233 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
235 wpt->speed = 450; //speed;
236 wpt->crossat = -10000;
237 wpt->gear_down = false;
238 wpt->flaps_down= false;
239 wpt->finished = false;
240 wpt->on_ground = false;
241 waypoints.push_back(wpt);
244 if (!(routefile.exists()))
247 fstream outf( routefile.c_str(), fstream::out );
248 while (route.next(&node))
249 outf << node << endl;
253 arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
254 if (!(globals->get_runways()->search(arr->getId(),
258 cout << "Failed to find runway for " << arr->getId() << endl;
259 // Hmm, how do we handle a potential error like this?
263 //arr->getActiveRunway(string("com"), 1, test);
266 //cerr << "Altitude = " << alt << endl;
267 //cerr << "Done" << endl;
268 //if (arr->getId() == "EHAM")
270 // cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
272 heading = rwy._heading;
273 azimuth = heading + 180.0;
274 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
277 // Note: This places us at the location of the active
278 // runway during initial cruise. This needs to be
280 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
282 &lat2, &lon2, &az2 );
284 wpt->name = "BOD"; //wpt_node->getStringValue("name", "END");
285 wpt->latitude = lat2;
286 wpt->longitude = lon2;
290 wpt->gear_down = false;
291 wpt->flaps_down= false;
292 wpt->finished = false;
293 wpt->on_ground = false;
294 waypoints.push_back(wpt);
299 /*******************************************************************
301 * initialize the Aircraft at the parking location
303 * Note that this is the original version that does not
304 * do any dynamic route computation.
305 ******************************************************************/
306 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
307 FGAirport *arr, double latitude,
308 double longitude, double speed,
309 double alt, const string& fltType)
315 double lat2, lon2, az2;
320 wpt->name = "Cruise"; //wpt_node->getStringValue("name", "END");
321 wpt->latitude = latitude;
322 wpt->longitude = longitude;
325 wpt->crossat = -10000;
326 wpt->gear_down = false;
327 wpt->flaps_down= false;
328 wpt->finished = false;
329 wpt->on_ground = false;
331 waypoints.push_back(wpt);
334 string rwyClass = getRunwayClassFromTrafficType(fltType);
335 arr->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway);
336 if (!(globals->get_runways()->search(arr->getId(),
340 SG_LOG(SG_INPUT, SG_ALERT, "Failed to find runway " <<
342 " at airport " << arr->getId()<< " of class " << rwyClass << " (5)");
345 heading = rwy._heading;
346 azimuth = heading + 180.0;
347 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
350 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
352 &lat2, &lon2, &az2 );
355 wpt->latitude = lat2;
356 wpt->longitude = lon2;
360 wpt->gear_down = false;
361 wpt->flaps_down= false;
362 wpt->finished = false;
363 wpt->on_ground = false;
365 waypoints.push_back(wpt);