src/Airports/groundnetwork.cxx

   1 // groundnet.cxx - Implimentation of the FlightGear airport ground handling code
   2 //
   3 // Written by Durk Talsma, started June 2005.
   4 //
   5 // Copyright (C) 2004 Durk Talsma.
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  20 //
  21 // $Id$
  22
  23 #ifdef HAVE_CONFIG_H
  24 #  include <config.h>
  25 #endif
  26
  27 #include <math.h>
  28 #include <algorithm>
  29
  30 //#include <plib/sg.h>
  31 //#include <plib/ul.h>
  32
  33 //#include <Environment/environment_mgr.hxx>
  34 //#include <Environment/environment.hxx>
  35 //#include <simgear/misc/sg_path.hxx>
  36 //#include <simgear/props/props.hxx>
  37 //#include <simgear/structure/subsystem_mgr.hxx>
  38 #include <simgear/debug/logstream.hxx>
  39 #include <simgear/route/waypoint.hxx>
  40 //#include <Main/globals.hxx>
  41 //#include <Main/fg_props.hxx>
  42 //#include <Airports/runways.hxx>
  43
  44 //#include STL_STRING
  45
  46 #include "groundnetwork.hxx"
  47
  48 SG_USING_STD(sort);
  49
  50 /**************************************************************************
  51  * FGTaxiNode
  52  *************************************************************************/
  53 FGTaxiNode::FGTaxiNode()
  54 {
  55 }
  56
  57 /***************************************************************************
  58  * FGTaxiSegment
  59  **************************************************************************/
  60 FGTaxiSegment::FGTaxiSegment()
  61 {
  62 }
  63
  64 void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes)
  65 {
  66   FGTaxiNodeVectorIterator i = nodes->begin();
  67   while (i != nodes->end())
  68     {
  69       if (i->getIndex() == startNode)
  70         {
  71           start = i->getAddress();
  72           i->addSegment(this);
  73           return;
  74         }
  75       i++;
  76     }
  77 }
  78
  79 void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes)
  80 {
  81   FGTaxiNodeVectorIterator i = nodes->begin();
  82   while (i != nodes->end())
  83     {
  84       if (i->getIndex() == endNode)
  85         {
  86           end = i->getAddress();
  87           return;
  88         }
  89       i++;
  90     }
  91 }
  92
  93 // There is probably a computationally cheaper way of
  94 // doing this.
  95 void FGTaxiSegment::setTrackDistance()
  96 {
  97   double course;
  98   SGWayPoint first  (start->getLongitude(),
  99                      start->getLatitude(),
 100                      0);
 101   SGWayPoint second (end->getLongitude(),
 102                      end->getLatitude(),
 103                      0);
 104   first.CourseAndDistance(second, &course, &length);
 105 }
 106
 107 bool FGTaxiRoute::next(int *nde)
 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   //if (nodes.size() != (routes.size()) +1)
 117   //  cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
 118
 119   if (currNode == nodes.end())
 120     return false;
 121   *nde = *(currNode);
 122   currNode++;
 123   currRoute++;
 124   return true;
 125 };
 126
 127 bool FGTaxiRoute::next(int *nde, int *rte)
 128 {
 129   //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 130   //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 131   //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 132   //if (currNode != nodes.end())
 133   //  cerr << "true" << endl;
 134   //else
 135   //  cerr << "false" << endl;
 136   if (nodes.size() != (routes.size()) +1) {
 137     SG_LOG(SG_GENERAL, SG_ALERT, "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size());
 138     exit(1);
 139   }
 140   if (currNode == nodes.end())
 141     return false;
 142   *nde = *(currNode);
 143   *rte = *(currRoute);
 144   currNode++;
 145   currRoute++;
 146   return true;
 147 };
 148 /***************************************************************************
 149  * FGGroundNetwork()
 150  **************************************************************************/
 151
 152 FGGroundNetwork::FGGroundNetwork()
 153 {
 154   hasNetwork = false;
 155   foundRoute = false;
 156   totalDistance = 0;
 157   maxDistance = 0;
 158 }
 159
 160 void FGGroundNetwork::addSegment(const FGTaxiSegment &seg)
 161 {
 162   segments.push_back(seg);
 163 }
 164
 165 void FGGroundNetwork::addNode(const FGTaxiNode &node)
 166 {
 167   nodes.push_back(node);
 168 }
 169
 170 void FGGroundNetwork::addNodes(FGParkingVec *parkings)
 171 {
 172   FGTaxiNode n;
 173   FGParkingVecIterator i = parkings->begin();
 174   while (i != parkings->end())
 175     {
 176       n.setIndex(i->getIndex());
 177       n.setLatitude(i->getLatitude());
 178       n.setLongitude(i->getLongitude());
 179       nodes.push_back(n);
 180
 181       i++;
 182     }
 183 }
 184
 185
 186
 187 void FGGroundNetwork::init()
 188 {
 189   hasNetwork = true;
 190   int index = 0;
 191   FGTaxiSegmentVectorIterator i = segments.begin();
 192   while(i != segments.end()) {
 193     //cerr << "initializing node " << i->getIndex() << endl;
 194     i->setStart(&nodes);
 195     i->setEnd  (&nodes);
 196     i->setTrackDistance();
 197     i->setIndex(index++);
 198     //cerr << "Track distance = " << i->getLength() << endl;
 199     //cerr << "Track ends at"      << i->getEnd()->getIndex() << endl;
 200     i++;
 201   }
 202   //exit(1);
 203 }
 204
 205 int FGGroundNetwork::findNearestNode(double lat, double lon)
 206 {
 207   double minDist = HUGE_VAL;
 208   double course, dist;
 209   int index;
 210   SGWayPoint first  (lon,
 211                      lat,
 212                      0);
 213
 214   for (FGTaxiNodeVectorIterator
 215          itr = nodes.begin();
 216        itr != nodes.end(); itr++)
 217     {
 218       double course;
 219       SGWayPoint second (itr->getLongitude(),
 220                          itr->getLatitude(),
 221                          0);
 222       first.CourseAndDistance(second, &course, &dist);
 223       if (dist < minDist)
 224         {
 225           minDist = dist;
 226           index = itr->getIndex();
 227           //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
 228         }
 229     }
 230   return index;
 231 }
 232
 233 FGTaxiNode *FGGroundNetwork::findNode(int idx)
 234 {
 235   for (FGTaxiNodeVectorIterator
 236          itr = nodes.begin();
 237        itr != nodes.end(); itr++)
 238     {
 239       if (itr->getIndex() == idx)
 240         return itr->getAddress();
 241     }
 242   return 0;
 243 }
 244
 245 FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
 246 {
 247   for (FGTaxiSegmentVectorIterator
 248          itr = segments.begin();
 249        itr != segments.end(); itr++)
 250     {
 251       if (itr->getIndex() == idx)
 252         return itr->getAddress();
 253     }
 254   return 0;
 255 }
 256
 257 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
 258 {
 259   foundRoute = false;
 260   totalDistance = 0;
 261   FGTaxiNode *firstNode = findNode(start);
 262   FGTaxiNode *lastNode  = findNode(end);
 263   //prevNode = prevPrevNode = -1;
 264   //prevNode = start;
 265   routes.clear();
 266   nodesStack.clear();
 267   routesStack.clear();
 268
 269   trace(firstNode, end, 0, 0);
 270   FGTaxiRoute empty;
 271
 272   if (!foundRoute)
 273     {
 274       SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
 275       exit(1);
 276     }
 277   sort(routes.begin(), routes.end());
 278   //for (intVecIterator i = route.begin(); i != route.end(); i++)
 279   //  {
 280   //    rte->push_back(*i);
 281   //  }
 282
 283   if (routes.begin() != routes.end())
 284     return *(routes.begin());
 285   else
 286     return empty;
 287 }
 288
 289
 290 void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance)
 291 {
 292   // Just check some preconditions of the trace algorithm
 293   if (nodesStack.size() != routesStack.size())
 294     {
 295       SG_LOG(SG_GENERAL, SG_ALERT, "size of nodesStack and routesStack is not equal. NodesStack :"
 296              << nodesStack.size() << ". RoutesStack : " << routesStack.size());
 297     }
 298   nodesStack.push_back(currNode->getIndex());
 299   totalDistance += distance;
 300   //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl;
 301   //<< currNode->getIndex() << endl;
 302
 303   // If the current route matches the required end point we found a valid route
 304   // So we can add this to the routing table
 305   if (currNode->getIndex() == end)
 306     {
 307       //cerr << "Found route : " <<  totalDistance << "" << " " << *(nodesStack.end()-1) << endl;
 308       routes.push_back(FGTaxiRoute(nodesStack,routesStack,totalDistance));
 309       if (nodesStack.empty() || routesStack.empty())
 310         {
 311           printRoutingError(string("while finishing route"));
 312         }
 313       nodesStack.pop_back();
 314       routesStack.pop_back();
 315       if (!(foundRoute))
 316         maxDistance = totalDistance;
 317       else
 318         if (totalDistance < maxDistance)
 319           maxDistance = totalDistance;
 320       foundRoute = true;
 321       totalDistance -= distance;
 322       return;
 323     }
 324
 325
 326   // search if the currentNode has been encountered before
 327   // if so, we should step back one level, because it is
 328   // rather rediculous to proceed further from here.
 329   // if the current node has not been encountered before,
 330   // i should point to nodesStack.end()-1; and we can continue
 331   // if i is not nodesStack.end, the previous node was found,
 332   // and we should return.
 333   // This only works at trace levels of 1 or higher though
 334   if (depth > 0) {
 335     intVecIterator i = nodesStack.begin();
 336     while ((*i) != currNode->getIndex()) {
 337       //cerr << "Route so far : " << (*i) << endl;
 338       i++;
 339     }
 340     if (i != nodesStack.end()-1) {
 341       if (nodesStack.empty() || routesStack.empty())
 342         {
 343           printRoutingError(string("while returning from an already encountered node"));
 344         }
 345       nodesStack.pop_back();
 346       routesStack.pop_back();
 347       totalDistance -= distance;
 348       return;
 349     }
 350     // If the total distance from start to the current waypoint
 351     // is longer than that of a route we can also stop this trace
 352     // and go back one level.
 353     if ((totalDistance > maxDistance) && foundRoute)
 354       {
 355         //cerr << "Stopping rediculously long trace: " << totalDistance << endl;
 356         if (nodesStack.empty() || routesStack.empty())
 357         {
 358           printRoutingError(string("while returning from finding a rediculously long route"));
 359         }
 360         nodesStack.pop_back();
 361         routesStack.pop_back();
 362         totalDistance -= distance;
 363         return;
 364       }
 365   }
 366
 367   //cerr << "2" << endl;
 368   if (currNode->getBeginRoute() != currNode->getEndRoute())
 369     {
 370       //cerr << "3" << endl;
 371       for (FGTaxiSegmentPointerVectorIterator
 372              i = currNode->getBeginRoute();
 373            i != currNode->getEndRoute();
 374            i++)
 375         {
 376           //cerr << (*i)->getLength() << endl;
 377           //cerr << (*i)->getIndex() << endl;
 378           int idx = (*i)->getIndex();
 379           routesStack.push_back((*i)->getIndex());
 380           trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
 381         //  {
 382         //      // cerr << currNode -> getIndex() << " ";
 383         //      route.push_back(currNode->getIndex());
 384         //      return true;
 385         //    }
 386         }
 387     }
 388   else
 389     {
 390       //SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
 391     }
 392   if (nodesStack.empty())
 393     {
 394       printRoutingError(string("while finishing trace"));
 395     }
 396   nodesStack.pop_back();
 397   // Make sure not to dump the level-zero routesStack entry, because that was never created.
 398   if (depth)
 399     {
 400       routesStack.pop_back();
 401       //cerr << "leaving trace " << routesStack.size() << endl;
 402     }
 403   totalDistance -= distance;
 404   return;
 405 }
 406
 407 void FGGroundNetwork::printRoutingError(string mess)
 408 {
 409   SG_LOG(SG_GENERAL, SG_ALERT,  "Error in ground network trace algorithm " << mess);
 410   if (nodesStack.empty())
 411     {
 412       SG_LOG(SG_GENERAL, SG_ALERT, " nodesStack is empty. Dumping routesStack");
 413       for (intVecIterator i = routesStack.begin() ; i != routesStack.end(); i++)
 414         SG_LOG(SG_GENERAL, SG_ALERT, "Route " << (*i));
 415     }
 416   if (routesStack.empty())
 417     {
 418       SG_LOG(SG_GENERAL, SG_ALERT, " routesStack is empty. Dumping nodesStack");
 419       for (intVecIterator i = nodesStack.begin() ; i != nodesStack.end(); i++)
 420         SG_LOG(SG_GENERAL, SG_ALERT, "Node " << (*i));
 421     }
 422   //exit(1);
 423 }