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