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 SGGeod geod = SGGeod::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
82 tmpNode = globals->get_airwaynet()->findNearestNode(geod);
84 FGNode* node = globals->get_airwaynet()->findNode(tmpNode);
86 if ((tmpNode != prevNode) && (SGGeodesy::distanceM(geod, node->getPosition()) < 25000)) {
87 nodes.push_back(tmpNode);
91 intVecIterator i = nodes.begin();
92 intVecIterator j = nodes.end();
93 while (i != nodes.end())
99 FGAirRoute routePart = globals->get_airwaynet()->findShortestRoute(*i, *j);
100 if (!(routePart.empty()))
102 airRoute.add(routePart);
113 void FGAIFlightPlan::createCruise(bool firstFlight, FGAirport *dep,
114 FGAirport *arr, double latitude,
115 double longitude, double speed, double alt)
117 bool useInitialWayPoint = true;
118 bool useCurrentWayPoint = false;
121 double lat2, lon2, az2;
124 waypoint *init_waypoint;
127 SGPath routefile = globals->get_fg_root();
128 init_waypoint = new waypoint;
129 init_waypoint->name = "Initial waypoint";
130 init_waypoint->latitude = latitude;
131 init_waypoint->longitude = longitude;;
132 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
133 init_waypoint->altitude = alt;
134 init_waypoint->speed = 450; //speed;
135 init_waypoint->crossat = -10000;
136 init_waypoint->gear_down = false;
137 init_waypoint->flaps_down= false;
138 init_waypoint->finished = false;
139 init_waypoint->on_ground = false;
140 waypoints.push_back(init_waypoint);
141 routefile.append("Data/AI/FlightPlans");
142 snprintf(buffer, 32, "%s-%s.txt",
143 dep->getId().c_str(),
144 arr->getId().c_str());
145 routefile.append(buffer);
146 cerr << "trying to read " << routefile.c_str()<< endl;
148 if (routefile.exists())
150 sg_gzifstream in( routefile.str() );
153 } while (!(in.eof()));
156 //int runwayId = apt->getDynamics()->getGroundNetwork()->findNearestNode(lat2, lon2);
157 //int startId = globals->get_airwaynet()->findNearestNode(dep->getLatitude(), dep->getLongitude());
158 //int endId = globals->get_airwaynet()->findNearestNode(arr->getLatitude(), arr->getLongitude());
160 evaluateRoutePart(dep->getLatitude(), dep->getLongitude(),
161 arr->getLatitude(), arr->getLongitude());
167 // if no route could be found, create a direct gps route...
168 cerr << "still no route found from " << dep->getName() << " to << " << arr->getName() <<endl;
172 while(route.next(&node))
174 FGNode *fn = globals->get_airwaynet()->findNode(node);
175 //cerr << "Checking status of each waypoint: " << fn->getIdent();
177 SGWayPoint first(init_waypoint->longitude,
178 init_waypoint->latitude,
180 SGWayPoint curr (fn->getLongitude(),
183 SGWayPoint arr (arr->getLongitude(),
187 double crse, crsDiff;
189 first.CourseAndDistance(arr, &course, &distance);
190 first.CourseAndDistance(curr, &crse, &dist);
192 dist *= SG_METER_TO_NM;
194 // We're only interested in the absolute value of crsDiff
195 // wich should fall in the 0-180 deg range.
196 crsDiff = fabs(crse-course);
198 crsDiff = 360-crsDiff;
199 // These are the three conditions that we consider including
200 // in our flight plan:
201 // 1) current waypoint is less then 100 miles away OR
202 // 2) curren waypoint is ahead of us, at any distance
203 //cerr << " Distance : " << dist << " : Course diff " << crsDiff
204 // << " crs to dest : " << course
205 // << " crs to wpt : " << crse;
206 if ((dist > 20.0) && (crsDiff > 90.0))
209 // Once we start including waypoints, we have to continue, even though
210 // one of the following way point would suffice.
211 // so once is the useWpt flag is set to true, we cannot reset it to false.
212 //cerr << " discarding " << endl;
213 // << ": Course difference = " << crsDiff
214 // << "Course = " << course
215 // << "crse = " << crse << endl;
219 //cerr << " accepting " << endl;
223 wpt->name = "Airway"; // fixme: should be the name of the waypoint
224 wpt->latitude = fn->getLatitude();
225 wpt->longitude = fn->getLongitude();
226 //wpt->altitude = apt->getElevation(); // should maybe be tn->elev too
228 wpt->speed = 450; //speed;
229 wpt->crossat = -10000;
230 wpt->gear_down = false;
231 wpt->flaps_down= false;
232 wpt->finished = false;
233 wpt->on_ground = false;
234 waypoints.push_back(wpt);
237 if (!(routefile.exists()))
240 fstream outf( routefile.c_str(), fstream::out );
241 while (route.next(&node))
242 outf << node << endl;
246 arr->getDynamics()->getActiveRunway("com", 2, activeRunway);
247 if (!(globals->get_runways()->search(arr->getId(),
251 cout << "Failed to find runway for " << arr->getId() << endl;
252 // Hmm, how do we handle a potential error like this?
256 //arr->getActiveRunway(string("com"), 1, test);
259 //cerr << "Altitude = " << alt << endl;
260 //cerr << "Done" << endl;
261 //if (arr->getId() == "EHAM")
263 // cerr << "Creating cruise to EHAM " << latitude << " " << longitude << endl;
265 heading = rwy._heading;
266 azimuth = heading + 180.0;
267 while ( azimuth >= 360.0 ) { azimuth -= 360.0; }
270 // Note: This places us at the location of the active
271 // runway during initial cruise. This needs to be
273 geo_direct_wgs_84 ( 0, rwy._lat, rwy._lon, azimuth,
275 &lat2, &lon2, &az2 );
277 wpt->name = "BOD"; //wpt_node->getStringValue("name", "END");
278 wpt->latitude = lat2;
279 wpt->longitude = lon2;
283 wpt->gear_down = false;
284 wpt->flaps_down= false;
285 wpt->finished = false;
286 wpt->on_ground = false;
287 waypoints.push_back(wpt);
292 /*******************************************************************
294 * initialize the Aircraft at the parking location
296 * Note that this is the original version that does not
297 * do any dynamic route computation.
298 ******************************************************************/
299 void FGAIFlightPlan::createCruise(FGAIAircraft *ac, bool firstFlight, FGAirport *dep,
300 FGAirport *arr, double latitude,
301 double longitude, double speed,
302 double alt, const string& fltType)
304 double vCruise = ac->getPerformance()->vCruise();
306 wpt = createInAir(ac, "Cruise", SGGeod::fromDeg(longitude, latitude), alt, vCruise);
307 waypoints.push_back(wpt);
309 string rwyClass = getRunwayClassFromTrafficType(fltType);
310 double heading = ac->getTrafficRef()->getCourse();
311 arr->getDynamics()->getActiveRunway(rwyClass, 2, activeRunway, heading);
312 rwy = arr->getRunwayByIdent(activeRunway);
313 // begin descent 110km out
314 SGGeod beginDescentPoint = rwy->pointOnCenterline(-110000);
316 wpt = createInAir(ac, "BOD", beginDescentPoint, alt, vCruise);
317 waypoints.push_back(wpt);