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   SGWayPoint first  (lon,
 319                      lat,
 320                      0);
 321   //cerr << "Lat " << lat << " lon " << lon << endl;
 322   for (FGNodeVectorIterator
 323          itr = nodes.begin();
 324        itr != nodes.end(); itr++)
 325     {
 326       //double course;
 327       //if ((fabs(lat - ((*itr)->getLatitude())) < 0.001) &&
 328       //  (fabs(lon - ((*itr)->getLongitude()) < 0.001)))
 329       //cerr << "Warning: nodes are near" << endl;
 330       //SGWayPoint second ((*itr)->getLongitude(),
 331       //                 (*itr)->getLatitude(),
 332       //                 0);
 333       //first.CourseAndDistance(second, &course, &dist);
 334       lat2 = (*itr)->getLatitude();
 335       lon2 = (*itr)->getLongitude();
 336       // Note: This equation should adjust for decreasing distance per longitude
 337       // with increasing lat.
 338       distsqrt =
 339         (lat-lat2)*(lat-lat2) +
 340         (lon-lon2)*(lon-lon2);
 341
 342       if (distsqrt < minDist)
 343         {
 344           minDist = distsqrt;
 345           //cerr << "Test" << endl;
 346           index = (*itr)->getIndex();
 347           //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
 348           //cerr << (*itr)->getLatitude() << " " << (*itr)->getLongitude() << endl;
 349         }
 350       //cerr << (*itr)->getIndex() << endl;
 351     }
 352   //cerr << " returning  " << index << endl;
 353   return index;
 354 }
 355
 356 FGNode *FGAirwayNetwork::findNode(int idx)
 357 {
 358   for (FGNodeVectorIterator
 359          itr = nodes.begin();
 360        itr != nodes.end(); itr++)
 361     {
 362       if ((*itr)->getIndex() == idx)
 363         return (*itr)->getAddress();
 364     }
 365   return 0;
 366 }
 367
 368 FGAirRoute FGAirwayNetwork::findShortestRoute(int start, int end)
 369 {
 370   foundRoute = false;
 371   totalDistance = 0;
 372   FGNode *firstNode = findNode(start);
 373   FGNode *lastNode  = findNode(end);
 374   //prevNode = prevPrevNode = -1;
 375   //prevNode = start;
 376   routes.clear();
 377   traceStack.clear();
 378   trace(firstNode, end, 0, 0);
 379   FGAirRoute empty;
 380
 381   if (!foundRoute)
 382     {
 383       SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
 384       cerr << "Failed to find route from waypoint " << start << " to " << end << endl;
 385       //exit(1);
 386     }
 387   sort(routes.begin(), routes.end());
 388   //for (intVecIterator i = route.begin(); i != route.end(); i++)
 389   //  {
 390   //    rte->push_back(*i);
 391   //  }
 392
 393   if (routes.begin() != routes.end())
 394     return *(routes.begin());
 395   else
 396     return empty;
 397 }
 398
 399
 400 void FGAirwayNetwork::trace(FGNode *currNode, int end, int depth, double distance)
 401 {
 402   traceStack.push_back(currNode->getIndex());
 403   totalDistance += distance;
 404   //cerr << depth << " ";
 405   //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl;
 406   //<< currNode->getIndex() << endl;
 407
 408   // If the current route matches the required end point we found a valid route
 409   // So we can add this to the routing table
 410   if (currNode->getIndex() == end)
 411     {
 412       cerr << "Found route : " <<  totalDistance << "" << " " << *(traceStack.end()-1) << endl;
 413       routes.push_back(FGAirRoute(traceStack,totalDistance));
 414       traceStack.pop_back();
 415       if (!(foundRoute))
 416         maxDistance = totalDistance;
 417       else
 418         if (totalDistance < maxDistance)
 419           maxDistance = totalDistance;
 420       foundRoute = true;
 421       totalDistance -= distance;
 422       return;
 423     }
 424
 425
 426   // search if the currentNode has been encountered before
 427   // if so, we should step back one level, because it is
 428   // rather rediculous to proceed further from here.
 429   // if the current node has not been encountered before,
 430   // i should point to traceStack.end()-1; and we can continue
 431   // if i is not traceStack.end, the previous node was found,
 432   // and we should return.
 433   // This only works at trace levels of 1 or higher though
 434   if (depth > 0) {
 435     intVecIterator i = traceStack.begin();
 436     while ((*i) != currNode->getIndex()) {
 437       //cerr << "Route so far : " << (*i) << endl;
 438       i++;
 439     }
 440     if (i != traceStack.end()-1) {
 441       traceStack.pop_back();
 442       totalDistance -= distance;
 443       return;
 444     }
 445     // If the total distance from start to the current waypoint
 446     // is longer than that of a route we can also stop this trace
 447     // and go back one level.
 448     if ((totalDistance > maxDistance) && foundRoute)
 449       {
 450         cerr << "Stopping rediculously long trace: " << totalDistance << endl;
 451         traceStack.pop_back();
 452         totalDistance -= distance;
 453         return;
 454       }
 455   }
 456
 457   //cerr << "2" << endl;
 458   if (currNode->getBeginRoute() != currNode->getEndRoute())
 459     {
 460       //cerr << "l3l" << endl;
 461       for (FGAirwayPointerVectorIterator
 462              i = currNode->getBeginRoute();
 463            i != currNode->getEndRoute();
 464            i++)
 465         {
 466           //cerr << (*i)->getLenght() << endl;
 467           trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
 468         //  {
 469         //      // cerr << currNode -> getIndex() << " ";
 470         //      route.push_back(currNode->getIndex());
 471         //      return true;
 472         //    }
 473         }
 474     }
 475   else
 476     {
 477       //SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
 478       //cerr << "4" << endl;
 479     }
 480   traceStack.pop_back();
 481   totalDistance -= distance;
 482   return;
 483 }
 484