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);
108 bool FGTaxiRoute::next(int *val)
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;
116 // cerr << "false" << endl;
118 if (currNode == nodes.end())
124 /***************************************************************************
126 **************************************************************************/
128 FGGroundNetwork::FGGroundNetwork()
136 void FGGroundNetwork::addSegment(const FGTaxiSegment &seg)
138 segments.push_back(seg);
141 void FGGroundNetwork::addNode(const FGTaxiNode &node)
143 nodes.push_back(node);
146 void FGGroundNetwork::addNodes(FGParkingVec *parkings)
149 FGParkingVecIterator i = parkings->begin();
150 while (i != parkings->end())
152 n.setIndex(i->getIndex());
153 n.setLatitude(i->getLatitude());
154 n.setLongitude(i->getLongitude());
163 void FGGroundNetwork::init()
166 FGTaxiSegmentVectorIterator i = segments.begin();
167 while(i != segments.end()) {
168 //cerr << "initializing node " << i->getIndex() << endl;
171 i->setTrackDistance();
172 //cerr << "Track distance = " << i->getLength() << endl;
173 //cerr << "Track ends at" << i->getEnd()->getIndex() << endl;
179 int FGGroundNetwork::findNearestNode(double lat, double lon)
181 double minDist = HUGE_VAL;
184 SGWayPoint first (lon,
188 for (FGTaxiNodeVectorIterator
190 itr != nodes.end(); itr++)
193 SGWayPoint second (itr->getLongitude(),
196 first.CourseAndDistance(second, &course, &dist);
200 index = itr->getIndex();
201 //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
207 FGTaxiNode *FGGroundNetwork::findNode(int idx)
209 for (FGTaxiNodeVectorIterator
211 itr != nodes.end(); itr++)
213 if (itr->getIndex() == idx)
214 return itr->getAddress();
219 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end)
223 FGTaxiNode *firstNode = findNode(start);
224 FGTaxiNode *lastNode = findNode(end);
225 //prevNode = prevPrevNode = -1;
229 trace(firstNode, end, 0, 0);
234 SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
237 sort(routes.begin(), routes.end());
238 //for (intVecIterator i = route.begin(); i != route.end(); i++)
240 // rte->push_back(*i);
243 if (routes.begin() != routes.end())
244 return *(routes.begin());
250 void FGGroundNetwork::trace(FGTaxiNode *currNode, int end, int depth, double distance)
252 traceStack.push_back(currNode->getIndex());
253 totalDistance += distance;
254 //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl;
255 //<< currNode->getIndex() << endl;
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)
261 //cerr << "Found route : " << totalDistance << "" << " " << *(traceStack.end()-1) << endl;
262 routes.push_back(FGTaxiRoute(traceStack,totalDistance));
263 traceStack.pop_back();
265 maxDistance = totalDistance;
267 if (totalDistance < maxDistance)
268 maxDistance = totalDistance;
270 totalDistance -= distance;
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
284 intVecIterator i = traceStack.begin();
285 while ((*i) != currNode->getIndex()) {
286 //cerr << "Route so far : " << (*i) << endl;
289 if (i != traceStack.end()-1) {
290 traceStack.pop_back();
291 totalDistance -= distance;
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)
299 //cerr << "Stopping rediculously long trace: " << totalDistance << endl;
300 traceStack.pop_back();
301 totalDistance -= distance;
306 //cerr << "2" << endl;
307 if (currNode->getBeginRoute() != currNode->getEndRoute())
309 //cerr << "3" << endl;
310 for (FGTaxiSegmentPointerVectorIterator
311 i = currNode->getBeginRoute();
312 i != currNode->getEndRoute();
315 //cerr << (*i)->getLenght() << endl;
316 trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
318 // // cerr << currNode -> getIndex() << " ";
319 // route.push_back(currNode->getIndex());
326 SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
328 traceStack.pop_back();
329 totalDistance -= distance;