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   //  cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
 138
 139   if (currNode == nodes.end())
 140     return false;
 141   *nde = *(currNode);
 142   *rte = *(currRoute);
 143   currNode++;
 144   currRoute++;
 145   return true;
 146 };
 147 /***************************************************************************
 148  * FGGroundNetwork()
 149  **************************************************************************/
 150
 151 FGGroundNetwork::FGGroundNetwork()
 152 {
 153   hasNetwork = false;
 154   foundRoute = false;
 155   totalDistance = 0;
 156   maxDistance = 0;
 157 }
 158
 159 void FGGroundNetwork::addSegment(const FGTaxiSegment &seg)
 160 {
 161   segments.push_back(seg);
 162 }
 163
 164 void FGGroundNetwork::addNode(const FGTaxiNode &node)
 165 {
 166   nodes.push_back(node);
 167 }
 168
 169 void FGGroundNetwork::addNodes(FGParkingVec *parkings)
 170 {
 171   FGTaxiNode n;
 172   FGParkingVecIterator i = parkings->begin();
 173   while (i != parkings->end())
 174     {
 175       n.setIndex(i->getIndex());
 176       n.setLatitude(i->getLatitude());
 177       n.setLongitude(i->getLongitude());
 178       nodes.push_back(n);
 179
 180       i++;
 181     }
 182 }
 183
 184
 185
 186 void FGGroundNetwork::init()
 187 {
 188   hasNetwork = true;
 189   int index = 0;
 190   FGTaxiSegmentVectorIterator i = segments.begin();
 191   while(i != segments.end()) {
 192     //cerr << "initializing node " << i->getIndex() << endl;
 193     i->setStart(&nodes);
 194     i->setEnd  (&nodes);
 195     i->setTrackDistance();
 196     i->setIndex(index++);
 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 int FGGroundNetwork::findNearestNode(double lat, double lon)
 205 {
 206   double minDist = HUGE_VAL;
 207   double course, dist;
 208   int index;
 209   SGWayPoint first  (lon,
 210                      lat,
 211                      0);
 212
 213   for (FGTaxiNodeVectorIterator
 214          itr = nodes.begin();
 215        itr != nodes.end(); itr++)
 216     {
 217       double course;
 218       SGWayPoint second (itr->getLongitude(),
 219                          itr->getLatitude(),
 220                          0);
 221       first.CourseAndDistance(second, &course, &dist);
 222       if (dist < minDist)
 223         {
 224           minDist = dist;
 225           index = itr->getIndex();
 226           //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
 227         }
 228     }
 229   return index;
 230 }
 231
 232 FGTaxiNode *FGGroundNetwork::findNode(int idx)
 233 {
 234   for (FGTaxiNodeVectorIterator
 235          itr = nodes.begin();
 236        itr != nodes.end(); itr++)
 237     {
 238       if (itr->getIndex() == idx)
 239         return itr->getAddress();
 240     }
 241   return 0;
 242 }
 243
 244 FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
 245 {
 246   for (FGTaxiSegmentVectorIterator
 247          itr = segments.begin();
 248        itr != segments.end(); itr++)
 249     {
 250       if (itr->getIndex() == idx)
 251         return itr->getAddress();
 252     }
 253   return 0;
 254 }
 255
 256 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
 257 {
 258   foundRoute = false;
 259   totalDistance = 0;
 260   FGTaxiNode *firstNode = findNode(start);
 261   FGTaxiNode *lastNode  = findNode(end);
 262   //prevNode = prevPrevNode = -1;
 263   //prevNode = start;
 264   routes.clear();
 265   nodesStack.clear();
 266   routesStack.clear();
 267
 268   trace(firstNode, end, 0, 0);
 269   FGTaxiRoute empty;
 270
 271   if (!foundRoute)
 272     {
 273       SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
 274       exit(1);
 275     }
 276   sort(routes.begin(), routes.end());
 277   //for (intVecIterator i = route.begin(); i != route.end(); i++)
 278   //  {
 279   //    rte->push_back(*i);
 280   //  }
 281
 282   if (routes.begin() != routes.end())
 283     return *(routes.begin());
 284   else
 285     return empty;
 286 }
 287
 288
 289 void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance)
 290 {
 291   nodesStack.push_back(currNode->getIndex());
 292   totalDistance += distance;
 293   //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl;
 294   //<< currNode->getIndex() << endl;
 295
 296   // If the current route matches the required end point we found a valid route
 297   // So we can add this to the routing table
 298   if (currNode->getIndex() == end)
 299     {
 300       //cerr << "Found route : " <<  totalDistance << "" << " " << *(nodesStack.end()-1) << endl;
 301       routes.push_back(FGTaxiRoute(nodesStack,routesStack,totalDistance));
 302       nodesStack.pop_back();
 303       routesStack.pop_back();
 304       if (!(foundRoute))
 305         maxDistance = totalDistance;
 306       else
 307         if (totalDistance < maxDistance)
 308           maxDistance = totalDistance;
 309       foundRoute = true;
 310       totalDistance -= distance;
 311       return;
 312     }
 313
 314
 315   // search if the currentNode has been encountered before
 316   // if so, we should step back one level, because it is
 317   // rather rediculous to proceed further from here.
 318   // if the current node has not been encountered before,
 319   // i should point to nodesStack.end()-1; and we can continue
 320   // if i is not nodesStack.end, the previous node was found,
 321   // and we should return.
 322   // This only works at trace levels of 1 or higher though
 323   if (depth > 0) {
 324     intVecIterator i = nodesStack.begin();
 325     while ((*i) != currNode->getIndex()) {
 326       //cerr << "Route so far : " << (*i) << endl;
 327       i++;
 328     }
 329     if (i != nodesStack.end()-1) {
 330       nodesStack.pop_back();
 331       routesStack.pop_back();
 332       totalDistance -= distance;
 333       return;
 334     }
 335     // If the total distance from start to the current waypoint
 336     // is longer than that of a route we can also stop this trace
 337     // and go back one level.
 338     if ((totalDistance > maxDistance) && foundRoute)
 339       {
 340         //cerr << "Stopping rediculously long trace: " << totalDistance << endl;
 341         nodesStack.pop_back();
 342         routesStack.pop_back();
 343         totalDistance -= distance;
 344         return;
 345       }
 346   }
 347
 348   //cerr << "2" << endl;
 349   if (currNode->getBeginRoute() != currNode->getEndRoute())
 350     {
 351       //cerr << "3" << endl;
 352       for (FGTaxiSegmentPointerVectorIterator
 353              i = currNode->getBeginRoute();
 354            i != currNode->getEndRoute();
 355            i++)
 356         {
 357           //cerr << (*i)->getLength() << endl;
 358           //cerr << (*i)->getIndex() << endl;
 359           int idx = (*i)->getIndex();
 360           routesStack.push_back((*i)->getIndex());
 361           trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
 362         //  {
 363         //      // cerr << currNode -> getIndex() << " ";
 364         //      route.push_back(currNode->getIndex());
 365         //      return true;
 366         //    }
 367         }
 368     }
 369   else
 370     {
 371       SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
 372     }
 373   nodesStack.pop_back();
 374   routesStack.pop_back();
 375   totalDistance -= distance;
 376   return;
 377 }
 378