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"
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;
53 SGGeoc dep(SGGeoc::fromDegM(deplon, deplat, 100.0));
54 SGGeoc arr(SGGeoc::fromDegM(arrlon, arrlat, 100.0));
56 SGVec3d a = SGVec3d::fromGeoc(dep);
57 SGVec3d nb = normalize(SGVec3d::fromGeoc(arr));
58 SGVec3d na = normalize(a);
60 SGVec3d _cross = cross(nb, na);
62 double angle = acos(dot(na, nb));
63 const double angleStep = 0.05 * SG_DEGREES_TO_RADIANS;
65 for (double ang = 0.0; ang < angle; ang += angleStep)
67 SGQuatd q = SGQuatd::fromAngleAxis(ang, _cross);
68 SGGeod geod = SGGeod::fromCart(q.transform(a));
71 tmpNode = globals->get_airwaynet()->findNearestNode(geod);
73 FGNode* node = globals->get_airwaynet()->findNode(tmpNode);
75 if ((tmpNode != prevNode) && (SGGeodesy::distanceM(geod, node->getPosition()) < 25000)) {
76 nodes.push_back(tmpNode);
80 intVecIterator i = nodes.begin();
81 intVecIterator j = nodes.end();
82 while (i != nodes.end())
88 FGAirRoute routePart = globals->get_airwaynet()->findShortestRoute(*i, *j);
89 if (!(routePart.empty()))
91 airRoute.add(routePart);
102 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
103 FGAirport *arr, double latitude,
104 double longitude, double speed, double alt)
106 bool useInitialWayPoint = true;
107 bool useCurrentWayPoint = false;
110 double lat2, lon2, az2;
113 waypoint *init_waypoint;
116 SGPath routefile = globals->get_fg_root();
117 init_waypoint = new waypoint;
118 init_waypoint->name = "Initial waypoint";
119 init_waypoint->latitude = latitude;
120 init_waypoint->longitude = longitude;;
121 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
122 init_waypoint->altitude = alt;
123 init_waypoint->speed = 450; //speed;
124 init_waypoint->crossat = -10000;
125 init_waypoint->gear_down = false;
126 init_waypoint->flaps_down= false;
127 init_waypoint->finished = false;
128 init_waypoint->on_ground = false;
129 waypoints.push_back(init_waypoint);
130 routefile.append("Data/AI/FlightPlans");
131 snprintf(buffer, 32, "%s-%s.txt",
132 dep->getId().c_str(),
133 arr->getId().c_str());
134 routefile.append(buffer);
135 cerr << "trying to read " << routefile.c_str()<< endl;
137 if (routefile.exists())
139 sg_gzifstream in( routefile.str() );
142 } while (!(in.eof()));
145 //int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2);
146 //int startId = globals->get_airwaynet()->findNearestNode(dep->getLatitude(), dep->getLongitude());
147 //int endId = globals->get_airwaynet()->findNearestNode(arr->getLatitude(), arr->getLongitude());
149 evaluateRoutePart(dep->getLatitude(), dep->getLongitude(),
150 arr->getLatitude(), arr->getLongitude());
156 // if no route could be found, create a direct gps route...
157 cerr << "still no route found from " << dep->getName() << " to << " << arr->getName() <<endl;
161 while(route.next(&node))
163 FGNode *fn = globals->get_airwaynet()->findNode(node);
164 //cerr << "Checking status of each waypoint: " << fn->getIdent();
166 SGWayPoint first(init_waypoint->longitude,
167 init_waypoint->latitude,
169 SGWayPoint curr (fn->getLongitude(),
172 SGWayPoint arr (arr->getLongitude(),
176 double crse, crsDiff;
178 first.CourseAndDistance(arr, &course, &distance);
179 first.CourseAndDistance(curr, &crse, &dist);
181 dist *= SG_METER_TO_NM;
183 // We're only interested in the absolute value of crsDiff
184 // wich should fall in the 0-180 deg range.
185 crsDiff = fabs(crse-course);
187 crsDiff = 360-crsDiff;
188 // These are the three conditions that we consider including
189 // in our flight plan:
190 // 1) current waypoint is less then 100 miles away OR
191 // 2) curren waypoint is ahead of us, at any distance
192 //cerr << " Distance : " << dist << " : Course diff " << crsDiff
193 // << " crs to dest : " << course
194 // << " crs to wpt : " << crse;
195 if ((dist > 20.0) && (crsDiff > 90.0))
198 // Once we start including waypoints, we have to continue, even though
199 // one of the following way point would suffice.
200 // so once is the useWpt flag is set to true, we cannot reset it to false.
201 //cerr << " discarding " << endl;
202 // << ": Course difference = " << crsDiff
203 // << "Course = " << course
204 // << "crse = " << crse << endl;
208 //cerr << " accepting " << endl;
212 wpt->name = "Airway"; // fixme: should be the name of the waypoint
213 wpt->latitude = fn->getLatitude();
214 wpt->longitude = fn->getLongitude();
215 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
217 wpt->speed = 450; //speed;
218 wpt->crossat = -10000;
219 wpt->gear_down = false;
220 wpt->flaps_down= false;
221 wpt->finished = false;
222 wpt->on_ground = false;
223 waypoints.push_back(wpt);
226 if (!(routefile.exists()))
229 fstream outf( routefile.c_str(), fstream::out );
230 while (route.next(&node))
231 outf << node << endl;
235 arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
236 if (!(globals->get_runways()->search(arr->getId(),
240 cout << "Failed to find runway for " << arr->getId() << endl;
241 // Hmm, how do we handle a potential error like this?
245 //arr->getActiveRunway(string("com"), 1, test);
248 //cerr << "Altitude = " << alt << endl;
249 //cerr << "Done" << endl;
250 //if (arr->getId() == "EHAM")
252 // cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
254 heading = rwy._heading;
255 azimuth = heading + 180.0;
256 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
259 // Note: This places us at the location of the active
260 // runway during initial cruise. This needs to be
262 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
264 &lat2, &lon2, &az2 );
266 wpt->name = "BOD"; //wpt_node->getStringValue("name", "END");
267 wpt->latitude = lat2;
268 wpt->longitude = lon2;
272 wpt->gear_down = false;
273 wpt->flaps_down= false;
274 wpt->finished = false;
275 wpt->on_ground = false;
276 waypoints.push_back(wpt);
281 /*******************************************************************
283 * initialize the Aircraft at the parking location
285 * Note that this is the original version that does not
286 * do any dynamic route computation.
287 ******************************************************************/
288 void FGAIFlightPlan::createCruise(FGAIAircraft *ac, bool firstFlight, FGAirport *dep,
289 FGAirport *arr, double latitude,
290 double longitude, double speed,
291 double alt, const string& fltType)
293 double vCruise = ac->getPerformance()->vCruise();
295 wpt = createInAir(ac, "Cruise", SGGeod::fromDeg(longitude, latitude), alt, vCruise);
296 waypoints.push_back(wpt);
298 string rwyClass = getRunwayClassFromTrafficType(fltType);
299 double heading = ac->getTrafficRef()->getCourse();
300 arr->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway, heading);
301 rwy = arr->getRunwayByIdent(activeRunway);
302 // begin descent 110km out
303 SGGeod beginDescentPoint = rwy->pointOnCenterline(-110000);
305 wpt = createInAir(ac, "BOD", beginDescentPoint, alt, vCruise);
306 waypoints.push_back(wpt);