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