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.
30 #include <simgear/compiler.h>
32 #include <simgear/debug/logstream.hxx>
33 #include <simgear/misc/sgstream.hxx>
34 #include <simgear/route/waypoint.hxx>
43 /**************************************************************************
45 *************************************************************************/
50 FGNode::FGNode(double lt, double ln, int idx, std::string id) :
52 geod(SGGeod::fromDeg(ln, lt)),
57 bool FGNode::matches(string id, double lt, double ln)
60 (fabs(lt - geod.getLatitudeDeg()) < 1.0) &&
61 (fabs(ln - geod.getLongitudeDeg()) < 1.0))
67 /***************************************************************************
69 **************************************************************************/
75 void FGAirway::setStart(node_map *nodes)
77 node_map_iterator itr = nodes->find(startNode);
78 if (itr == nodes->end()) {
79 cerr << "Couldn't find node: " << startNode << endl;
82 start = itr->second->getAddress();
83 itr->second->addAirway(this);
87 void FGAirway::setEnd(node_map *nodes)
89 node_map_iterator itr = nodes->find(endNode);
90 if (itr == nodes->end()) {
91 cerr << "Couldn't find node: " << endNode << endl;
94 end = itr->second->getAddress();
98 // There is probably a computationally cheaper way of
100 void FGAirway::setTrackDistance()
102 length = SGGeodesy::distanceM(start->getPosition(), end->getPosition());
105 /***************************************************************************
107 **************************************************************************/
110 bool FGAirRoute::next(int *val)
112 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
113 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
114 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
115 //if (currNode != nodes.end())
116 // cerr << "true" << endl;
118 // cerr << "false" << endl;
120 if (currNode == nodes.end())
127 void FGAirRoute::add(const FGAirRoute &other) {
128 for (constIntVecIterator i = other.nodes.begin() ;
129 i != other.nodes.end(); i++)
131 nodes.push_back((*i));
133 distance += other.distance;
135 /***************************************************************************
137 **************************************************************************/
139 FGAirwayNetwork::FGAirwayNetwork()
147 FGAirwayNetwork::~FGAirwayNetwork()
149 for (unsigned int it = 0; it < nodes.size(); it++) {
153 void FGAirwayNetwork::addAirway(const FGAirway &seg)
155 segments.push_back(seg);
158 //void FGAirwayNetwork::addNode(const FGNode &node)
160 // nodes.push_back(node);
164 void FGAirwayNetwork::addNodes(FGParkingVec *parkings)
167 FGParkingVecIterator i = parkings->begin();
168 while (i != parkings->end())
170 n.setIndex(i->getIndex());
171 n.setLatitude(i->getLatitude());
172 n.setLongitude(i->getLongitude());
181 void FGAirwayNetwork::init()
184 FGAirwayVectorIterator i = segments.begin();
185 while(i != segments.end()) {
186 //cerr << "initializing Airway " << i->getIndex() << endl;
187 i->setStart(&nodesMap);
188 i->setEnd (&nodesMap);
189 //i->setTrackDistance();
190 //cerr << "Track distance = " << i->getLength() << endl;
191 //cerr << "Track ends at" << i->getEnd()->getIndex() << endl;
198 void FGAirwayNetwork::load(SGPath path)
200 string identStart, identEnd, token, name;
201 double latStart, lonStart, latEnd, lonEnd;
206 sg_gzifstream in( path.str() );
207 if ( !in.is_open() ) {
208 SG_LOG( SG_GENERAL, SG_ALERT, "Cannot open file: " << path.str() );
211 // toss the first two lines of the file
215 // read in each remaining line of the file
216 while ( ! in.eof() ) {
220 if ( token == "99" ) {
221 return; //in >> skipeol;
223 // Read each line from the database
225 in >> latStart >> lonStart >> identEnd >> latEnd >> lonEnd >> type >> base >> top >> name;
227 /*out << identStart << " "
238 //first determine whether the start and end reference database already exist
239 //if not we should create them
240 int startIndex = 0, endIndex=0;
241 // FGNodeVectorIterator i = nodes.begin();
242 // while (i != nodes.end() && (!(i->matches(identStart,latStart, lonStart))))
247 // if (i == nodes.end())
249 // nodes.push_back(FGNode(latStart, lonStart, startIndex, identStart));
250 // //cout << "Adding node: " << identStart << endl;
253 // i = nodes.begin();
254 // while (i != nodes.end() && (!(i->matches(identEnd,latEnd, lonEnd)))) {
258 // if (i == nodes.end()) {
259 // nodes.push_back(FGNode(latEnd, lonEnd, endIndex, identEnd));
260 // //cout << "Adding node: " << identEnd << endl;
262 // generate unique IDs for the nodes, consisting of a combination
263 // of the Navaid identifier + the integer value of the lat/lon position.
264 // identifier alone will not suffice, because they might not be globally unique.
266 string startNode, endNode;
268 buffer[sizeof(buffer)-1] = 0;
269 snprintf(buffer, sizeof(buffer)-1, "%s%d%d", identStart.c_str(), (int) latStart, (int) lonStart);
272 node_map_iterator itr = nodesMap.find(string(buffer));
273 if (itr == nodesMap.end()) {
274 startIndex = nodes.size();
275 n = new FGNode(latStart, lonStart, startIndex, identStart);
276 nodesMap[string(buffer)] = n;
278 //cout << "Adding node: " << identStart << endl;
281 startIndex = itr->second->getIndex();
284 snprintf(buffer, 32, "%s%d%d", identEnd.c_str(), (int) latEnd, (int) lonEnd);
287 itr = nodesMap.find(string(buffer));
288 if (itr == nodesMap.end()) {
289 endIndex = nodes.size();
290 n = new FGNode(latEnd, lonEnd, endIndex, identEnd);
291 nodesMap[string(buffer)] = n;
293 //cout << "Adding node: " << identEnd << endl;
296 endIndex = itr->second->getIndex();
301 airway.setIndex ( airwayIndex++ );
302 airway.setStartNodeRef ( startNode );
303 airway.setEndNodeRef ( endNode );
304 airway.setType ( type );
305 airway.setBase ( base );
306 airway.setTop ( top );
307 airway.setName ( name );
308 segments.push_back(airway);
309 //cout << " Adding Airway: " << name << " " << startIndex << " " << endIndex << endl;
313 int FGAirwayNetwork::findNearestNode(double lat, double lon)
315 double minDist = HUGE_VAL;
316 double distsqrt, lat2, lon2;
318 //cerr << "Lat " << lat << " lon " << lon << endl;
319 for (FGNodeVectorIterator
321 itr != nodes.end(); itr++)
323 lat2 = (*itr)->getLatitude();
324 lon2 = (*itr)->getLongitude();
325 // Note: This equation should adjust for decreasing distance per longitude
326 // with increasing lat.
328 (lat-lat2)*(lat-lat2) +
329 (lon-lon2)*(lon-lon2);
331 if (distsqrt < minDist)
334 //cerr << "Test" << endl;
335 index = (*itr)->getIndex();
336 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
337 //cerr << (*itr)->getLatitude() << " " << (*itr)->getLongitude() << endl;
339 //cerr << (*itr)->getIndex() << endl;
341 //cerr << " returning " << index << endl;
345 FGNode *FGAirwayNetwork::findNode(int idx)
347 for (FGNodeVectorIterator
349 itr != nodes.end(); itr++)
351 if ((*itr)->getIndex() == idx)
352 return (*itr)->getAddress();
357 FGAirRoute FGAirwayNetwork::findShortestRoute(int start, int end)
361 FGNode *firstNode = findNode(start);
362 FGNode *lastNode = findNode(end);
363 //prevNode = prevPrevNode = -1;
367 trace(firstNode, end, 0, 0);
372 SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
373 cerr << "Failed to find route from waypoint " << start << " to " << end << endl;
376 sort(routes.begin(), routes.end());
377 //for (intVecIterator i = route.begin(); i != route.end(); i++)
379 // rte->push_back(*i);
382 if (routes.begin() != routes.end())
383 return *(routes.begin());
389 void FGAirwayNetwork::trace(FGNode *currNode, int end, int depth, double distance)
391 traceStack.push_back(currNode->getIndex());
392 totalDistance += distance;
393 //cerr << depth << " ";
394 //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl;
395 //<< currNode->getIndex() << endl;
397 // If the current route matches the required end point we found a valid route
398 // So we can add this to the routing table
399 if (currNode->getIndex() == end)
401 cerr << "Found route : " << totalDistance << "" << " " << *(traceStack.end()-1) << endl;
402 routes.push_back(FGAirRoute(traceStack,totalDistance));
403 traceStack.pop_back();
405 maxDistance = totalDistance;
407 if (totalDistance < maxDistance)
408 maxDistance = totalDistance;
410 totalDistance -= distance;
415 // search if the currentNode has been encountered before
416 // if so, we should step back one level, because it is
417 // rather rediculous to proceed further from here.
418 // if the current node has not been encountered before,
419 // i should point to traceStack.end()-1; and we can continue
420 // if i is not traceStack.end, the previous node was found,
421 // and we should return.
422 // This only works at trace levels of 1 or higher though
424 intVecIterator i = traceStack.begin();
425 while ((*i) != currNode->getIndex()) {
426 //cerr << "Route so far : " << (*i) << endl;
429 if (i != traceStack.end()-1) {
430 traceStack.pop_back();
431 totalDistance -= distance;
434 // If the total distance from start to the current waypoint
435 // is longer than that of a route we can also stop this trace
436 // and go back one level.
437 if ((totalDistance > maxDistance) && foundRoute)
439 cerr << "Stopping rediculously long trace: " << totalDistance << endl;
440 traceStack.pop_back();
441 totalDistance -= distance;
446 //cerr << "2" << endl;
447 if (currNode->getBeginRoute() != currNode->getEndRoute())
449 //cerr << "l3l" << endl;
450 for (FGAirwayPointerVectorIterator
451 i = currNode->getBeginRoute();
452 i != currNode->getEndRoute();
455 //cerr << (*i)->getLenght() << endl;
456 trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
458 // // cerr << currNode -> getIndex() << " ";
459 // route.push_back(currNode->getIndex());
466 //SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
467 //cerr << "4" << endl;
469 traceStack.pop_back();
470 totalDistance -= distance;