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