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
 108 bool FGTaxiRoute::next(int *val)
 109 {
 110   //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
 111   //  cerr << "FGTaxiRoute contains : " << *(i) << endl;
 112   //cerr << "Offset from end: " << nodes.end() - currNode << endl;
 113   //if (currNode != nodes.end())
 114   //  cerr << "true" << endl;
 115   //else
 116   //  cerr << "false" << endl;
 117
 118   if (currNode == nodes.end())
 119     return false;
 120   *val = *(currNode);
 121   currNode++;
 122   return true;
 123 };
 124 /***************************************************************************
 125  * FGGroundNetwork()
 126  **************************************************************************/
 127
 128 FGGroundNetwork::FGGroundNetwork()
 129 {
 130   hasNetwork = false;
 131   foundRoute = false;
 132   totalDistance = 0;
 133   maxDistance = 0;
 134 }
 135
 136 void FGGroundNetwork::addSegment(const FGTaxiSegment &seg)
 137 {
 138   segments.push_back(seg);
 139 }
 140
 141 void FGGroundNetwork::addNode(const FGTaxiNode &node)
 142 {
 143   nodes.push_back(node);
 144 }
 145
 146 void FGGroundNetwork::addNodes(FGParkingVec *parkings)
 147 {
 148   FGTaxiNode n;
 149   FGParkingVecIterator i = parkings->begin();
 150   while (i != parkings->end())
 151     {
 152       n.setIndex(i->getIndex());
 153       n.setLatitude(i->getLatitude());
 154       n.setLongitude(i->getLongitude());
 155       nodes.push_back(n);
 156
 157       i++;
 158     }
 159 }
 160
 161
 162
 163 void FGGroundNetwork::init()
 164 {
 165   hasNetwork = true;
 166   FGTaxiSegmentVectorIterator i = segments.begin();
 167   while(i != segments.end()) {
 168     //cerr << "initializing node " << i->getIndex() << endl;
 169     i->setStart(&nodes);
 170     i->setEnd  (&nodes);
 171     i->setTrackDistance();
 172     //cerr << "Track distance = " << i->getLength() << endl;
 173     //cerr << "Track ends at"      << i->getEnd()->getIndex() << endl;
 174     i++;
 175   }
 176   //exit(1);
 177 }
 178
 179 int FGGroundNetwork::findNearestNode(double lat, double lon)
 180 {
 181   double minDist = HUGE_VAL;
 182   double course, dist;
 183   int index;
 184   SGWayPoint first  (lon,
 185                      lat,
 186                      0);
 187
 188   for (FGTaxiNodeVectorIterator
 189          itr = nodes.begin();
 190        itr != nodes.end(); itr++)
 191     {
 192       double course;
 193       SGWayPoint second (itr->getLongitude(),
 194                          itr->getLatitude(),
 195                          0);
 196       first.CourseAndDistance(second, &course, &dist);
 197       if (dist < minDist)
 198         {
 199           minDist = dist;
 200           index = itr->getIndex();
 201           //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
 202         }
 203     }
 204   return index;
 205 }
 206
 207 FGTaxiNode *FGGroundNetwork::findNode(int idx)
 208 {
 209   for (FGTaxiNodeVectorIterator
 210          itr = nodes.begin();
 211        itr != nodes.end(); itr++)
 212     {
 213       if (itr->getIndex() == idx)
 214         return itr->getAddress();
 215     }
 216   return 0;
 217 }
 218
 219 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
 220 {
 221   foundRoute = false;
 222   totalDistance = 0;
 223   FGTaxiNode *firstNode = findNode(start);
 224   FGTaxiNode *lastNode  = findNode(end);
 225   //prevNode = prevPrevNode = -1;
 226   //prevNode = start;
 227   routes.clear();
 228   traceStack.clear();
 229   trace(firstNode, end, 0, 0);
 230   FGTaxiRoute empty;
 231
 232   if (!foundRoute)
 233     {
 234       SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
 235       exit(1);
 236     }
 237   sort(routes.begin(), routes.end());
 238   //for (intVecIterator i = route.begin(); i != route.end(); i++)
 239   //  {
 240   //    rte->push_back(*i);
 241   //  }
 242
 243   if (routes.begin() != routes.end())
 244     return *(routes.begin());
 245   else
 246     return empty;
 247 }
 248
 249
 250 void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance)
 251 {
 252   traceStack.push_back(currNode->getIndex());
 253   totalDistance += distance;
 254   //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl;
 255   //<< currNode->getIndex() << endl;
 256
 257   // If the current route matches the required end point we found a valid route
 258   // So we can add this to the routing table
 259   if (currNode->getIndex() == end)
 260     {
 261       //cerr << "Found route : " <<  totalDistance << "" << " " << *(traceStack.end()-1) << endl;
 262       routes.push_back(FGTaxiRoute(traceStack,totalDistance));
 263       traceStack.pop_back();
 264       if (!(foundRoute))
 265         maxDistance = totalDistance;
 266       else
 267         if (totalDistance < maxDistance)
 268           maxDistance = totalDistance;
 269       foundRoute = true;
 270       totalDistance -= distance;
 271       return;
 272     }
 273
 274
 275   // search if the currentNode has been encountered before
 276   // if so, we should step back one level, because it is
 277   // rather rediculous to proceed further from here.
 278   // if the current node has not been encountered before,
 279   // i should point to traceStack.end()-1; and we can continue
 280   // if i is not traceStack.end, the previous node was found,
 281   // and we should return.
 282   // This only works at trace levels of 1 or higher though
 283   if (depth > 0) {
 284     intVecIterator i = traceStack.begin();
 285     while ((*i) != currNode->getIndex()) {
 286       //cerr << "Route so far : " << (*i) << endl;
 287       i++;
 288     }
 289     if (i != traceStack.end()-1) {
 290       traceStack.pop_back();
 291       totalDistance -= distance;
 292       return;
 293     }
 294     // If the total distance from start to the current waypoint
 295     // is longer than that of a route we can also stop this trace
 296     // and go back one level.
 297     if ((totalDistance > maxDistance) && foundRoute)
 298       {
 299         //cerr << "Stopping rediculously long trace: " << totalDistance << endl;
 300         traceStack.pop_back();
 301         totalDistance -= distance;
 302         return;
 303       }
 304   }
 305
 306   //cerr << "2" << endl;
 307   if (currNode->getBeginRoute() != currNode->getEndRoute())
 308     {
 309       //cerr << "3" << endl;
 310       for (FGTaxiSegmentPointerVectorIterator
 311              i = currNode->getBeginRoute();
 312            i != currNode->getEndRoute();
 313            i++)
 314         {
 315           //cerr << (*i)->getLenght() << endl;
 316           trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
 317         //  {
 318         //      // cerr << currNode -> getIndex() << " ";
 319         //      route.push_back(currNode->getIndex());
 320         //      return true;
 321         //    }
 322         }
 323     }
 324   else
 325     {
 326       SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
 327     }
 328   traceStack.pop_back();
 329   totalDistance -= distance;
 330   return;
 331 }
 332