1 // groundnet.cxx - Implimentation of the FlightGear airport ground handling code
3 // Written by Durk Talsma, started June 2005.
5 // Copyright (C) 2004 Durk Talsma.
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.
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.
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.
30 //#include <plib/sg.h>
31 //#include <plib/ul.h>
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>
46 #include "groundnetwork.hxx"
50 /**************************************************************************
52 *************************************************************************/
53 FGTaxiNode::FGTaxiNode()
57 /***************************************************************************
59 **************************************************************************/
60 FGTaxiSegment::FGTaxiSegment()
64 void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes)
66 FGTaxiNodeVectorIterator i = nodes->begin();
67 while (i != nodes->end())
69 if (i->getIndex() == startNode)
71 start = i->getAddress();
79 void FGTaxiSegment::setEnd(FGTaxiNodeVector *nodes)
81 FGTaxiNodeVectorIterator i = nodes->begin();
82 while (i != nodes->end())
84 if (i->getIndex() == endNode)
86 end = i->getAddress();
93 // There is probably a computationally cheaper way of
95 void FGTaxiSegment::setTrackDistance()
98 SGWayPoint first (start->getLongitude(),
101 SGWayPoint second (end->getLongitude(),
104 first.CourseAndDistance(second, &course, &length);
107 bool FGTaxiRoute::next(int *nde)
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;
115 // cerr << "false" << endl;
116 //if (nodes.size() != (routes.size()) +1)
117 // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
119 if (currNode == nodes.end())
127 bool FGTaxiRoute::next(int *nde, int *rte)
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;
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());
140 if (currNode == nodes.end())
148 /***************************************************************************
150 **************************************************************************/
152 FGGroundNetwork::FGGroundNetwork()
160 void FGGroundNetwork::addSegment(const FGTaxiSegment &seg)
162 segments.push_back(seg);
165 void FGGroundNetwork::addNode(const FGTaxiNode &node)
167 nodes.push_back(node);
170 void FGGroundNetwork::addNodes(FGParkingVec *parkings)
173 FGParkingVecIterator i = parkings->begin();
174 while (i != parkings->end())
176 n.setIndex(i->getIndex());
177 n.setLatitude(i->getLatitude());
178 n.setLongitude(i->getLongitude());
187 void FGGroundNetwork::init()
191 FGTaxiSegmentVectorIterator i = segments.begin();
192 while(i != segments.end()) {
193 //cerr << "initializing node " << i->getIndex() << endl;
196 i->setTrackDistance();
197 i->setIndex(index++);
198 //cerr << "Track distance = " << i->getLength() << endl;
199 //cerr << "Track ends at" << i->getEnd()->getIndex() << endl;
205 int FGGroundNetwork::findNearestNode(double lat, double lon)
207 double minDist = HUGE_VAL;
210 SGWayPoint first (lon,
214 for (FGTaxiNodeVectorIterator
216 itr != nodes.end(); itr++)
219 SGWayPoint second (itr->getLongitude(),
222 first.CourseAndDistance(second, &course, &dist);
226 index = itr->getIndex();
227 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
233 FGTaxiNode *FGGroundNetwork::findNode(int idx)
235 for (FGTaxiNodeVectorIterator
237 itr != nodes.end(); itr++)
239 if (itr->getIndex() == idx)
240 return itr->getAddress();
245 FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
247 for (FGTaxiSegmentVectorIterator
248 itr = segments.begin();
249 itr != segments.end(); itr++)
251 if (itr->getIndex() == idx)
252 return itr->getAddress();
257 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
261 FGTaxiNode *firstNode = findNode(start);
262 FGTaxiNode *lastNode = findNode(end);
263 //prevNode = prevPrevNode = -1;
269 trace(firstNode, end, 0, 0);
274 SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
277 sort(routes.begin(), routes.end());
278 //for (intVecIterator i = route.begin(); i != route.end(); i++)
280 // rte->push_back(*i);
283 if (routes.begin() != routes.end())
284 return *(routes.begin());
290 void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance)
292 // Just check some preconditions of the trace algorithm
293 if (nodesStack.size() != routesStack.size())
295 SG_LOG(SG_GENERAL, SG_ALERT, "size of nodesStack and routesStack is not equal. NodesStack :"
296 << nodesStack.size() << ". RoutesStack : " << routesStack.size());
298 nodesStack.push_back(currNode->getIndex());
299 totalDistance += distance;
300 //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl;
301 //<< currNode->getIndex() << endl;
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)
307 //cerr << "Found route : " << totalDistance << "" << " " << *(nodesStack.end()-1) << endl;
308 routes.push_back(FGTaxiRoute(nodesStack,routesStack,totalDistance));
309 if (nodesStack.empty() || routesStack.empty())
311 printRoutingError(string("while finishing route"));
313 nodesStack.pop_back();
314 routesStack.pop_back();
316 maxDistance = totalDistance;
318 if (totalDistance < maxDistance)
319 maxDistance = totalDistance;
321 totalDistance -= distance;
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
335 intVecIterator i = nodesStack.begin();
336 while ((*i) != currNode->getIndex()) {
337 //cerr << "Route so far : " << (*i) << endl;
340 if (i != nodesStack.end()-1) {
341 if (nodesStack.empty() || routesStack.empty())
343 printRoutingError(string("while returning from an already encountered node"));
345 nodesStack.pop_back();
346 routesStack.pop_back();
347 totalDistance -= distance;
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)
355 //cerr << "Stopping rediculously long trace: " << totalDistance << endl;
356 if (nodesStack.empty() || routesStack.empty())
358 printRoutingError(string("while returning from finding a rediculously long route"));
360 nodesStack.pop_back();
361 routesStack.pop_back();
362 totalDistance -= distance;
367 //cerr << "2" << endl;
368 if (currNode->getBeginRoute() != currNode->getEndRoute())
370 //cerr << "3" << endl;
371 for (FGTaxiSegmentPointerVectorIterator
372 i = currNode->getBeginRoute();
373 i != currNode->getEndRoute();
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());
382 // // cerr << currNode -> getIndex() << " ";
383 // route.push_back(currNode->getIndex());
390 //SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
392 if (nodesStack.empty())
394 printRoutingError(string("while finishing trace"));
396 nodesStack.pop_back();
397 // Make sure not to dump the level-zero routesStack entry, because that was never created.
400 routesStack.pop_back();
401 //cerr << "leaving trace " << routesStack.size() << endl;
403 totalDistance -= distance;
407 void FGGroundNetwork::printRoutingError(string mess)
409 SG_LOG(SG_GENERAL, SG_ALERT, "Error in ground network trace algorithm " << mess);
410 if (nodesStack.empty())
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));
416 if (routesStack.empty())
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));