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 **************************************************************************/
28 #include "AIFlightPlan.hxx"
29 #include <simgear/math/polar3d.hxx>
30 #include <simgear/math/sg_geodesy.hxx>
31 #include <simgear/route/waypoint.hxx>
33 #include <Navaids/awynet.hxx>
34 #include <Airports/runways.hxx>
35 #include <Airports/dynamics.hxx>
37 #include <Environment/environment_mgr.hxx>
38 #include <Environment/environment.hxx>
42 void FGAIFlightPlan::evaluateRoutePart(double deplat,
47 // First do a prescan of all the waypoints that are within a reasonable distance of the
50 int tmpNode, prevNode;
53 SGWayPoint first (deplon,
56 SGWayPoint sec (arrlon,
59 double course, distance;
60 first.CourseAndDistance(sec, &course, &distance);
61 distance *= SG_METER_TO_NM;
63 SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(deplon, deplat, 1));
64 SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arrlon, arrlat, 1));
65 SGVec3d _cross = cross(b, a);
67 double angle = sgACos(dot(a, b));
69 for (double ang = 0.0; ang < angle; ang += 0.05)
73 //cerr << "Angle = " << ang << endl;
74 sgdMakeRotMat4(matrix, ang, _cross.sg());
75 for(int j = 0; j < 3; j++)
78 for (int k = 0; k<3; k++)
80 newPos[j] += matrix[j][k]*a[k];
84 SGGeoc geoc = SGGeoc::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
86 double midlat = geoc.getLatitudeDeg();
87 double midlon = geoc.getLongitudeDeg();
90 tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
92 double nodelat = globals->get_airwaynet()->findNode(tmpNode)->getLatitude ();
93 double nodelon = globals->get_airwaynet()->findNode(tmpNode)->getLongitude ();
94 SGWayPoint curr(midlat,
97 SGWayPoint node(nodelat,
100 curr.CourseAndDistance(node, &course, &distance);
101 if ((distance < 25000) && (tmpNode != prevNode))
102 nodes.push_back(tmpNode);
105 intVecIterator i = nodes.begin();
106 intVecIterator j = nodes.end();
107 while (i != nodes.end())
113 FGAirRoute routePart = globals->get_airwaynet()->findShortestRoute(*i, *j);
114 if (!(routePart.empty()))
116 airRoute.add(routePart);
127 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
128 FGAirport *arr, double latitude,
129 double longitude, double speed, double alt)
131 bool useInitialWayPoint = true;
132 bool useCurrentWayPoint = false;
135 double lat2, lon2, az2;
138 waypoint *init_waypoint;
141 SGPath routefile = globals->get_fg_root();
142 init_waypoint = new waypoint;
143 init_waypoint->name = "Initial waypoint";
144 init_waypoint->latitude = latitude;
145 init_waypoint->longitude = longitude;;
146 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
147 init_waypoint->altitude = alt;
148 init_waypoint->speed = 450; //speed;
149 init_waypoint->crossat = -10000;
150 init_waypoint->gear_down = false;
151 init_waypoint->flaps_down= false;
152 init_waypoint->finished = false;
153 init_waypoint->on_ground = false;
154 waypoints.push_back(init_waypoint);
155 routefile.append("Data/AI/FlightPlans");
156 snprintf(buffer, 32, "%s-%s.txt",
157 dep->getId().c_str(),
158 arr->getId().c_str());
159 routefile.append(buffer);
160 cerr << "trying to read " << routefile.c_str()<< endl;
162 if (routefile.exists())
164 sg_gzifstream in( routefile.str() );
167 } while (!(in.eof()));
170 //int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2);
171 //int startId = globals->get_airwaynet()->findNearestNode(dep->getLatitude(), dep->getLongitude());
172 //int endId = globals->get_airwaynet()->findNearestNode(arr->getLatitude(), arr->getLongitude());
174 evaluateRoutePart(dep->getLatitude(), dep->getLongitude(),
175 arr->getLatitude(), arr->getLongitude());
181 // if no route could be found, create a direct gps route...
182 cerr << "still no route found from " << dep->getName() << " to << " << arr->getName() <<endl;
186 while(route.next(&node))
188 FGNode *fn = globals->get_airwaynet()->findNode(node);
189 //cerr << "Checking status of each waypoint: " << fn->getIdent();
191 SGWayPoint first(init_waypoint->longitude,
192 init_waypoint->latitude,
194 SGWayPoint curr (fn->getLongitude(),
197 SGWayPoint arr (arr->getLongitude(),
201 double crse, crsDiff;
203 first.CourseAndDistance(arr, &course, &distance);
204 first.CourseAndDistance(curr, &crse, &dist);
206 dist *= SG_METER_TO_NM;
208 // We're only interested in the absolute value of crsDiff
209 // wich should fall in the 0-180 deg range.
210 crsDiff = fabs(crse-course);
212 crsDiff = 360-crsDiff;
213 // These are the three conditions that we consider including
214 // in our flight plan:
215 // 1) current waypoint is less then 100 miles away OR
216 // 2) curren waypoint is ahead of us, at any distance
217 //cerr << " Distance : " << dist << " : Course diff " << crsDiff
218 // << " crs to dest : " << course
219 // << " crs to wpt : " << crse;
220 if ((dist > 20.0) && (crsDiff > 90.0))
223 // Once we start including waypoints, we have to continue, even though
224 // one of the following way point would suffice.
225 // so once is the useWpt flag is set to true, we cannot reset it to false.
226 //cerr << " discarding " << endl;
227 // << ": Course difference = " << crsDiff
228 // << "Course = " << course
229 // << "crse = " << crse << endl;
233 //cerr << " accepting " << endl;
237 wpt->name = "Airway"; // fixme: should be the name of the waypoint
238 wpt->latitude = fn->getLatitude();
239 wpt->longitude = fn->getLongitude();
240 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
242 wpt->speed = 450; //speed;
243 wpt->crossat = -10000;
244 wpt->gear_down = false;
245 wpt->flaps_down= false;
246 wpt->finished = false;
247 wpt->on_ground = false;
248 waypoints.push_back(wpt);
251 if (!(routefile.exists()))
254 fstream outf( routefile.c_str(), fstream::out );
255 while (route.next(&node))
256 outf << node << endl;
260 arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
261 if (!(globals->get_runways()->search(arr->getId(),
265 cout << "Failed to find runway for " << arr->getId() << endl;
266 // Hmm, how do we handle a potential error like this?
270 //arr->getActiveRunway(string("com"), 1, test);
273 //cerr << "Altitude = " << alt << endl;
274 //cerr << "Done" << endl;
275 //if (arr->getId() == "EHAM")
277 // cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
279 heading = rwy._heading;
280 azimuth = heading + 180.0;
281 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
284 // Note: This places us at the location of the active
285 // runway during initial cruise. This needs to be
287 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
289 &lat2, &lon2, &az2 );
291 wpt->name = "BOD"; //wpt_node->getStringValue("name", "END");
292 wpt->latitude = lat2;
293 wpt->longitude = lon2;
297 wpt->gear_down = false;
298 wpt->flaps_down= false;
299 wpt->finished = false;
300 wpt->on_ground = false;
301 waypoints.push_back(wpt);
306 /*******************************************************************
308 * initialize the Aircraft at the parking location
310 * Note that this is the original version that does not
311 * do any dynamic route computation.
312 ******************************************************************/
313 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
314 FGAirport *arr, double latitude,
315 double longitude, double speed,
316 double alt, const string& fltType)
319 wpt = createInAir("Cruise", SGGeod::fromDeg(longitude, latitude), alt, speed);
320 waypoints.push_back(wpt);
322 string rwyClass = getRunwayClassFromTrafficType(fltType);
323 arr->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway);
324 rwy = arr->getRunwayByIdent(activeRunway);
325 // begin descent 110km out
326 SGGeod beginDescentPoint = rwy->pointOnCenterline(-110000);
328 wpt = createInAir("BOD", beginDescentPoint, alt, speed);
329 waypoints.push_back(wpt);