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 "AIAircraft.hxx"
30 #include <simgear/math/polar3d.hxx>
31 #include <simgear/math/sg_geodesy.hxx>
32 #include <simgear/route/waypoint.hxx>
34 #include <Navaids/awynet.hxx>
35 #include <Airports/runways.hxx>
36 #include <Airports/dynamics.hxx>
38 #include <Environment/environment_mgr.hxx>
39 #include <Environment/environment.hxx>
43 void FGAIFlightPlan::evaluateRoutePart(double deplat,
48 // First do a prescan of all the waypoints that are within a reasonable distance of the
51 int tmpNode, prevNode;
54 SGWayPoint first (deplon,
57 SGWayPoint sec (arrlon,
60 double course, distance;
61 first.CourseAndDistance(sec, &course, &distance);
62 distance *= SG_METER_TO_NM;
64 SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(deplon, deplat, 1));
65 SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arrlon, arrlat, 1));
66 SGVec3d _cross = cross(b, a);
68 double angle = sgACos(dot(a, b));
70 for (double ang = 0.0; ang < angle; ang += 0.05)
74 //cerr << "Angle = " << ang << endl;
75 sgdMakeRotMat4(matrix, ang, _cross.sg());
76 for(int j = 0; j < 3; j++)
79 for (int k = 0; k<3; k++)
81 newPos[j] += matrix[j][k]*a[k];
85 SGGeoc geoc = SGGeoc::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
87 double midlat = geoc.getLatitudeDeg();
88 double midlon = geoc.getLongitudeDeg();
91 tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
93 double nodelat = globals->get_airwaynet()->findNode(tmpNode)->getLatitude ();
94 double nodelon = globals->get_airwaynet()->findNode(tmpNode)->getLongitude ();
95 SGWayPoint curr(midlat,
98 SGWayPoint node(nodelat,
101 curr.CourseAndDistance(node, &course, &distance);
102 if ((distance < 25000) && (tmpNode != prevNode))
103 nodes.push_back(tmpNode);
106 intVecIterator i = nodes.begin();
107 intVecIterator j = nodes.end();
108 while (i != nodes.end())
114 FGAirRoute routePart = globals->get_airwaynet()->findShortestRoute(*i, *j);
115 if (!(routePart.empty()))
117 airRoute.add(routePart);
128 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
129 FGAirport *arr, double latitude,
130 double longitude, double speed, double alt)
132 bool useInitialWayPoint = true;
133 bool useCurrentWayPoint = false;
136 double lat2, lon2, az2;
139 waypoint *init_waypoint;
142 SGPath routefile = globals->get_fg_root();
143 init_waypoint = new waypoint;
144 init_waypoint->name = "Initial waypoint";
145 init_waypoint->latitude = latitude;
146 init_waypoint->longitude = longitude;;
147 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
148 init_waypoint->altitude = alt;
149 init_waypoint->speed = 450; //speed;
150 init_waypoint->crossat = -10000;
151 init_waypoint->gear_down = false;
152 init_waypoint->flaps_down= false;
153 init_waypoint->finished = false;
154 init_waypoint->on_ground = false;
155 waypoints.push_back(init_waypoint);
156 routefile.append("Data/AI/FlightPlans");
157 snprintf(buffer, 32, "%s-%s.txt",
158 dep->getId().c_str(),
159 arr->getId().c_str());
160 routefile.append(buffer);
161 cerr << "trying to read " << routefile.c_str()<< endl;
163 if (routefile.exists())
165 sg_gzifstream in( routefile.str() );
168 } while (!(in.eof()));
171 //int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2);
172 //int startId = globals->get_airwaynet()->findNearestNode(dep->getLatitude(), dep->getLongitude());
173 //int endId = globals->get_airwaynet()->findNearestNode(arr->getLatitude(), arr->getLongitude());
175 evaluateRoutePart(dep->getLatitude(), dep->getLongitude(),
176 arr->getLatitude(), arr->getLongitude());
182 // if no route could be found, create a direct gps route...
183 cerr << "still no route found from " << dep->getName() << " to << " << arr->getName() <<endl;
187 while(route.next(&node))
189 FGNode *fn = globals->get_airwaynet()->findNode(node);
190 //cerr << "Checking status of each waypoint: " << fn->getIdent();
192 SGWayPoint first(init_waypoint->longitude,
193 init_waypoint->latitude,
195 SGWayPoint curr (fn->getLongitude(),
198 SGWayPoint arr (arr->getLongitude(),
202 double crse, crsDiff;
204 first.CourseAndDistance(arr, &course, &distance);
205 first.CourseAndDistance(curr, &crse, &dist);
207 dist *= SG_METER_TO_NM;
209 // We're only interested in the absolute value of crsDiff
210 // wich should fall in the 0-180 deg range.
211 crsDiff = fabs(crse-course);
213 crsDiff = 360-crsDiff;
214 // These are the three conditions that we consider including
215 // in our flight plan:
216 // 1) current waypoint is less then 100 miles away OR
217 // 2) curren waypoint is ahead of us, at any distance
218 //cerr << " Distance : " << dist << " : Course diff " << crsDiff
219 // << " crs to dest : " << course
220 // << " crs to wpt : " << crse;
221 if ((dist > 20.0) && (crsDiff > 90.0))
224 // Once we start including waypoints, we have to continue, even though
225 // one of the following way point would suffice.
226 // so once is the useWpt flag is set to true, we cannot reset it to false.
227 //cerr << " discarding " << endl;
228 // << ": Course difference = " << crsDiff
229 // << "Course = " << course
230 // << "crse = " << crse << endl;
234 //cerr << " accepting " << endl;
238 wpt->name = "Airway"; // fixme: should be the name of the waypoint
239 wpt->latitude = fn->getLatitude();
240 wpt->longitude = fn->getLongitude();
241 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
243 wpt->speed = 450; //speed;
244 wpt->crossat = -10000;
245 wpt->gear_down = false;
246 wpt->flaps_down= false;
247 wpt->finished = false;
248 wpt->on_ground = false;
249 waypoints.push_back(wpt);
252 if (!(routefile.exists()))
255 fstream outf( routefile.c_str(), fstream::out );
256 while (route.next(&node))
257 outf << node << endl;
261 arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
262 if (!(globals->get_runways()->search(arr->getId(),
266 cout << "Failed to find runway for " << arr->getId() << endl;
267 // Hmm, how do we handle a potential error like this?
271 //arr->getActiveRunway(string("com"), 1, test);
274 //cerr << "Altitude = " << alt << endl;
275 //cerr << "Done" << endl;
276 //if (arr->getId() == "EHAM")
278 // cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
280 heading = rwy._heading;
281 azimuth = heading + 180.0;
282 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
285 // Note: This places us at the location of the active
286 // runway during initial cruise. This needs to be
288 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
290 &lat2, &lon2, &az2 );
292 wpt->name = "BOD"; //wpt_node->getStringValue("name", "END");
293 wpt->latitude = lat2;
294 wpt->longitude = lon2;
298 wpt->gear_down = false;
299 wpt->flaps_down= false;
300 wpt->finished = false;
301 wpt->on_ground = false;
302 waypoints.push_back(wpt);
307 /*******************************************************************
309 * initialize the Aircraft at the parking location
311 * Note that this is the original version that does not
312 * do any dynamic route computation.
313 ******************************************************************/
314 void FGAIFlightPlan::createCruise(FGAIAircraft *ac, bool firstFlight, FGAirport *dep,
315 FGAirport *arr, double latitude,
316 double longitude, double speed,
317 double alt, const string& fltType)
320 wpt = createInAir(ac, "Cruise", SGGeod::fromDeg(longitude, latitude), alt, speed);
321 waypoints.push_back(wpt);
323 string rwyClass = getRunwayClassFromTrafficType(fltType);
324 double heading = ac->getTrafficRef()->getCourse();
325 arr->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway, heading);
326 rwy = arr->getRunwayByIdent(activeRunway);
327 // begin descent 110km out
328 SGGeod beginDescentPoint = rwy->pointOnCenterline(-110000);
330 wpt = createInAir(ac, "BOD", beginDescentPoint, alt, speed);
331 waypoints.push_back(wpt);