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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301, 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 #include <simgear/misc/sg_path.hxx>
  36
  37 #include "awynet.hxx"
  38
  39 using std::sort;
  40 using std::string;
  41 using std::cerr;
  42 using std::endl;
  43
  44 /**************************************************************************
  45  * FGNode
  46  *************************************************************************/
  47 FGNode::FGNode()
  48 {
  49 }
  50
  51 FGNode::FGNode(const SGGeod& aPos, int idx, std::string id) :
  52   ident(id),
  53   geod(aPos),
  54   index(idx)
  55 {
  56   cart = SGVec3d::fromGeod(geod);
  57 }
  58
  59 bool FGNode::matches(std::string id, const SGGeod& aPos)
  60 {
  61   if ((ident == id) &&
  62       (fabs(aPos.getLatitudeDeg() - geod.getLatitudeDeg()) < 1.0) &&
  63       (fabs(aPos.getLongitudeDeg() - geod.getLongitudeDeg()) < 1.0))
  64     return true;
  65   else
  66     return false;
  67 }
  68
  69 /***************************************************************************
  70  * FGAirway
  71  **************************************************************************/
  72 FGAirway::FGAirway()
  73 {
  74   length = 0;
  75 }
  76
  77 void FGAirway::setStart(node_map *nodes)
  78 {
  79   node_map_iterator itr = nodes->find(startNode);
  80   if (itr == nodes->end()) {
  81     cerr << "Couldn't find node: " << startNode << endl;
  82   }
  83   else {
  84     start = itr->second->getAddress();
  85     itr->second->addAirway(this);
  86   }
  87 }
  88
  89 void FGAirway::setEnd(node_map *nodes)
  90 {
  91  node_map_iterator itr = nodes->find(endNode);
  92   if (itr == nodes->end()) {
  93     cerr << "Couldn't find node: " << endNode << endl;
  94   }
  95   else {
  96     end = itr->second->getAddress();
  97   }
  98 }
  99
 100 // There is probably a computationally cheaper way of
 101 // doing this.
 102 void FGAirway::setTrackDistance()
 103 {
 104   length = SGGeodesy::distanceM(start->getPosition(), end->getPosition());
 105 }
 106
 107 /***************************************************************************
 108  * FGAirRoute()
 109  **************************************************************************/
 110
 111
 112 bool FGAirRoute::next(int *val)
 113 {
 114   //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 115   //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 116   //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 117   //if (currNode != nodes.end())
 118   //  cerr << "true" << endl;
 119   //else
 120   //  cerr << "false" << endl;
 121
 122   if (currNode == nodes.end())
 123     return false;
 124   *val = *(currNode);
 125   currNode++;
 126   return true;
 127 };
 128
 129 void FGAirRoute::add(const FGAirRoute &other) {
 130   for (constIntVecIterator i = other.nodes.begin() ;
 131        i != other.nodes.end(); i++)
 132     {
 133       nodes.push_back((*i));
 134     }
 135   distance += other.distance;
 136 }
 137 /***************************************************************************
 138  * FGAirwayNetwork()
 139  **************************************************************************/
 140
 141 FGAirwayNetwork::FGAirwayNetwork()
 142 {
 143   hasNetwork = false;
 144   foundRoute = false;
 145   totalDistance = 0;
 146   maxDistance = 0;
 147 }
 148
 149 FGAirwayNetwork::~FGAirwayNetwork()
 150 {
 151     for (unsigned int it = 0; it < nodes.size(); it++) {
 152         delete nodes[ it];
 153     }
 154 }
 155 void FGAirwayNetwork::addAirway(const FGAirway &seg)
 156 {
 157   segments.push_back(seg);
 158 }
 159
 160 //void FGAirwayNetwork::addNode(const FGNode &node)
 161 //{
 162 //  nodes.push_back(node);
 163 //}
 164
 165 /*
 166   void FGAirwayNetwork::addNodes(FGParkingList *parkings)
 167   {
 168   FGTaxiNode n;
 169   FGParkingList::iterator i = parkings->begin();
 170   while (i != parkings->end())
 171   {
 172   n.setIndex(i->getIndex());
 173   n.setLatitude(i->getLatitude());
 174   n.setLongitude(i->getLongitude());
 175   nodes.push_back(n);
 176
 177   i++;
 178   }
 179   }
 180 */
 181
 182
 183 void FGAirwayNetwork::init()
 184 {
 185   hasNetwork = true;
 186   FGAirwayVectorIterator i = segments.begin();
 187   while(i != segments.end()) {
 188     //cerr << "initializing Airway " << i->getIndex() << endl;
 189     i->setStart(&nodesMap);
 190     i->setEnd  (&nodesMap);
 191     //i->setTrackDistance();
 192     //cerr << "Track distance = " << i->getLength() << endl;
 193     //cerr << "Track ends at"      << i->getEnd()->getIndex() << endl;
 194     i++;
 195   }
 196   //exit(1);
 197 }
 198
 199
 200 void FGAirwayNetwork::load(const SGPath& path)
 201 {
 202   std::string identStart, identEnd, token, name;
 203   double latStart, lonStart, latEnd, lonEnd;
 204   int type, base, top;
 205   int airwayIndex = 0;
 206   FGNode *n;
 207
 208   sg_gzifstream in( path.str() );
 209   if ( !in.is_open() ) {
 210     SG_LOG( SG_NAVAID, SG_ALERT, "Cannot open file: " << path.str() );
 211     exit(-1);
 212   }
 213   // toss the first two lines of the file
 214   in >> skipeol;
 215   in >> skipeol;
 216
 217   // read in each remaining line of the file
 218     while ( ! in.eof() ) {
 219       string token;
 220       in >> token;
 221
 222       if ( token == "99" ) {
 223         return; //in >> skipeol;
 224       }
 225       // Read each line from the database
 226       identStart = token;
 227       in >> latStart >> lonStart >> identEnd >> latEnd >> lonEnd >> type >> base >> top >> name;
 228       in >> skipeol;
 229       /*out << identStart << " "
 230         << latStart   << " "
 231         << lonStart   << " "
 232         << identEnd   << " "
 233         << latEnd     << " "
 234         << lonEnd     << " "
 235         << type       << " "
 236         << base       << " "
 237         << top        << " "
 238         << name       << " "
 239         << endl;*/
 240       //first determine whether the start and end reference database already exist
 241       //if not we should create them
 242       int startIndex = 0, endIndex=0;
 243       //     FGNodeVectorIterator i = nodes.begin();
 244   //     while (i != nodes.end() && (!(i->matches(identStart,latStart, lonStart))))
 245 //      {
 246 //        i++;
 247 //        startIndex++;
 248 //      }
 249 //       if (i == nodes.end())
 250 //      {
 251 //        nodes.push_back(FGNode(latStart, lonStart, startIndex, identStart));
 252 //      //cout << "Adding node: " << identStart << endl;
 253 //      }
 254
 255 //       i = nodes.begin();
 256 //       while (i != nodes.end() && (!(i->matches(identEnd,latEnd, lonEnd)))) {
 257 //      i++;
 258 //      endIndex++;
 259 //       }
 260 //       if (i == nodes.end()) {
 261 //      nodes.push_back(FGNode(latEnd, lonEnd, endIndex, identEnd));
 262 //      //cout << "Adding node: " << identEnd << endl;
 263 //       }
 264       // generate unique IDs for the nodes, consisting of a combination
 265       // of the Navaid identifier + the integer value of the lat/lon position.
 266       // identifier alone will not suffice, because they might not be globally unique.
 267       char buffer[32];
 268       string startNode, endNode;
 269       // Start
 270       buffer[sizeof(buffer)-1] = 0;
 271       snprintf(buffer, sizeof(buffer)-1, "%s%d%d", identStart.c_str(), (int) latStart, (int) lonStart);
 272       startNode = buffer;
 273
 274       node_map_iterator itr = nodesMap.find(string(buffer));
 275       if (itr == nodesMap.end()) {
 276         startIndex = nodes.size();
 277   SGGeod startPos(SGGeod::fromDeg(lonStart, latStart));
 278         n = new FGNode(startPos, startIndex, identStart);
 279         nodesMap[string(buffer)] = n;
 280         nodes.push_back(n);
 281         //cout << "Adding node: " << identStart << endl;
 282       }
 283       else {
 284         startIndex = itr->second->getIndex();
 285       }
 286       // Start
 287       snprintf(buffer, 32, "%s%d%d", identEnd.c_str(), (int) latEnd, (int) lonEnd);
 288       endNode = buffer;
 289
 290       itr = nodesMap.find(string(buffer));
 291       if (itr == nodesMap.end()) {
 292         endIndex = nodes.size();
 293   SGGeod endPos(SGGeod::fromDeg(lonEnd, latEnd));
 294         n = new FGNode(endPos, endIndex, identEnd);
 295         nodesMap[string(buffer)] = n;
 296         nodes.push_back(n);
 297         //cout << "Adding node: " << identEnd << endl;
 298       }
 299       else {
 300         endIndex = itr->second->getIndex();
 301       }
 302
 303
 304       FGAirway airway;
 305       airway.setIndex        ( airwayIndex++ );
 306       airway.setStartNodeRef ( startNode     );
 307       airway.setEndNodeRef   ( endNode       );
 308       airway.setType         ( type          );
 309       airway.setBase         ( base          );
 310       airway.setTop          ( top           );
 311       airway.setName         ( name          );
 312       segments.push_back(airway);
 313       //cout << "    Adding Airway: " << name << " " << startIndex << " " << endIndex << endl;
 314     }
 315   }
 316
 317 int FGAirwayNetwork::findNearestNode(const SGGeod& aPos)
 318 {
 319   double minDist = HUGE_VAL;
 320   int index = -1;
 321   SGVec3d cart = SGVec3d::fromGeod(aPos);
 322
 323   //cerr << "Lat " << lat << " lon " << lon << endl;
 324   for (FGNodeVectorIterator
 325          itr = nodes.begin();
 326        itr != nodes.end(); itr++)
 327     {
 328       double d2 = distSqr(cart, (*itr)->getCart());
 329       if (d2 < minDist)
 330       {
 331         minDist = d2;
 332                   index = (*itr)->getIndex();
 333       }
 334       //cerr << (*itr)->getIndex() << endl;
 335     }
 336   //cerr << " returning  " << index << endl;
 337   return index;
 338 }
 339
 340 FGNode *FGAirwayNetwork::findNode(int idx)
 341 {
 342   for (FGNodeVectorIterator
 343          itr = nodes.begin();
 344        itr != nodes.end(); itr++)
 345     {
 346       if ((*itr)->getIndex() == idx)
 347         return (*itr)->getAddress();
 348     }
 349   return 0;
 350 }
 351
 352 FGAirRoute FGAirwayNetwork::findShortestRoute(int start, int end)
 353 {
 354   foundRoute = false;
 355   totalDistance = 0;
 356   FGNode *firstNode = findNode(start);
 357   //FGNode *lastNode  = findNode(end);
 358   //prevNode = prevPrevNode = -1;
 359   //prevNode = start;
 360   routes.clear();
 361   traceStack.clear();
 362   trace(firstNode, end, 0, 0);
 363   FGAirRoute empty;
 364
 365   if (!foundRoute)
 366     {
 367       SG_LOG( SG_NAVAID, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
 368       cerr << "Failed to find route from waypoint " << start << " to " << end << endl;
 369       //exit(1);
 370     }
 371   sort(routes.begin(), routes.end());
 372   //for (intVecIterator i = route.begin(); i != route.end(); i++)
 373   //  {
 374   //    rte->push_back(*i);
 375   //  }
 376
 377   if (routes.begin() != routes.end())
 378     return *(routes.begin());
 379   else
 380     return empty;
 381 }
 382
 383
 384 void FGAirwayNetwork::trace(FGNode *currNode, int end, int depth, double distance)
 385 {
 386   traceStack.push_back(currNode->getIndex());
 387   totalDistance += distance;
 388   //cerr << depth << " ";
 389   //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl;
 390   //<< currNode->getIndex() << endl;
 391
 392   // If the current route matches the required end point we found a valid route
 393   // So we can add this to the routing table
 394   if (currNode->getIndex() == end)
 395     {
 396       cerr << "Found route : " <<  totalDistance << "" << " " << *(traceStack.end()-1) << endl;
 397       routes.push_back(FGAirRoute(traceStack,totalDistance));
 398       traceStack.pop_back();
 399       if (!(foundRoute))
 400         maxDistance = totalDistance;
 401       else
 402         if (totalDistance < maxDistance)
 403           maxDistance = totalDistance;
 404       foundRoute = true;
 405       totalDistance -= distance;
 406       return;
 407     }
 408
 409
 410   // search if the currentNode has been encountered before
 411   // if so, we should step back one level, because it is
 412   // rather rediculous to proceed further from here.
 413   // if the current node has not been encountered before,
 414   // i should point to traceStack.end()-1; and we can continue
 415   // if i is not traceStack.end, the previous node was found,
 416   // and we should return.
 417   // This only works at trace levels of 1 or higher though
 418   if (depth > 0) {
 419     intVecIterator i = traceStack.begin();
 420     while ((*i) != currNode->getIndex()) {
 421       //cerr << "Route so far : " << (*i) << endl;
 422       i++;
 423     }
 424     if (i != traceStack.end()-1) {
 425       traceStack.pop_back();
 426       totalDistance -= distance;
 427       return;
 428     }
 429     // If the total distance from start to the current waypoint
 430     // is longer than that of a route we can also stop this trace
 431     // and go back one level.
 432     if ((totalDistance > maxDistance) && foundRoute)
 433       {
 434         cerr << "Stopping rediculously long trace: " << totalDistance << endl;
 435         traceStack.pop_back();
 436         totalDistance -= distance;
 437         return;
 438       }
 439   }
 440
 441   //cerr << "2" << endl;
 442   if (currNode->getBeginRoute() != currNode->getEndRoute())
 443     {
 444       //cerr << "l3l" << endl;
 445       for (FGAirwayPointerVectorIterator
 446              i = currNode->getBeginRoute();
 447            i != currNode->getEndRoute();
 448            i++)
 449         {
 450           //cerr << (*i)->getLenght() << endl;
 451           trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
 452         //  {
 453         //      // cerr << currNode -> getIndex() << " ";
 454         //      route.push_back(currNode->getIndex());
 455         //      return true;
 456         //    }
 457         }
 458     }
 459   else
 460     {
 461       //SG_LOG( SG_NAVAID, SG_DEBUG, "4" );
 462       //cerr << "4" << endl;
 463     }
 464   traceStack.pop_back();
 465   totalDistance -= distance;
 466   return;
 467 }
 468