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
 218 #ifdef __MWERKS__
 219   char c = 0;
 220   while ( in.get(c) && c != '\0' ) {
 221     in.putback(c);
 222 #else
 223     while ( ! in.eof() ) {
 224 #endif
 225       string token;
 226       in >> token;
 227
 228       if ( token == "99" ) {
 229         return; //in >> skipeol;
 230       }
 231       // Read each line from the database
 232       identStart = token;
 233       in >> latStart >> lonStart >> identEnd >> latEnd >> lonEnd >> type >> base >> top >> name;
 234       in >> skipeol;
 235       /*out << identStart << " "
 236         << latStart   << " "
 237         << lonStart   << " "
 238         << identEnd   << " "
 239         << latEnd     << " "
 240         << lonEnd     << " "
 241         << type       << " "
 242         << base       << " "
 243         << top        << " "
 244         << name       << " "
 245         << endl;*/
 246       //first determine whether the start and end reference database already exist
 247       //if not we should create them
 248       int startIndex = 0, endIndex=0;
 249       //     FGNodeVectorIterator i = nodes.begin();
 250   //     while (i != nodes.end() && (!(i->matches(identStart,latStart, lonStart))))
 251 //      {
 252 //        i++;
 253 //        startIndex++;
 254 //      }
 255 //       if (i == nodes.end())
 256 //      {
 257 //        nodes.push_back(FGNode(latStart, lonStart, startIndex, identStart));
 258 //      //cout << "Adding node: " << identStart << endl;
 259 //      }
 260
 261 //       i = nodes.begin();
 262 //       while (i != nodes.end() && (!(i->matches(identEnd,latEnd, lonEnd)))) {
 263 //      i++;
 264 //      endIndex++;
 265 //       }
 266 //       if (i == nodes.end()) {
 267 //      nodes.push_back(FGNode(latEnd, lonEnd, endIndex, identEnd));
 268 //      //cout << "Adding node: " << identEnd << endl;
 269 //       }
 270       // generate unique IDs for the nodes, consisting of a combination
 271       // of the Navaid identifier + the integer value of the lat/lon position.
 272       // identifier alone will not suffice, because they might not be globally unique.
 273       char buffer[32];
 274       string startNode, endNode;
 275       // Start
 276       buffer[sizeof(buffer)-1] = 0;
 277       snprintf(buffer, sizeof(buffer)-1, "%s%d%d", identStart.c_str(), (int) latStart, (int) lonStart);
 278       startNode = buffer;
 279
 280       node_map_iterator itr = nodesMap.find(string(buffer));
 281       if (itr == nodesMap.end()) {
 282         startIndex = nodes.size();
 283         n = new FGNode(latStart, lonStart, startIndex, identStart);
 284         nodesMap[string(buffer)] = n;
 285         nodes.push_back(n);
 286         //cout << "Adding node: " << identStart << endl;
 287       }
 288       else {
 289         startIndex = itr->second->getIndex();
 290       }
 291       // Start
 292       snprintf(buffer, 32, "%s%d%d", identEnd.c_str(), (int) latEnd, (int) lonEnd);
 293       endNode = buffer;
 294
 295       itr = nodesMap.find(string(buffer));
 296       if (itr == nodesMap.end()) {
 297         endIndex = nodes.size();
 298         n = new FGNode(latEnd, lonEnd, endIndex, identEnd);
 299         nodesMap[string(buffer)] = n;
 300         nodes.push_back(n);
 301         //cout << "Adding node: " << identEnd << endl;
 302       }
 303       else {
 304         endIndex = itr->second->getIndex();
 305       }
 306
 307
 308       FGAirway airway;
 309       airway.setIndex        ( airwayIndex++ );
 310       airway.setStartNodeRef ( startNode     );
 311       airway.setEndNodeRef   ( endNode       );
 312       airway.setType         ( type          );
 313       airway.setBase         ( base          );
 314       airway.setTop          ( top           );
 315       airway.setName         ( name          );
 316       segments.push_back(airway);
 317       //cout << "    Adding Airway: " << name << " " << startIndex << " " << endIndex << endl;
 318     }
 319   }
 320
 321   int FGAirwayNetwork::findNearestNode(double lat, double lon)
 322 {
 323   double minDist = HUGE_VAL;
 324   double distsqrt, lat2, lon2;
 325   int index;
 326   SGWayPoint first  (lon,
 327                      lat,
 328                      0);
 329   //cerr << "Lat " << lat << " lon " << lon << endl;
 330   for (FGNodeVectorIterator
 331          itr = nodes.begin();
 332        itr != nodes.end(); itr++)
 333     {
 334       //double course;
 335       //if ((fabs(lat - ((*itr)->getLatitude())) < 0.001) &&
 336       //  (fabs(lon - ((*itr)->getLongitude()) < 0.001)))
 337       //cerr << "Warning: nodes are near" << endl;
 338       //SGWayPoint second ((*itr)->getLongitude(),
 339       //                 (*itr)->getLatitude(),
 340       //                 0);
 341       //first.CourseAndDistance(second, &course, &dist);
 342       lat2 = (*itr)->getLatitude();
 343       lon2 = (*itr)->getLongitude();
 344       // Note: This equation should adjust for decreasing distance per longitude
 345       // with increasing lat.
 346       distsqrt =
 347         (lat-lat2)*(lat-lat2) +
 348         (lon-lon2)*(lon-lon2);
 349
 350       if (distsqrt < minDist)
 351         {
 352           minDist = distsqrt;
 353           //cerr << "Test" << endl;
 354           index = (*itr)->getIndex();
 355           //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
 356           //cerr << (*itr)->getLatitude() << " " << (*itr)->getLongitude() << endl;
 357         }
 358       //cerr << (*itr)->getIndex() << endl;
 359     }
 360   //cerr << " returning  " << index << endl;
 361   return index;
 362 }
 363
 364 FGNode *FGAirwayNetwork::findNode(int idx)
 365 {
 366   for (FGNodeVectorIterator
 367          itr = nodes.begin();
 368        itr != nodes.end(); itr++)
 369     {
 370       if ((*itr)->getIndex() == idx)
 371         return (*itr)->getAddress();
 372     }
 373   return 0;
 374 }
 375
 376 FGAirRoute FGAirwayNetwork::findShortestRoute(int start, int end)
 377 {
 378   foundRoute = false;
 379   totalDistance = 0;
 380   FGNode *firstNode = findNode(start);
 381   FGNode *lastNode  = findNode(end);
 382   //prevNode = prevPrevNode = -1;
 383   //prevNode = start;
 384   routes.clear();
 385   traceStack.clear();
 386   trace(firstNode, end, 0, 0);
 387   FGAirRoute empty;
 388
 389   if (!foundRoute)
 390     {
 391       SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
 392       cerr << "Failed to find route from waypoint " << start << " to " << end << endl;
 393       //exit(1);
 394     }
 395   sort(routes.begin(), routes.end());
 396   //for (intVecIterator i = route.begin(); i != route.end(); i++)
 397   //  {
 398   //    rte->push_back(*i);
 399   //  }
 400
 401   if (routes.begin() != routes.end())
 402     return *(routes.begin());
 403   else
 404     return empty;
 405 }
 406
 407
 408 void FGAirwayNetwork::trace(FGNode *currNode, int end, int depth, double distance)
 409 {
 410   traceStack.push_back(currNode->getIndex());
 411   totalDistance += distance;
 412   //cerr << depth << " ";
 413   //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl;
 414   //<< currNode->getIndex() << endl;
 415
 416   // If the current route matches the required end point we found a valid route
 417   // So we can add this to the routing table
 418   if (currNode->getIndex() == end)
 419     {
 420       cerr << "Found route : " <<  totalDistance << "" << " " << *(traceStack.end()-1) << endl;
 421       routes.push_back(FGAirRoute(traceStack,totalDistance));
 422       traceStack.pop_back();
 423       if (!(foundRoute))
 424         maxDistance = totalDistance;
 425       else
 426         if (totalDistance < maxDistance)
 427           maxDistance = totalDistance;
 428       foundRoute = true;
 429       totalDistance -= distance;
 430       return;
 431     }
 432
 433
 434   // search if the currentNode has been encountered before
 435   // if so, we should step back one level, because it is
 436   // rather rediculous to proceed further from here.
 437   // if the current node has not been encountered before,
 438   // i should point to traceStack.end()-1; and we can continue
 439   // if i is not traceStack.end, the previous node was found,
 440   // and we should return.
 441   // This only works at trace levels of 1 or higher though
 442   if (depth > 0) {
 443     intVecIterator i = traceStack.begin();
 444     while ((*i) != currNode->getIndex()) {
 445       //cerr << "Route so far : " << (*i) << endl;
 446       i++;
 447     }
 448     if (i != traceStack.end()-1) {
 449       traceStack.pop_back();
 450       totalDistance -= distance;
 451       return;
 452     }
 453     // If the total distance from start to the current waypoint
 454     // is longer than that of a route we can also stop this trace
 455     // and go back one level.
 456     if ((totalDistance > maxDistance) && foundRoute)
 457       {
 458         cerr << "Stopping rediculously long trace: " << totalDistance << endl;
 459         traceStack.pop_back();
 460         totalDistance -= distance;
 461         return;
 462       }
 463   }
 464
 465   //cerr << "2" << endl;
 466   if (currNode->getBeginRoute() != currNode->getEndRoute())
 467     {
 468       //cerr << "l3l" << endl;
 469       for (FGAirwayPointerVectorIterator
 470              i = currNode->getBeginRoute();
 471            i != currNode->getEndRoute();
 472            i++)
 473         {
 474           //cerr << (*i)->getLenght() << endl;
 475           trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
 476         //  {
 477         //      // cerr << currNode -> getIndex() << " ";
 478         //      route.push_back(currNode->getIndex());
 479         //      return true;
 480         //    }
 481         }
 482     }
 483   else
 484     {
 485       //SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
 486       //cerr << "4" << endl;
 487     }
 488   traceStack.pop_back();
 489   totalDistance -= distance;
 490   return;
 491 }
 492