src/Navaids/awynet.cxx

   1 // awynet.cxx
   2 // by Durk Talsma, started June 2005.
   3 //
   4 // Copyright (C) 2004 Durk Talsma.
   5 //
   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.
  10 //
  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.
  15 //
  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.
  19 //
  20 // $Id$
  21
  22 #ifdef HAVE_CONFIG_H
  23 #  include <config.h>
  24 #endif
  25
  26 #include <math.h>
  27 #include <algorithm>
  28 #include <iostream>
  29
  30 #include <simgear/compiler.h>
  31
  32 #include <simgear/debug/logstream.hxx>
  33 #include <simgear/misc/sgstream.hxx>
  34 #include <simgear/route/waypoint.hxx>
  35
  36 #include "awynet.hxx"
  37
  38 using std::sort;
  39
  40 using std::cerr;
  41 using std::endl;
  42
  43 /**************************************************************************
  44  * FGNode
  45  *************************************************************************/
  46 FGNode::FGNode()
  47 {
  48 }
  49
  50 FGNode::FGNode(double lt, double ln, int idx, std::string id) :
  51   ident(id),
  52   geod(SGGeod::fromDeg(ln, lt)),
  53   index(idx)
  54 {
  55 }
  56
  57 bool FGNode::matches(string id, double lt, double ln)
  58 {
  59   if ((ident == id) &&
  60       (fabs(lt - geod.getLatitudeDeg()) < 1.0) &&
  61       (fabs(ln - geod.getLongitudeDeg()) < 1.0))
  62     return true;
  63   else
  64     return false;
  65 }
  66
  67 /***************************************************************************
  68  * FGAirway
  69  **************************************************************************/
  70 FGAirway::FGAirway()
  71 {
  72   length = 0;
  73 }
  74
  75 void FGAirway::setStart(node_map *nodes)
  76 {
  77   node_map_iterator itr = nodes->find(startNode);
  78   if (itr == nodes->end()) {
  79     cerr << "Couldn't find node: " << startNode << endl;
  80   }
  81   else {
  82     start = itr->second->getAddress();
  83     itr->second->addAirway(this);
  84   }
  85 }
  86
  87 void FGAirway::setEnd(node_map *nodes)
  88 {
  89  node_map_iterator itr = nodes->find(endNode);
  90   if (itr == nodes->end()) {
  91     cerr << "Couldn't find node: " << endNode << endl;
  92   }
  93   else {
  94     end = itr->second->getAddress();
  95   }
  96 }
  97
  98 // There is probably a computationally cheaper way of
  99 // doing this.
 100 void FGAirway::setTrackDistance()
 101 {
 102   length = SGGeodesy::distanceM(start->getPosition(), end->getPosition());
 103 }
 104
 105 /***************************************************************************
 106  * FGAirRoute()
 107  **************************************************************************/
 108
 109
 110 bool FGAirRoute::next(int *val)
 111 {
 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;
 117   //else
 118   //  cerr << "false" << endl;
 119
 120   if (currNode == nodes.end())
 121     return false;
 122   *val = *(currNode);
 123   currNode++;
 124   return true;
 125 };
 126
 127 void FGAirRoute::add(const FGAirRoute &other) {
 128   for (constIntVecIterator i = other.nodes.begin() ;
 129        i != other.nodes.end(); i++)
 130     {
 131       nodes.push_back((*i));
 132     }
 133   distance += other.distance;
 134 }
 135 /***************************************************************************
 136  * FGAirwayNetwork()
 137  **************************************************************************/
 138
 139 FGAirwayNetwork::FGAirwayNetwork()
 140 {
 141   hasNetwork = false;
 142   foundRoute = false;
 143   totalDistance = 0;
 144   maxDistance = 0;
 145 }
 146
 147 FGAirwayNetwork::~FGAirwayNetwork()
 148 {
 149     for (unsigned int it = 0; it < nodes.size(); it++) {
 150         delete nodes[ it];
 151     }
 152 }
 153 void FGAirwayNetwork::addAirway(const FGAirway &seg)
 154 {
 155   segments.push_back(seg);
 156 }
 157
 158 //void FGAirwayNetwork::addNode(const FGNode &node)
 159 //{
 160 //  nodes.push_back(node);
 161 //}
 162
 163 /*
 164   void FGAirwayNetwork::addNodes(FGParkingVec *parkings)
 165   {
 166   FGTaxiNode n;
 167   FGParkingVecIterator i = parkings->begin();
 168   while (i != parkings->end())
 169   {
 170   n.setIndex(i->getIndex());
 171   n.setLatitude(i->getLatitude());
 172   n.setLongitude(i->getLongitude());
 173   nodes.push_back(n);
 174
 175   i++;
 176   }
 177   }
 178 */
 179
 180
 181 void FGAirwayNetwork::init()
 182 {
 183   hasNetwork = true;
 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;
 192     i++;
 193   }
 194   //exit(1);
 195 }
 196
 197
 198 void FGAirwayNetwork::load(SGPath path)
 199 {
 200   string identStart, identEnd, token, name;
 201   double latStart, lonStart, latEnd, lonEnd;
 202   int type, base, top;
 203   int airwayIndex = 0;
 204   FGNode *n;
 205
 206   sg_gzifstream in( path.str() );
 207   if ( !in.is_open() ) {
 208     SG_LOG( SG_GENERAL, SG_ALERT, "Cannot open file: " << path.str() );
 209     exit(-1);
 210   }
 211   // toss the first two lines of the file
 212   in >> skipeol;
 213   in >> skipeol;
 214
 215   // read in each remaining line of the file
 216     while ( ! in.eof() ) {
 217       string token;
 218       in >> token;
 219
 220       if ( token == "99" ) {
 221         return; //in >> skipeol;
 222       }
 223       // Read each line from the database
 224       identStart = token;
 225       in >> latStart >> lonStart >> identEnd >> latEnd >> lonEnd >> type >> base >> top >> name;
 226       in >> skipeol;
 227       /*out << identStart << " "
 228         << latStart   << " "
 229         << lonStart   << " "
 230         << identEnd   << " "
 231         << latEnd     << " "
 232         << lonEnd     << " "
 233         << type       << " "
 234         << base       << " "
 235         << top        << " "
 236         << name       << " "
 237         << endl;*/
 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))))
 243 //      {
 244 //        i++;
 245 //        startIndex++;
 246 //      }
 247 //       if (i == nodes.end())
 248 //      {
 249 //        nodes.push_back(FGNode(latStart, lonStart, startIndex, identStart));
 250 //      //cout << "Adding node: " << identStart << endl;
 251 //      }
 252
 253 //       i = nodes.begin();
 254 //       while (i != nodes.end() && (!(i->matches(identEnd,latEnd, lonEnd)))) {
 255 //      i++;
 256 //      endIndex++;
 257 //       }
 258 //       if (i == nodes.end()) {
 259 //      nodes.push_back(FGNode(latEnd, lonEnd, endIndex, identEnd));
 260 //      //cout << "Adding node: " << identEnd << endl;
 261 //       }
 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.
 265       char buffer[32];
 266       string startNode, endNode;
 267       // Start
 268       buffer[sizeof(buffer)-1] = 0;
 269       snprintf(buffer, sizeof(buffer)-1, "%s%d%d", identStart.c_str(), (int) latStart, (int) lonStart);
 270       startNode = buffer;
 271
 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;
 277         nodes.push_back(n);
 278         //cout << "Adding node: " << identStart << endl;
 279       }
 280       else {
 281         startIndex = itr->second->getIndex();
 282       }
 283       // Start
 284       snprintf(buffer, 32, "%s%d%d", identEnd.c_str(), (int) latEnd, (int) lonEnd);
 285       endNode = buffer;
 286
 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;
 292         nodes.push_back(n);
 293         //cout << "Adding node: " << identEnd << endl;
 294       }
 295       else {
 296         endIndex = itr->second->getIndex();
 297       }
 298
 299
 300       FGAirway airway;
 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;
 310     }
 311   }
 312
 313   int FGAirwayNetwork::findNearestNode(double lat, double lon)
 314 {
 315   double minDist = HUGE_VAL;
 316   double distsqrt, lat2, lon2;
 317   int index;
 318   //cerr << "Lat " << lat << " lon " << lon << endl;
 319   for (FGNodeVectorIterator
 320          itr = nodes.begin();
 321        itr != nodes.end(); itr++)
 322     {
 323       lat2 = (*itr)->getLatitude();
 324       lon2 = (*itr)->getLongitude();
 325       // Note: This equation should adjust for decreasing distance per longitude
 326       // with increasing lat.
 327       distsqrt =
 328         (lat-lat2)*(lat-lat2) +
 329         (lon-lon2)*(lon-lon2);
 330
 331       if (distsqrt < minDist)
 332         {
 333           minDist = distsqrt;
 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;
 338         }
 339       //cerr << (*itr)->getIndex() << endl;
 340     }
 341   //cerr << " returning  " << index << endl;
 342   return index;
 343 }
 344
 345 FGNode *FGAirwayNetwork::findNode(int idx)
 346 {
 347   for (FGNodeVectorIterator
 348          itr = nodes.begin();
 349        itr != nodes.end(); itr++)
 350     {
 351       if ((*itr)->getIndex() == idx)
 352         return (*itr)->getAddress();
 353     }
 354   return 0;
 355 }
 356
 357 FGAirRoute FGAirwayNetwork::findShortestRoute(int start, int end)
 358 {
 359   foundRoute = false;
 360   totalDistance = 0;
 361   FGNode *firstNode = findNode(start);
 362   FGNode *lastNode  = findNode(end);
 363   //prevNode = prevPrevNode = -1;
 364   //prevNode = start;
 365   routes.clear();
 366   traceStack.clear();
 367   trace(firstNode, end, 0, 0);
 368   FGAirRoute empty;
 369
 370   if (!foundRoute)
 371     {
 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;
 374       //exit(1);
 375     }
 376   sort(routes.begin(), routes.end());
 377   //for (intVecIterator i = route.begin(); i != route.end(); i++)
 378   //  {
 379   //    rte->push_back(*i);
 380   //  }
 381
 382   if (routes.begin() != routes.end())
 383     return *(routes.begin());
 384   else
 385     return empty;
 386 }
 387
 388
 389 void FGAirwayNetwork::trace(FGNode *currNode, int end, int depth, double distance)
 390 {
 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;
 396
 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)
 400     {
 401       cerr << "Found route : " <<  totalDistance << "" << " " << *(traceStack.end()-1) << endl;
 402       routes.push_back(FGAirRoute(traceStack,totalDistance));
 403       traceStack.pop_back();
 404       if (!(foundRoute))
 405         maxDistance = totalDistance;
 406       else
 407         if (totalDistance < maxDistance)
 408           maxDistance = totalDistance;
 409       foundRoute = true;
 410       totalDistance -= distance;
 411       return;
 412     }
 413
 414
 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
 423   if (depth > 0) {
 424     intVecIterator i = traceStack.begin();
 425     while ((*i) != currNode->getIndex()) {
 426       //cerr << "Route so far : " << (*i) << endl;
 427       i++;
 428     }
 429     if (i != traceStack.end()-1) {
 430       traceStack.pop_back();
 431       totalDistance -= distance;
 432       return;
 433     }
 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)
 438       {
 439         cerr << "Stopping rediculously long trace: " << totalDistance << endl;
 440         traceStack.pop_back();
 441         totalDistance -= distance;
 442         return;
 443       }
 444   }
 445
 446   //cerr << "2" << endl;
 447   if (currNode->getBeginRoute() != currNode->getEndRoute())
 448     {
 449       //cerr << "l3l" << endl;
 450       for (FGAirwayPointerVectorIterator
 451              i = currNode->getBeginRoute();
 452            i != currNode->getEndRoute();
 453            i++)
 454         {
 455           //cerr << (*i)->getLenght() << endl;
 456           trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
 457         //  {
 458         //      // cerr << currNode -> getIndex() << " ";
 459         //      route.push_back(currNode->getIndex());
 460         //      return true;
 461         //    }
 462         }
 463     }
 464   else
 465     {
 466       //SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
 467       //cerr << "4" << endl;
 468     }
 469   traceStack.pop_back();
 470   totalDistance -= distance;
 471   return;
 472 }
 473