2 // by Durk Talsma, started June 2005.
4 // Copyright (C) 2004 Durk Talsma.
6 // This program is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU General Public License as
8 // published by the Free Software Foundation; either version 2 of the
9 // License, or (at your option) any later version.
11 // This program is distributed in the hope that it will be useful, but
12 // WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Public License for more details.
16 // You should have received a copy of the GNU General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
27 # define _USE_MATH_DEFINES
32 #include <simgear/compiler.h>
34 //#include <plib/sg.h>
35 //#include <plib/ul.h>
37 //#include <Environment/environment_mgr.hxx>
38 //#include <Environment/environment.hxx>
39 //#include <simgear/misc/sg_path.hxx>
40 //#include <simgear/props/props.hxx>
41 //#include <simgear/structure/subsystem_mgr.hxx>
42 #include <simgear/debug/logstream.hxx>
43 #include <simgear/misc/sgstream.hxx>
44 #include <simgear/route/waypoint.hxx>
45 //#include <Main/globals.hxx>
46 //#include <Main/fg_props.hxx>
47 //#include <Airports/runways.hxx>
55 /**************************************************************************
57 *************************************************************************/
62 bool FGNode::matches(string id, double lt, double ln)
65 (fabs(lt - lat) < 1.0) &&
66 (fabs(ln - lon) < 1.0))
72 /***************************************************************************
74 **************************************************************************/
80 void FGAirway::setStart(node_map *nodes)
82 node_map_iterator itr = nodes->find(startNode);
83 if (itr == nodes->end()) {
84 cerr << "Couldn't find node: " << startNode << endl;
87 start = itr->second->getAddress();
88 itr->second->addAirway(this);
92 void FGAirway::setEnd(node_map *nodes)
94 node_map_iterator itr = nodes->find(endNode);
95 if (itr == nodes->end()) {
96 cerr << "Couldn't find node: " << endNode << endl;
99 end = itr->second->getAddress();
103 // There is probably a computationally cheaper way of
105 void FGAirway::setTrackDistance()
108 SGWayPoint first (start->getLongitude(),
109 start->getLatitude(),
111 SGWayPoint second (end->getLongitude(),
114 first.CourseAndDistance(second, &course, &length);
118 /***************************************************************************
120 **************************************************************************/
123 bool FGAirRoute::next(int *val)
125 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
126 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
127 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
128 //if (currNode != nodes.end())
129 // cerr << "true" << endl;
131 // cerr << "false" << endl;
133 if (currNode == nodes.end())
140 void FGAirRoute::add(const FGAirRoute &other) {
141 for (constIntVecIterator i = other.nodes.begin() ;
142 i != other.nodes.end(); i++)
144 nodes.push_back((*i));
146 distance += other.distance;
148 /***************************************************************************
150 **************************************************************************/
152 FGAirwayNetwork::FGAirwayNetwork()
160 void FGAirwayNetwork::addAirway(const FGAirway &seg)
162 segments.push_back(seg);
165 //void FGAirwayNetwork::addNode(const FGNode &node)
167 // nodes.push_back(node);
171 void FGAirwayNetwork::addNodes(FGParkingVec *parkings)
174 FGParkingVecIterator i = parkings->begin();
175 while (i != parkings->end())
177 n.setIndex(i->getIndex());
178 n.setLatitude(i->getLatitude());
179 n.setLongitude(i->getLongitude());
188 void FGAirwayNetwork::init()
191 FGAirwayVectorIterator i = segments.begin();
192 while(i != segments.end()) {
193 //cerr << "initializing Airway " << i->getIndex() << endl;
194 i->setStart(&nodesMap);
195 i->setEnd (&nodesMap);
196 //i->setTrackDistance();
197 //cerr << "Track distance = " << i->getLength() << endl;
198 //cerr << "Track ends at" << i->getEnd()->getIndex() << endl;
205 void FGAirwayNetwork::load(SGPath path)
207 string identStart, identEnd, token, name;
208 double latStart, lonStart, latEnd, lonEnd;
213 sg_gzifstream in( path.str() );
214 if ( !in.is_open() ) {
215 SG_LOG( SG_GENERAL, SG_ALERT, "Cannot open file: " << path.str() );
218 // toss the first two lines of the file
222 // read in each remaining line of the file
226 while ( in.get(c) && c != '\0' ) {
229 while ( ! in.eof() ) {
234 if ( token == "99" ) {
235 return; //in >> skipeol;
237 // Read each line from the database
239 in >> latStart >> lonStart >> identEnd >> latEnd >> lonEnd >> type >> base >> top >> name;
240 /*out << identStart << " "
251 //first determine whether the start and end reference database already exist
252 //if not we should create them
253 int startIndex = 0, endIndex=0;
254 // FGNodeVectorIterator i = nodes.begin();
255 // while (i != nodes.end() && (!(i->matches(identStart,latStart, lonStart))))
260 // if (i == nodes.end())
262 // nodes.push_back(FGNode(latStart, lonStart, startIndex, identStart));
263 // //cout << "Adding node: " << identStart << endl;
266 // i = nodes.begin();
267 // while (i != nodes.end() && (!(i->matches(identEnd,latEnd, lonEnd)))) {
271 // if (i == nodes.end()) {
272 // nodes.push_back(FGNode(latEnd, lonEnd, endIndex, identEnd));
273 // //cout << "Adding node: " << identEnd << endl;
275 // generate unique IDs for the nodes, consisting of a combination
276 // of the Navaid identifier + the integer value of the lat/lon position.
277 // identifier alone will not suffice, because they might not be globally unique.
279 string startNode, endNode;
281 snprintf(buffer, 32, "%s%d%d", identStart.c_str(), (int) latStart, (int) lonStart);
284 node_map_iterator itr = nodesMap.find(string(buffer));
285 if (itr == nodesMap.end()) {
286 startIndex = nodes.size();
287 n = new FGNode(latStart, lonStart, startIndex, identStart);
288 nodesMap[string(buffer)] = n;
290 //cout << "Adding node: " << identStart << endl;
293 startIndex = itr->second->getIndex();
296 snprintf(buffer, 32, "%s%d%d", identEnd.c_str(), (int) latEnd, (int) lonEnd);
299 itr = nodesMap.find(string(buffer));
300 if (itr == nodesMap.end()) {
301 endIndex = nodes.size();
302 n = new FGNode(latEnd, lonEnd, endIndex, identEnd);
303 nodesMap[string(buffer)] = n;
305 //cout << "Adding node: " << identEnd << endl;
308 endIndex = itr->second->getIndex();
313 airway.setIndex ( airwayIndex++ );
314 airway.setStartNodeRef ( startNode );
315 airway.setEndNodeRef ( endNode );
316 airway.setType ( type );
317 airway.setBase ( base );
318 airway.setTop ( top );
319 airway.setName ( name );
320 segments.push_back(airway);
321 //cout << " Adding Airway: " << name << " " << startIndex << " " << endIndex << endl;
325 int FGAirwayNetwork::findNearestNode(double lat, double lon)
327 double minDist = HUGE_VAL;
328 double distsqrt, lat2, lon2;
330 SGWayPoint first (lon,
333 //cerr << "Lat " << lat << " lon " << lon << endl;
334 for (FGNodeVectorIterator
336 itr != nodes.end(); itr++)
339 //if ((fabs(lat - ((*itr)->getLatitude())) < 0.001) &&
340 // (fabs(lon - ((*itr)->getLongitude()) < 0.001)))
341 //cerr << "Warning: nodes are near" << endl;
342 //SGWayPoint second ((*itr)->getLongitude(),
343 // (*itr)->getLatitude(),
345 //first.CourseAndDistance(second, &course, &dist);
346 lat2 = (*itr)->getLatitude();
347 lon2 = (*itr)->getLongitude();
348 // Note: This equation should adjust for decreasing distance per longitude
349 // with increasing lat.
351 (lat-lat2)*(lat-lat2) +
352 (lon-lon2)*(lon-lon2);
354 if (distsqrt < minDist)
357 //cerr << "Test" << endl;
358 index = (*itr)->getIndex();
359 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
360 //cerr << (*itr)->getLatitude() << " " << (*itr)->getLongitude() << endl;
362 //cerr << (*itr)->getIndex() << endl;
364 //cerr << " returning " << index << endl;
368 FGNode *FGAirwayNetwork::findNode(int idx)
370 for (FGNodeVectorIterator
372 itr != nodes.end(); itr++)
374 if ((*itr)->getIndex() == idx)
375 return (*itr)->getAddress();
380 FGAirRoute FGAirwayNetwork::findShortestRoute(int start, int end)
384 FGNode *firstNode = findNode(start);
385 FGNode *lastNode = findNode(end);
386 //prevNode = prevPrevNode = -1;
390 trace(firstNode, end, 0, 0);
395 SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
396 cerr << "Failed to find route from waypoint " << start << " to " << end << endl;
399 sort(routes.begin(), routes.end());
400 //for (intVecIterator i = route.begin(); i != route.end(); i++)
402 // rte->push_back(*i);
405 if (routes.begin() != routes.end())
406 return *(routes.begin());
412 void FGAirwayNetwork::trace(FGNode *currNode, int end, int depth, double distance)
414 traceStack.push_back(currNode->getIndex());
415 totalDistance += distance;
416 //cerr << depth << " ";
417 //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl;
418 //<< currNode->getIndex() << endl;
420 // If the current route matches the required end point we found a valid route
421 // So we can add this to the routing table
422 if (currNode->getIndex() == end)
424 cerr << "Found route : " << totalDistance << "" << " " << *(traceStack.end()-1) << endl;
425 routes.push_back(FGAirRoute(traceStack,totalDistance));
426 traceStack.pop_back();
428 maxDistance = totalDistance;
430 if (totalDistance < maxDistance)
431 maxDistance = totalDistance;
433 totalDistance -= distance;
438 // search if the currentNode has been encountered before
439 // if so, we should step back one level, because it is
440 // rather rediculous to proceed further from here.
441 // if the current node has not been encountered before,
442 // i should point to traceStack.end()-1; and we can continue
443 // if i is not traceStack.end, the previous node was found,
444 // and we should return.
445 // This only works at trace levels of 1 or higher though
447 intVecIterator i = traceStack.begin();
448 while ((*i) != currNode->getIndex()) {
449 //cerr << "Route so far : " << (*i) << endl;
452 if (i != traceStack.end()-1) {
453 traceStack.pop_back();
454 totalDistance -= distance;
457 // If the total distance from start to the current waypoint
458 // is longer than that of a route we can also stop this trace
459 // and go back one level.
460 if ((totalDistance > maxDistance) && foundRoute)
462 cerr << "Stopping rediculously long trace: " << totalDistance << endl;
463 traceStack.pop_back();
464 totalDistance -= distance;
469 //cerr << "2" << endl;
470 if (currNode->getBeginRoute() != currNode->getEndRoute())
472 //cerr << "l3l" << endl;
473 for (FGAirwayPointerVectorIterator
474 i = currNode->getBeginRoute();
475 i != currNode->getEndRoute();
478 //cerr << (*i)->getLenght() << endl;
479 trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
481 // // cerr << currNode -> getIndex() << " ";
482 // route.push_back(currNode->getIndex());
489 //SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
490 //cerr << "4" << endl;
492 traceStack.pop_back();
493 totalDistance -= distance;