2 // by Durk Talsma, started June 2005.
4 // Copyright (C) 2004 Durk Talsma.
6 // This program is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU General Public License as
8 // published by the Free Software Foundation; either version 2 of the
9 // License, or (at your option) any later version.
11 // This program is distributed in the hope that it will be useful, but
12 // WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Public License for more details.
16 // You should have received a copy of the GNU General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
30 #include <simgear/compiler.h>
32 #include <simgear/debug/logstream.hxx>
33 #include <simgear/misc/sgstream.hxx>
34 #include <simgear/route/waypoint.hxx>
35 #include <simgear/misc/sg_path.hxx>
44 /**************************************************************************
46 *************************************************************************/
51 FGNode::FGNode(const SGGeod& aPos, int idx, std::string id) :
56 cart = SGVec3d::fromGeod(geod);
59 bool FGNode::matches(std::string id, const SGGeod& aPos)
62 (fabs(aPos.getLatitudeDeg() - geod.getLatitudeDeg()) < 1.0) &&
63 (fabs(aPos.getLongitudeDeg() - geod.getLongitudeDeg()) < 1.0))
69 /***************************************************************************
71 **************************************************************************/
77 void FGAirway::setStart(node_map *nodes)
79 node_map_iterator itr = nodes->find(startNode);
80 if (itr == nodes->end()) {
81 cerr << "Couldn't find node: " << startNode << endl;
84 start = itr->second->getAddress();
85 itr->second->addAirway(this);
89 void FGAirway::setEnd(node_map *nodes)
91 node_map_iterator itr = nodes->find(endNode);
92 if (itr == nodes->end()) {
93 cerr << "Couldn't find node: " << endNode << endl;
96 end = itr->second->getAddress();
100 // There is probably a computationally cheaper way of
102 void FGAirway::setTrackDistance()
104 length = SGGeodesy::distanceM(start->getPosition(), end->getPosition());
107 /***************************************************************************
109 **************************************************************************/
112 bool FGAirRoute::next(int *val)
114 //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++)
115 // cerr << "FGTaxiRoute contains : " << *(i) << endl;
116 //cerr << "Offset from end: " << nodes.end() - currNode << endl;
117 //if (currNode != nodes.end())
118 // cerr << "true" << endl;
120 // cerr << "false" << endl;
122 if (currNode == nodes.end())
129 void FGAirRoute::add(const FGAirRoute &other) {
130 for (constIntVecIterator i = other.nodes.begin() ;
131 i != other.nodes.end(); i++)
133 nodes.push_back((*i));
135 distance += other.distance;
137 /***************************************************************************
139 **************************************************************************/
141 FGAirwayNetwork::FGAirwayNetwork()
149 FGAirwayNetwork::~FGAirwayNetwork()
151 for (unsigned int it = 0; it < nodes.size(); it++) {
155 void FGAirwayNetwork::addAirway(const FGAirway &seg)
157 segments.push_back(seg);
160 //void FGAirwayNetwork::addNode(const FGNode &node)
162 // nodes.push_back(node);
166 void FGAirwayNetwork::addNodes(FGParkingList *parkings)
169 FGParkingList::iterator i = parkings->begin();
170 while (i != parkings->end())
172 n.setIndex(i->getIndex());
173 n.setLatitude(i->getLatitude());
174 n.setLongitude(i->getLongitude());
183 void FGAirwayNetwork::init()
186 FGAirwayVectorIterator i = segments.begin();
187 while(i != segments.end()) {
188 //cerr << "initializing Airway " << i->getIndex() << endl;
189 i->setStart(&nodesMap);
190 i->setEnd (&nodesMap);
191 //i->setTrackDistance();
192 //cerr << "Track distance = " << i->getLength() << endl;
193 //cerr << "Track ends at" << i->getEnd()->getIndex() << endl;
200 void FGAirwayNetwork::load(const SGPath& path)
202 std::string identStart, identEnd, token, name;
203 double latStart, lonStart, latEnd, lonEnd;
208 sg_gzifstream in( path.str() );
209 if ( !in.is_open() ) {
210 SG_LOG( SG_NAVAID, SG_ALERT, "Cannot open file: " << path.str() );
213 // toss the first two lines of the file
217 // read in each remaining line of the file
218 while ( ! in.eof() ) {
222 if ( token == "99" ) {
223 return; //in >> skipeol;
225 // Read each line from the database
227 in >> latStart >> lonStart >> identEnd >> latEnd >> lonEnd >> type >> base >> top >> name;
229 /*out << identStart << " "
240 //first determine whether the start and end reference database already exist
241 //if not we should create them
242 int startIndex = 0, endIndex=0;
243 // FGNodeVectorIterator i = nodes.begin();
244 // while (i != nodes.end() && (!(i->matches(identStart,latStart, lonStart))))
249 // if (i == nodes.end())
251 // nodes.push_back(FGNode(latStart, lonStart, startIndex, identStart));
252 // //cout << "Adding node: " << identStart << endl;
255 // i = nodes.begin();
256 // while (i != nodes.end() && (!(i->matches(identEnd,latEnd, lonEnd)))) {
260 // if (i == nodes.end()) {
261 // nodes.push_back(FGNode(latEnd, lonEnd, endIndex, identEnd));
262 // //cout << "Adding node: " << identEnd << endl;
264 // generate unique IDs for the nodes, consisting of a combination
265 // of the Navaid identifier + the integer value of the lat/lon position.
266 // identifier alone will not suffice, because they might not be globally unique.
268 string startNode, endNode;
270 buffer[sizeof(buffer)-1] = 0;
271 snprintf(buffer, sizeof(buffer)-1, "%s%d%d", identStart.c_str(), (int) latStart, (int) lonStart);
274 node_map_iterator itr = nodesMap.find(string(buffer));
275 if (itr == nodesMap.end()) {
276 startIndex = nodes.size();
277 SGGeod startPos(SGGeod::fromDeg(lonStart, latStart));
278 n = new FGNode(startPos, startIndex, identStart);
279 nodesMap[string(buffer)] = n;
281 //cout << "Adding node: " << identStart << endl;
284 startIndex = itr->second->getIndex();
287 snprintf(buffer, 32, "%s%d%d", identEnd.c_str(), (int) latEnd, (int) lonEnd);
290 itr = nodesMap.find(string(buffer));
291 if (itr == nodesMap.end()) {
292 endIndex = nodes.size();
293 SGGeod endPos(SGGeod::fromDeg(lonEnd, latEnd));
294 n = new FGNode(endPos, endIndex, identEnd);
295 nodesMap[string(buffer)] = n;
297 //cout << "Adding node: " << identEnd << endl;
300 endIndex = itr->second->getIndex();
305 airway.setIndex ( airwayIndex++ );
306 airway.setStartNodeRef ( startNode );
307 airway.setEndNodeRef ( endNode );
308 airway.setType ( type );
309 airway.setBase ( base );
310 airway.setTop ( top );
311 airway.setName ( name );
312 segments.push_back(airway);
313 //cout << " Adding Airway: " << name << " " << startIndex << " " << endIndex << endl;
317 int FGAirwayNetwork::findNearestNode(const SGGeod& aPos)
319 double minDist = HUGE_VAL;
321 SGVec3d cart = SGVec3d::fromGeod(aPos);
323 //cerr << "Lat " << lat << " lon " << lon << endl;
324 for (FGNodeVectorIterator
326 itr != nodes.end(); itr++)
328 double d2 = distSqr(cart, (*itr)->getCart());
332 index = (*itr)->getIndex();
334 //cerr << (*itr)->getIndex() << endl;
336 //cerr << " returning " << index << endl;
340 FGNode *FGAirwayNetwork::findNode(int idx)
342 for (FGNodeVectorIterator
344 itr != nodes.end(); itr++)
346 if ((*itr)->getIndex() == idx)
347 return (*itr)->getAddress();
352 FGAirRoute FGAirwayNetwork::findShortestRoute(int start, int end)
356 FGNode *firstNode = findNode(start);
357 //FGNode *lastNode = findNode(end);
358 //prevNode = prevPrevNode = -1;
362 trace(firstNode, end, 0, 0);
367 SG_LOG( SG_NAVAID, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
368 cerr << "Failed to find route from waypoint " << start << " to " << end << endl;
371 sort(routes.begin(), routes.end());
372 //for (intVecIterator i = route.begin(); i != route.end(); i++)
374 // rte->push_back(*i);
377 if (routes.begin() != routes.end())
378 return *(routes.begin());
384 void FGAirwayNetwork::trace(FGNode *currNode, int end, int depth, double distance)
386 traceStack.push_back(currNode->getIndex());
387 totalDistance += distance;
388 //cerr << depth << " ";
389 //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl;
390 //<< currNode->getIndex() << endl;
392 // If the current route matches the required end point we found a valid route
393 // So we can add this to the routing table
394 if (currNode->getIndex() == end)
396 cerr << "Found route : " << totalDistance << "" << " " << *(traceStack.end()-1) << endl;
397 routes.push_back(FGAirRoute(traceStack,totalDistance));
398 traceStack.pop_back();
400 maxDistance = totalDistance;
402 if (totalDistance < maxDistance)
403 maxDistance = totalDistance;
405 totalDistance -= distance;
410 // search if the currentNode has been encountered before
411 // if so, we should step back one level, because it is
412 // rather rediculous to proceed further from here.
413 // if the current node has not been encountered before,
414 // i should point to traceStack.end()-1; and we can continue
415 // if i is not traceStack.end, the previous node was found,
416 // and we should return.
417 // This only works at trace levels of 1 or higher though
419 intVecIterator i = traceStack.begin();
420 while ((*i) != currNode->getIndex()) {
421 //cerr << "Route so far : " << (*i) << endl;
424 if (i != traceStack.end()-1) {
425 traceStack.pop_back();
426 totalDistance -= distance;
429 // If the total distance from start to the current waypoint
430 // is longer than that of a route we can also stop this trace
431 // and go back one level.
432 if ((totalDistance > maxDistance) && foundRoute)
434 cerr << "Stopping rediculously long trace: " << totalDistance << endl;
435 traceStack.pop_back();
436 totalDistance -= distance;
441 //cerr << "2" << endl;
442 if (currNode->getBeginRoute() != currNode->getEndRoute())
444 //cerr << "l3l" << endl;
445 for (FGAirwayPointerVectorIterator
446 i = currNode->getBeginRoute();
447 i != currNode->getEndRoute();
450 //cerr << (*i)->getLenght() << endl;
451 trace((*i)->getEnd(), end, depth+1, (*i)->getLength());
453 // // cerr << currNode -> getIndex() << " ";
454 // route.push_back(currNode->getIndex());
461 //SG_LOG( SG_NAVAID, SG_DEBUG, "4" );
462 //cerr << "4" << endl;
464 traceStack.pop_back();
465 totalDistance -= distance;