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