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