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;
54 SGGeoc dep(SGGeoc::fromDegM(deplon, deplat, 100.0));
55 SGGeoc arr(SGGeoc::fromDegM(arrlon, arrlat, 100.0));
57 SGVec3d a = SGVec3d::fromGeoc(dep);
58 SGVec3d b = SGVec3d::fromGeoc(arr);
59 SGVec3d _cross = cross(b, a);
61 double angle = sgACos(dot(a, b));
63 for (double ang = 0.0; ang < angle; ang += 0.05)
67 //cerr << "Angle = " << ang << endl;
68 sgdMakeRotMat4(matrix, ang, _cross.data());
69 for(int j = 0; j < 3; j++)
72 for (int k = 0; k<3; k++)
74 newPos[j] += matrix[j][k]*a[k];
79 SGGeoc geoc = SGGeoc::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
81 double midlat = geoc.getLatitudeDeg();
82 double midlon = geoc.getLongitudeDeg();
85 tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
87 FGNode* node = globals->get_airwaynet()->findNode(tmpNode);
88 SGGeoc nodePos(SGGeoc::fromGeod(node->getPosition()));
90 if ((tmpNode != prevNode) && (SGGeodesy::distanceM(geoc, nodePos) < 25000)) {
91 nodes.push_back(tmpNode);
95 intVecIterator i = nodes.begin();
96 intVecIterator j = nodes.end();
97 while (i != nodes.end())
103 FGAirRoute routePart = globals->get_airwaynet()->findShortestRoute(*i, *j);
104 if (!(routePart.empty()))
106 airRoute.add(routePart);
117 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
118 FGAirport *arr, double latitude,
119 double longitude, double speed, double alt)
121 bool useInitialWayPoint = true;
122 bool useCurrentWayPoint = false;
125 double lat2, lon2, az2;
128 waypoint *init_waypoint;
131 SGPath routefile = globals->get_fg_root();
132 init_waypoint = new waypoint;
133 init_waypoint->name = "Initial waypoint";
134 init_waypoint->latitude = latitude;
135 init_waypoint->longitude = longitude;;
136 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
137 init_waypoint->altitude = alt;
138 init_waypoint->speed = 450; //speed;
139 init_waypoint->crossat = -10000;
140 init_waypoint->gear_down = false;
141 init_waypoint->flaps_down= false;
142 init_waypoint->finished = false;
143 init_waypoint->on_ground = false;
144 waypoints.push_back(init_waypoint);
145 routefile.append("Data/AI/FlightPlans");
146 snprintf(buffer, 32, "%s-%s.txt",
147 dep->getId().c_str(),
148 arr->getId().c_str());
149 routefile.append(buffer);
150 cerr << "trying to read " << routefile.c_str()<< endl;
152 if (routefile.exists())
154 sg_gzifstream in( routefile.str() );
157 } while (!(in.eof()));
160 //int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2);
161 //int startId = globals->get_airwaynet()->findNearestNode(dep->getLatitude(), dep->getLongitude());
162 //int endId = globals->get_airwaynet()->findNearestNode(arr->getLatitude(), arr->getLongitude());
164 evaluateRoutePart(dep->getLatitude(), dep->getLongitude(),
165 arr->getLatitude(), arr->getLongitude());
171 // if no route could be found, create a direct gps route...
172 cerr << "still no route found from " << dep->getName() << " to << " << arr->getName() <<endl;
176 while(route.next(&node))
178 FGNode *fn = globals->get_airwaynet()->findNode(node);
179 //cerr << "Checking status of each waypoint: " << fn->getIdent();
181 SGWayPoint first(init_waypoint->longitude,
182 init_waypoint->latitude,
184 SGWayPoint curr (fn->getLongitude(),
187 SGWayPoint arr (arr->getLongitude(),
191 double crse, crsDiff;
193 first.CourseAndDistance(arr, &course, &distance);
194 first.CourseAndDistance(curr, &crse, &dist);
196 dist *= SG_METER_TO_NM;
198 // We're only interested in the absolute value of crsDiff
199 // wich should fall in the 0-180 deg range.
200 crsDiff = fabs(crse-course);
202 crsDiff = 360-crsDiff;
203 // These are the three conditions that we consider including
204 // in our flight plan:
205 // 1) current waypoint is less then 100 miles away OR
206 // 2) curren waypoint is ahead of us, at any distance
207 //cerr << " Distance : " << dist << " : Course diff " << crsDiff
208 // << " crs to dest : " << course
209 // << " crs to wpt : " << crse;
210 if ((dist > 20.0) && (crsDiff > 90.0))
213 // Once we start including waypoints, we have to continue, even though
214 // one of the following way point would suffice.
215 // so once is the useWpt flag is set to true, we cannot reset it to false.
216 //cerr << " discarding " << endl;
217 // << ": Course difference = " << crsDiff
218 // << "Course = " << course
219 // << "crse = " << crse << endl;
223 //cerr << " accepting " << endl;
227 wpt->name = "Airway"; // fixme: should be the name of the waypoint
228 wpt->latitude = fn->getLatitude();
229 wpt->longitude = fn->getLongitude();
230 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
232 wpt->speed = 450; //speed;
233 wpt->crossat = -10000;
234 wpt->gear_down = false;
235 wpt->flaps_down= false;
236 wpt->finished = false;
237 wpt->on_ground = false;
238 waypoints.push_back(wpt);
241 if (!(routefile.exists()))
244 fstream outf( routefile.c_str(), fstream::out );
245 while (route.next(&node))
246 outf << node << endl;
250 arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
251 if (!(globals->get_runways()->search(arr->getId(),
255 cout << "Failed to find runway for " << arr->getId() << endl;
256 // Hmm, how do we handle a potential error like this?
260 //arr->getActiveRunway(string("com"), 1, test);
263 //cerr << "Altitude = " << alt << endl;
264 //cerr << "Done" << endl;
265 //if (arr->getId() == "EHAM")
267 // cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
269 heading = rwy._heading;
270 azimuth = heading + 180.0;
271 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
274 // Note: This places us at the location of the active
275 // runway during initial cruise. This needs to be
277 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
279 &lat2, &lon2, &az2 );
281 wpt->name = "BOD"; //wpt_node->getStringValue("name", "END");
282 wpt->latitude = lat2;
283 wpt->longitude = lon2;
287 wpt->gear_down = false;
288 wpt->flaps_down= false;
289 wpt->finished = false;
290 wpt->on_ground = false;
291 waypoints.push_back(wpt);
296 /*******************************************************************
298 * initialize the Aircraft at the parking location
300 * Note that this is the original version that does not
301 * do any dynamic route computation.
302 ******************************************************************/
303 void FGAIFlightPlan::createCruise(FGAIAircraft *ac, bool firstFlight, FGAirport *dep,
304 FGAirport *arr, double latitude,
305 double longitude, double speed,
306 double alt, const string& fltType)
308 double vCruise = ac->getPerformance()->vCruise();
310 wpt = createInAir(ac, "Cruise", SGGeod::fromDeg(longitude, latitude), alt, vCruise);
311 waypoints.push_back(wpt);
313 string rwyClass = getRunwayClassFromTrafficType(fltType);
314 double heading = ac->getTrafficRef()->getCourse();
315 arr->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway, heading);
316 rwy = arr->getRunwayByIdent(activeRunway);
317 // begin descent 110km out
318 SGGeod beginDescentPoint = rwy->pointOnCenterline(-110000);
320 wpt = createInAir(ac, "BOD", beginDescentPoint, alt, vCruise);
321 waypoints.push_back(wpt);