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 <simgear/route/waypoint.hxx>
30 #include <Navaids/awynet.hxx>
31 #include <Airports/runways.hxx>
32 #include <Airports/dynamics.hxx>
34 #include <Environment/environment_mgr.hxx>
35 #include <Environment/environment.hxx>
37 #include "AIFlightPlan.hxx"
38 #include "AIAircraft.hxx"
39 #include "performancedata.hxx"
44 void FGAIFlightPlan::evaluateRoutePart(double deplat,
49 // First do a prescan of all the waypoints that are within a reasonable distance of the
52 int tmpNode, prevNode;
55 SGWayPoint first (deplon,
58 SGWayPoint sec (arrlon,
61 double course, distance;
62 first.CourseAndDistance(sec, &course, &distance);
63 distance *= SG_METER_TO_NM;
65 SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(deplon, deplat, 1));
66 SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arrlon, arrlat, 1));
67 SGVec3d _cross = cross(b, a);
69 double angle = sgACos(dot(a, b));
71 for (double ang = 0.0; ang < angle; ang += 0.05)
75 //cerr << "Angle = " << ang << endl;
76 sgdMakeRotMat4(matrix, ang, _cross.sg());
77 for(int j = 0; j < 3; j++)
80 for (int k = 0; k<3; k++)
82 newPos[j] += matrix[j][k]*a[k];
86 SGGeoc geoc = SGGeoc::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
88 double midlat = geoc.getLatitudeDeg();
89 double midlon = geoc.getLongitudeDeg();
92 tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
94 double nodelat = globals->get_airwaynet()->findNode(tmpNode)->getLatitude ();
95 double nodelon = globals->get_airwaynet()->findNode(tmpNode)->getLongitude ();
96 SGWayPoint curr(midlat,
99 SGWayPoint node(nodelat,
102 curr.CourseAndDistance(node, &course, &distance);
103 if ((distance < 25000) && (tmpNode != prevNode))
104 nodes.push_back(tmpNode);
107 intVecIterator i = nodes.begin();
108 intVecIterator j = nodes.end();
109 while (i != nodes.end())
115 FGAirRoute routePart = globals->get_airwaynet()->findShortestRoute(*i, *j);
116 if (!(routePart.empty()))
118 airRoute.add(routePart);
129 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
130 FGAirport *arr, double latitude,
131 double longitude, double speed, double alt)
133 bool useInitialWayPoint = true;
134 bool useCurrentWayPoint = false;
137 double lat2, lon2, az2;
140 waypoint *init_waypoint;
143 SGPath routefile = globals->get_fg_root();
144 init_waypoint = new waypoint;
145 init_waypoint->name = "Initial waypoint";
146 init_waypoint->latitude = latitude;
147 init_waypoint->longitude = longitude;;
148 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
149 init_waypoint->altitude = alt;
150 init_waypoint->speed = 450; //speed;
151 init_waypoint->crossat = -10000;
152 init_waypoint->gear_down = false;
153 init_waypoint->flaps_down= false;
154 init_waypoint->finished = false;
155 init_waypoint->on_ground = false;
156 waypoints.push_back(init_waypoint);
157 routefile.append("Data/AI/FlightPlans");
158 snprintf(buffer, 32, "%s-%s.txt",
159 dep->getId().c_str(),
160 arr->getId().c_str());
161 routefile.append(buffer);
162 cerr << "trying to read " << routefile.c_str()<< endl;
164 if (routefile.exists())
166 sg_gzifstream in( routefile.str() );
169 } while (!(in.eof()));
172 //int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2);
173 //int startId = globals->get_airwaynet()->findNearestNode(dep->getLatitude(), dep->getLongitude());
174 //int endId = globals->get_airwaynet()->findNearestNode(arr->getLatitude(), arr->getLongitude());
176 evaluateRoutePart(dep->getLatitude(), dep->getLongitude(),
177 arr->getLatitude(), arr->getLongitude());
183 // if no route could be found, create a direct gps route...
184 cerr << "still no route found from " << dep->getName() << " to << " << arr->getName() <<endl;
188 while(route.next(&node))
190 FGNode *fn = globals->get_airwaynet()->findNode(node);
191 //cerr << "Checking status of each waypoint: " << fn->getIdent();
193 SGWayPoint first(init_waypoint->longitude,
194 init_waypoint->latitude,
196 SGWayPoint curr (fn->getLongitude(),
199 SGWayPoint arr (arr->getLongitude(),
203 double crse, crsDiff;
205 first.CourseAndDistance(arr, &course, &distance);
206 first.CourseAndDistance(curr, &crse, &dist);
208 dist *= SG_METER_TO_NM;
210 // We're only interested in the absolute value of crsDiff
211 // wich should fall in the 0-180 deg range.
212 crsDiff = fabs(crse-course);
214 crsDiff = 360-crsDiff;
215 // These are the three conditions that we consider including
216 // in our flight plan:
217 // 1) current waypoint is less then 100 miles away OR
218 // 2) curren waypoint is ahead of us, at any distance
219 //cerr << " Distance : " << dist << " : Course diff " << crsDiff
220 // << " crs to dest : " << course
221 // << " crs to wpt : " << crse;
222 if ((dist > 20.0) && (crsDiff > 90.0))
225 // Once we start including waypoints, we have to continue, even though
226 // one of the following way point would suffice.
227 // so once is the useWpt flag is set to true, we cannot reset it to false.
228 //cerr << " discarding " << endl;
229 // << ": Course difference = " << crsDiff
230 // << "Course = " << course
231 // << "crse = " << crse << endl;
235 //cerr << " accepting " << endl;
239 wpt->name = "Airway"; // fixme: should be the name of the waypoint
240 wpt->latitude = fn->getLatitude();
241 wpt->longitude = fn->getLongitude();
242 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
244 wpt->speed = 450; //speed;
245 wpt->crossat = -10000;
246 wpt->gear_down = false;
247 wpt->flaps_down= false;
248 wpt->finished = false;
249 wpt->on_ground = false;
250 waypoints.push_back(wpt);
253 if (!(routefile.exists()))
256 fstream outf( routefile.c_str(), fstream::out );
257 while (route.next(&node))
258 outf << node << endl;
262 arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
263 if (!(globals->get_runways()->search(arr->getId(),
267 cout << "Failed to find runway for " << arr->getId() << endl;
268 // Hmm, how do we handle a potential error like this?
272 //arr->getActiveRunway(string("com"), 1, test);
275 //cerr << "Altitude = " << alt << endl;
276 //cerr << "Done" << endl;
277 //if (arr->getId() == "EHAM")
279 // cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
281 heading = rwy._heading;
282 azimuth = heading + 180.0;
283 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
286 // Note: This places us at the location of the active
287 // runway during initial cruise. This needs to be
289 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
291 &lat2, &lon2, &az2 );
293 wpt->name = "BOD"; //wpt_node->getStringValue("name", "END");
294 wpt->latitude = lat2;
295 wpt->longitude = lon2;
299 wpt->gear_down = false;
300 wpt->flaps_down= false;
301 wpt->finished = false;
302 wpt->on_ground = false;
303 waypoints.push_back(wpt);
308 /*******************************************************************
310 * initialize the Aircraft at the parking location
312 * Note that this is the original version that does not
313 * do any dynamic route computation.
314 ******************************************************************/
315 void FGAIFlightPlan::createCruise(FGAIAircraft *ac, bool firstFlight, FGAirport *dep,
316 FGAirport *arr, double latitude,
317 double longitude, double speed,
318 double alt, const string& fltType)
320 double vCruise = ac->getPerformance()->vCruise();
322 wpt = createInAir(ac, "Cruise", SGGeod::fromDeg(longitude, latitude), alt, vCruise);
323 waypoints.push_back(wpt);
325 string rwyClass = getRunwayClassFromTrafficType(fltType);
326 double heading = ac->getTrafficRef()->getCourse();
327 arr->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway, heading);
328 rwy = arr->getRunwayByIdent(activeRunway);
329 // begin descent 110km out
330 SGGeod beginDescentPoint = rwy->pointOnCenterline(-110000);
332 wpt = createInAir(ac, "BOD", beginDescentPoint, alt, vCruise);
333 waypoints.push_back(wpt);