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