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 <simgear/compiler.h>
  31
  32 //#include <plib/sg.h>
  33 //#include <plib/ul.h>
  34
  35 //#include <Environment/environment_mgr.hxx>
  36 //#include <Environment/environment.hxx>
  37 //#include <simgear/misc/sg_path.hxx>
  38 //#include <simgear/props/props.hxx>
  39 //#include <simgear/structure/subsystem_mgr.hxx>
  40 #include <simgear/debug/logstream.hxx>
  41 #include <simgear/route/waypoint.hxx>
  42 //#include <Main/globals.hxx>
  43 //#include <Main/fg_props.hxx>
  44 //#include <Airports/runways.hxx>
  45
  46 //#include STL_STRING
  47
  48 #include "groundnetwork.hxx"
  49
  50 SG_USING_STD(sort);
  51
  52 /**************************************************************************
  53  * FGTaxiNode
  54  *************************************************************************/
  55 FGTaxiNode::FGTaxiNode()
  56 {
  57 }
  58
  59 /***************************************************************************
  60  * FGTaxiSegment
  61  **************************************************************************/
  62 FGTaxiSegment::FGTaxiSegment()
  63 {
  64 }
  65
  66 void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes)
  67 {
  68   FGTaxiNodeVectorIterator i = nodes->begin();
  69   while (i != nodes->end())
  70     {
  71       if (i->getIndex() == startNode)
  72         {
  73           start = i->getAddress();
  74           i->addSegment(this);
  75           return;
  76         }
  77       i++;
  78     }
  79 }
  80
  81 void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes)
  82 {
  83   FGTaxiNodeVectorIterator i = nodes->begin();
  84   while (i != nodes->end())
  85     {
  86       if (i->getIndex() == endNode)
  87         {
  88           end = i->getAddress();
  89           return;
  90         }
  91       i++;
  92     }
  93 }
  94
  95 // There is probably a computationally cheaper way of
  96 // doing this.
  97 void FGTaxiSegment::setTrackDistance()
  98 {
  99   double course;
 100   SGWayPoint first  (start->getLongitude(),
 101                      start->getLatitude(),
 102                      0);
 103   SGWayPoint second (end->getLongitude(),
 104                      end->getLatitude(),
 105                      0);
 106   first.CourseAndDistance(second, &course, &length);
 107
 108 }
 109
 110 bool FGTaxiRoute::next(int *val)
 111 {
 112   //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 113   //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 114   //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 115   //if (currNode != nodes.end())
 116   //  cerr << "true" << endl;
 117   //else
 118   //  cerr << "false" << endl;
 119
 120   if (currNode == nodes.end())
 121     return false;
 122   *val = *(currNode);
 123   currNode++;
 124   return true;
 125 };
 126 /***************************************************************************
 127  * FGGroundNetwork()
 128  **************************************************************************/
 129
 130 FGGroundNetwork::FGGroundNetwork()
 131 {
 132   hasNetwork = false;
 133   foundRoute = false;
 134   totalDistance = 0;
 135   maxDistance = 0;
 136 }
 137
 138 void FGGroundNetwork::addSegment(const FGTaxiSegment &seg)
 139 {
 140   segments.push_back(seg);
 141 }
 142
 143 void FGGroundNetwork::addNode(const FGTaxiNode &node)
 144 {
 145   nodes.push_back(node);
 146 }
 147
 148 void FGGroundNetwork::addNodes(FGParkingVec *parkings)
 149 {
 150   FGTaxiNode n;
 151   FGParkingVecIterator i = parkings->begin();
 152   while (i != parkings->end())
 153     {
 154       n.setIndex(i->getIndex());
 155       n.setLatitude(i->getLatitude());
 156       n.setLongitude(i->getLongitude());
 157       nodes.push_back(n);
 158
 159       i++;
 160     }
 161 }
 162
 163
 164
 165 void FGGroundNetwork::init()
 166 {
 167   hasNetwork = true;
 168   FGTaxiSegmentVectorIterator i = segments.begin();
 169   while(i != segments.end()) {
 170     //cerr << "initializing node " << i->getIndex() << endl;
 171     i->setStart(&nodes);
 172     i->setEnd  (&nodes);
 173     i->setTrackDistance();
 174     //cerr << "Track distance = " << i->getLength() << endl;
 175     //cerr << "Track ends at"      << i->getEnd()->getIndex() << endl;
 176     i++;
 177   }
 178   //exit(1);
 179 }
 180
 181 int FGGroundNetwork::findNearestNode(double lat, double lon)
 182 {
 183   double minDist = HUGE_VAL;
 184   double course, dist;
 185   int index;
 186   SGWayPoint first  (lon,
 187                      lat,
 188                      0);
 189
 190   for (FGTaxiNodeVectorIterator
 191          itr = nodes.begin();
 192        itr != nodes.end(); itr++)
 193     {
 194       double course;
 195       SGWayPoint second (itr->getLongitude(),
 196                          itr->getLatitude(),
 197                          0);
 198       first.CourseAndDistance(second, &course, &dist);
 199       if (dist < minDist)
 200         {
 201           minDist = dist;
 202           index = itr->getIndex();
 203           //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
 204         }
 205     }
 206   return index;
 207 }
 208
 209 FGTaxiNode *FGGroundNetwork::findNode(int idx)
 210 {
 211   for (FGTaxiNodeVectorIterator
 212          itr = nodes.begin();
 213        itr != nodes.end(); itr++)
 214     {
 215       if (itr->getIndex() == idx)
 216         return itr->getAddress();
 217     }
 218   return 0;
 219 }
 220
 221 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
 222 {
 223   foundRoute = false;
 224   totalDistance = 0;
 225   FGTaxiNode *firstNode = findNode(start);
 226   FGTaxiNode *lastNode  = findNode(end);
 227   //prevNode = prevPrevNode = -1;
 228   //prevNode = start;
 229   routes.clear();
 230   traceStack.clear();
 231   trace(firstNode, end, 0, 0);
 232   FGTaxiRoute empty;
 233
 234   if (!foundRoute)
 235     {
 236       SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
 237       exit(1);
 238     }
 239   sort(routes.begin(), routes.end());
 240   //for (intVecIterator i = route.begin(); i != route.end(); i++)
 241   //  {
 242   //    rte->push_back(*i);
 243   //  }
 244
 245   if (routes.begin() != routes.end())
 246     return *(routes.begin());
 247   else
 248     return empty;
 249 }
 250
 251
 252 void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance)
 253 {
 254   traceStack.push_back(currNode->getIndex());
 255   totalDistance += distance;
 256   //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl;
 257   //<< currNode->getIndex() << endl;
 258
 259   // If the current route matches the required end point we found a valid route
 260   // So we can add this to the routing table
 261   if (currNode->getIndex() == end)
 262     {
 263       //cerr << "Found route : " <<  totalDistance << "" << " " << *(traceStack.end()-1) << endl;
 264       routes.push_back(FGTaxiRoute(traceStack,totalDistance));
 265       traceStack.pop_back();
 266       if (!(foundRoute))
 267         maxDistance = totalDistance;
 268       else
 269         if (totalDistance < maxDistance)
 270           maxDistance = totalDistance;
 271       foundRoute = true;
 272       totalDistance -= distance;
 273       return;
 274     }
 275
 276
 277   // search if the currentNode has been encountered before
 278   // if so, we should step back one level, because it is
 279   // rather rediculous to proceed further from here.
 280   // if the current node has not been encountered before,
 281   // i should point to traceStack.end()-1; and we can continue
 282   // if i is not traceStack.end, the previous node was found,
 283   // and we should return.
 284   // This only works at trace levels of 1 or higher though
 285   if (depth > 0) {
 286     intVecIterator i = traceStack.begin();
 287     while ((*i) != currNode->getIndex()) {
 288       //cerr << "Route so far : " << (*i) << endl;
 289       i++;
 290     }
 291     if (i != traceStack.end()-1) {
 292       traceStack.pop_back();
 293       totalDistance -= distance;
 294       return;
 295     }
 296     // If the total distance from start to the current waypoint
 297     // is longer than that of a route we can also stop this trace
 298     // and go back one level.
 299     if ((totalDistance > maxDistance) && foundRoute)
 300       {
 301         //cerr << "Stopping rediculously long trace: " << totalDistance << endl;
 302         traceStack.pop_back();
 303         totalDistance -= distance;
 304         return;
 305       }
 306   }
 307
 308   //cerr << "2" << endl;
 309   if (currNode->getBeginRoute() != currNode->getEndRoute())
 310     {
 311       //cerr << "3" << endl;
 312       for (FGTaxiSegmentPointerVectorIterator
 313              i = currNode->getBeginRoute();
 314            i != currNode->getEndRoute();
 315            i++)
 316         {
 317           //cerr << (*i)->getLenght() << endl;
 318           trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
 319         //  {
 320         //      // cerr << currNode -> getIndex() << " ";
 321         //      route.push_back(currNode->getIndex());
 322         //      return true;
 323         //    }
 324         }
 325     }
 326   else
 327     {
 328       SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
 329     }
 330   traceStack.pop_back();
 331   totalDistance -= distance;
 332   return;
 333 }
 334