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/route/waypoint.hxx>
32 #include <Navaids/awynet.hxx>
33 #include <Airports/runways.hxx>
34 #include <Airports/dynamics.hxx>
36 #include <Environment/environment_mgr.hxx>
37 #include <Environment/environment.hxx>
41 void FGAIFlightPlan::evaluateRoutePart(double deplat,
46 // First do a prescan of all the waypoints that are within a reasonable distance of the
49 int tmpNode, prevNode;
52 SGWayPoint first (deplon,
55 SGWayPoint sec (arrlon,
58 double course, distance;
59 first.CourseAndDistance(sec, &course, &distance);
60 distance *= SG_METER_TO_NM;
62 SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(deplon, deplat, 1));
63 SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arrlon, arrlat, 1));
64 SGVec3d _cross = cross(b, a);
66 double angle = sgACos(dot(a, b));
68 for (double ang = 0.0; ang < angle; ang += 0.05)
72 //cerr << "Angle = " << ang << endl;
73 sgdMakeRotMat4(matrix, ang, _cross.sg());
74 for(int j = 0; j < 3; j++)
77 for (int k = 0; k<3; k++)
79 newPos[j] += matrix[j][k]*a[k];
83 SGGeoc geoc = SGGeoc::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
85 double midlat = geoc.getLatitudeDeg();
86 double midlon = geoc.getLongitudeDeg();
89 tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
91 double nodelat = globals->get_airwaynet()->findNode(tmpNode)->getLatitude ();
92 double nodelon = globals->get_airwaynet()->findNode(tmpNode)->getLongitude ();
93 SGWayPoint curr(midlat,
96 SGWayPoint node(nodelat,
99 curr.CourseAndDistance(node, &course, &distance);
100 if ((distance < 25000) && (tmpNode != prevNode))
101 nodes.push_back(tmpNode);
104 intVecIterator i = nodes.begin();
105 intVecIterator j = nodes.end();
106 while (i != nodes.end())
112 FGAirRoute routePart = globals->get_airwaynet()->findShortestRoute(*i, *j);
113 if (!(routePart.empty()))
115 airRoute.add(routePart);
126 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
127 FGAirport *arr, double latitude,
128 double longitude, double speed, double alt)
130 bool useInitialWayPoint = true;
131 bool useCurrentWayPoint = false;
134 double lat2, lon2, az2;
137 waypoint *init_waypoint;
140 SGPath routefile = globals->get_fg_root();
141 init_waypoint = new waypoint;
142 init_waypoint->name = "Initial waypoint";
143 init_waypoint->latitude = latitude;
144 init_waypoint->longitude = longitude;;
145 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
146 init_waypoint->altitude = alt;
147 init_waypoint->speed = 450; //speed;
148 init_waypoint->crossat = -10000;
149 init_waypoint->gear_down = false;
150 init_waypoint->flaps_down= false;
151 init_waypoint->finished = false;
152 init_waypoint->on_ground = false;
153 waypoints.push_back(init_waypoint);
154 routefile.append("Data/AI/FlightPlans");
155 snprintf(buffer, 32, "%s-%s.txt",
156 dep->getId().c_str(),
157 arr->getId().c_str());
158 routefile.append(buffer);
159 cerr << "trying to read " << routefile.c_str()<< endl;
161 if (routefile.exists())
163 sg_gzifstream in( routefile.str() );
166 } while (!(in.eof()));
169 //int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2);
170 //int startId = globals->get_airwaynet()->findNearestNode(dep->getLatitude(), dep->getLongitude());
171 //int endId = globals->get_airwaynet()->findNearestNode(arr->getLatitude(), arr->getLongitude());
173 evaluateRoutePart(dep->getLatitude(), dep->getLongitude(),
174 arr->getLatitude(), arr->getLongitude());
180 // if no route could be found, create a direct gps route...
181 cerr << "still no route found from " << dep->getName() << " to << " << arr->getName() <<endl;
185 while(route.next(&node))
187 FGNode *fn = globals->get_airwaynet()->findNode(node);
188 //cerr << "Checking status of each waypoint: " << fn->getIdent();
190 SGWayPoint first(init_waypoint->longitude,
191 init_waypoint->latitude,
193 SGWayPoint curr (fn->getLongitude(),
196 SGWayPoint arr (arr->getLongitude(),
200 double crse, crsDiff;
202 first.CourseAndDistance(arr, &course, &distance);
203 first.CourseAndDistance(curr, &crse, &dist);
205 dist *= SG_METER_TO_NM;
207 // We're only interested in the absolute value of crsDiff
208 // wich should fall in the 0-180 deg range.
209 crsDiff = fabs(crse-course);
211 crsDiff = 360-crsDiff;
212 // These are the three conditions that we consider including
213 // in our flight plan:
214 // 1) current waypoint is less then 100 miles away OR
215 // 2) curren waypoint is ahead of us, at any distance
216 //cerr << " Distance : " << dist << " : Course diff " << crsDiff
217 // << " crs to dest : " << course
218 // << " crs to wpt : " << crse;
219 if ((dist > 20.0) && (crsDiff > 90.0))
222 // Once we start including waypoints, we have to continue, even though
223 // one of the following way point would suffice.
224 // so once is the useWpt flag is set to true, we cannot reset it to false.
225 //cerr << " discarding " << endl;
226 // << ": Course difference = " << crsDiff
227 // << "Course = " << course
228 // << "crse = " << crse << endl;
232 //cerr << " accepting " << endl;
236 wpt->name = "Airway"; // fixme: should be the name of the waypoint
237 wpt->latitude = fn->getLatitude();
238 wpt->longitude = fn->getLongitude();
239 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
241 wpt->speed = 450; //speed;
242 wpt->crossat = -10000;
243 wpt->gear_down = false;
244 wpt->flaps_down= false;
245 wpt->finished = false;
246 wpt->on_ground = false;
247 waypoints.push_back(wpt);
250 if (!(routefile.exists()))
253 fstream outf( routefile.c_str(), fstream::out );
254 while (route.next(&node))
255 outf << node << endl;
259 arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
260 if (!(globals->get_runways()->search(arr->getId(),
264 cout << "Failed to find runway for " << arr->getId() << endl;
265 // Hmm, how do we handle a potential error like this?
269 //arr->getActiveRunway(string("com"), 1, test);
272 //cerr << "Altitude = " << alt << endl;
273 //cerr << "Done" << endl;
274 //if (arr->getId() == "EHAM")
276 // cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
278 heading = rwy._heading;
279 azimuth = heading + 180.0;
280 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
283 // Note: This places us at the location of the active
284 // runway during initial cruise. This needs to be
286 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
288 &lat2, &lon2, &az2 );
290 wpt->name = "BOD"; //wpt_node->getStringValue("name", "END");
291 wpt->latitude = lat2;
292 wpt->longitude = lon2;
296 wpt->gear_down = false;
297 wpt->flaps_down= false;
298 wpt->finished = false;
299 wpt->on_ground = false;
300 waypoints.push_back(wpt);
305 /*******************************************************************
307 * initialize the Aircraft at the parking location
309 * Note that this is the original version that does not
310 * do any dynamic route computation.
311 ******************************************************************/
312 void FGAIFlightPlan::createCruise(FGAIAircraft *ac, bool firstFlight, FGAirport *dep,
313 FGAirport *arr, double latitude,
314 double longitude, double speed,
315 double alt, const string& fltType)
318 wpt = createInAir(ac, "Cruise", SGGeod::fromDeg(longitude, latitude), alt, speed);
319 waypoints.push_back(wpt);
321 string rwyClass = getRunwayClassFromTrafficType(fltType);
322 double heading = ac->getTrafficRef()->getCourse();
323 arr->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway, heading);
324 rwy = arr->getRunwayByIdent(activeRunway);
325 // begin descent 110km out
326 SGGeod beginDescentPoint = rwy->pointOnCenterline(-110000);
328 wpt = createInAir(ac, "BOD", beginDescentPoint, alt, speed);
329 waypoints.push_back(wpt);