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 // cerr << "ALERT: Misconfigured TaxiRoute : " << nodes.size() << " " << routes.size() << endl;
139 if (currNode == nodes.end())
147 /***************************************************************************
149 **************************************************************************/
151 FGGroundNetwork::FGGroundNetwork()
159 void FGGroundNetwork::addSegment(const FGTaxiSegment &seg)
161 segments.push_back(seg);
164 void FGGroundNetwork::addNode(const FGTaxiNode &node)
166 nodes.push_back(node);
169 void FGGroundNetwork::addNodes(FGParkingVec *parkings)
172 FGParkingVecIterator i = parkings->begin();
173 while (i != parkings->end())
175 n.setIndex(i->getIndex());
176 n.setLatitude(i->getLatitude());
177 n.setLongitude(i->getLongitude());
186 void FGGroundNetwork::init()
190 FGTaxiSegmentVectorIterator i = segments.begin();
191 while(i != segments.end()) {
192 //cerr << "initializing node " << i->getIndex() << endl;
195 i->setTrackDistance();
196 i->setIndex(index++);
197 //cerr << "Track distance = " << i->getLength() << endl;
198 //cerr << "Track ends at" << i->getEnd()->getIndex() << endl;
204 int FGGroundNetwork::findNearestNode(double lat, double lon)
206 double minDist = HUGE_VAL;
209 SGWayPoint first (lon,
213 for (FGTaxiNodeVectorIterator
215 itr != nodes.end(); itr++)
218 SGWayPoint second (itr->getLongitude(),
221 first.CourseAndDistance(second, &course, &dist);
225 index = itr->getIndex();
226 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
232 FGTaxiNode *FGGroundNetwork::findNode(int idx)
234 for (FGTaxiNodeVectorIterator
236 itr != nodes.end(); itr++)
238 if (itr->getIndex() == idx)
239 return itr->getAddress();
244 FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
246 for (FGTaxiSegmentVectorIterator
247 itr = segments.begin();
248 itr != segments.end(); itr++)
250 if (itr->getIndex() == idx)
251 return itr->getAddress();
256 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
260 FGTaxiNode *firstNode = findNode(start);
261 FGTaxiNode *lastNode = findNode(end);
262 //prevNode = prevPrevNode = -1;
268 trace(firstNode, end, 0, 0);
273 SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
276 sort(routes.begin(), routes.end());
277 //for (intVecIterator i = route.begin(); i != route.end(); i++)
279 // rte->push_back(*i);
282 if (routes.begin() != routes.end())
283 return *(routes.begin());
289 void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance)
291 nodesStack.push_back(currNode->getIndex());
292 totalDistance += distance;
293 //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl;
294 //<< currNode->getIndex() << endl;
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)
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();
305 maxDistance = totalDistance;
307 if (totalDistance < maxDistance)
308 maxDistance = totalDistance;
310 totalDistance -= distance;
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
324 intVecIterator i = nodesStack.begin();
325 while ((*i) != currNode->getIndex()) {
326 //cerr << "Route so far : " << (*i) << endl;
329 if (i != nodesStack.end()-1) {
330 nodesStack.pop_back();
331 routesStack.pop_back();
332 totalDistance -= distance;
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)
340 //cerr << "Stopping rediculously long trace: " << totalDistance << endl;
341 nodesStack.pop_back();
342 routesStack.pop_back();
343 totalDistance -= distance;
348 //cerr << "2" << endl;
349 if (currNode->getBeginRoute() != currNode->getEndRoute())
351 //cerr << "3" << endl;
352 for (FGTaxiSegmentPointerVectorIterator
353 i = currNode->getBeginRoute();
354 i != currNode->getEndRoute();
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());
363 // // cerr << currNode -> getIndex() << " ";
364 // route.push_back(currNode->getIndex());
371 SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
373 nodesStack.pop_back();
374 routesStack.pop_back();
375 totalDistance -= distance;