diff --git a/src/Navaids/Makefile.am b/src/Navaids/Makefile.am index b54fc97de..7810dd4e5 100644 --- a/src/Navaids/Makefile.am +++ b/src/Navaids/Makefile.am @@ -5,6 +5,7 @@ noinst_LIBRARIES = libNavaids.a libNavaids_a_SOURCES = \ navdb.hxx navdb.cxx \ fix.hxx fixlist.hxx fixlist.cxx \ + awynet.hxx awynet.cxx \ navrecord.hxx navlist.hxx navlist.cxx # ils.hxx ilslist.hxx ilslist.cxx \ diff --git a/src/Navaids/awynet.cxx b/src/Navaids/awynet.cxx new file mode 100755 index 000000000..5a76a521c --- /dev/null +++ b/src/Navaids/awynet.cxx @@ -0,0 +1,496 @@ +// awynet.cxx +// by Durk Talsma, started June 2005. +// +// Copyright (C) 2004 Durk Talsma. +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// +// $Id$ + +#ifdef HAVE_CONFIG_H +# include +#endif + +#ifdef _MSC_VER +# define _USE_MATH_DEFINES +#endif +#include +#include + +#include + +//#include +//#include + +//#include +//#include +//#include +//#include +//#include +#include +#include +#include +//#include
+//#include
+//#include + +//#include STL_STRING + +#include "awynet.hxx" + +SG_USING_STD(sort); + +/************************************************************************** + * FGNode + *************************************************************************/ +FGNode::FGNode() +{ +} + +bool FGNode::matches(string id, double lt, double ln) +{ + if ((ident == id) && + (fabs(lt - lat) < 1.0) && + (fabs(ln - lon) < 1.0)) + return true; + else + return false; +} + +/*************************************************************************** + * FGAirway + **************************************************************************/ +FGAirway::FGAirway() +{ + length = 0; +} + +void FGAirway::setStart(node_map *nodes) +{ + node_map_iterator itr = nodes->find(startNode); + if (itr == nodes->end()) { + cerr << "Couldn't find node: " << startNode << endl; + } + else { + start = itr->second->getAddress(); + itr->second->addAirway(this); + } +} + +void FGAirway::setEnd(node_map *nodes) +{ + node_map_iterator itr = nodes->find(endNode); + if (itr == nodes->end()) { + cerr << "Couldn't find node: " << endNode << endl; + } + else { + end = itr->second->getAddress(); + } +} + +// There is probably a computationally cheaper way of +// doing this. +void FGAirway::setTrackDistance() +{ + double course; + SGWayPoint first (start->getLongitude(), + start->getLatitude(), + 0); + SGWayPoint second (end->getLongitude(), + end->getLatitude(), + 0); + first.CourseAndDistance(second, &course, &length); + +} + +/*************************************************************************** + * FGAirRoute() + **************************************************************************/ + + +bool FGAirRoute::next(int *val) +{ + //for (intVecIterator i = nodes.begin(); i != nodes.end(); i++) + // cerr << "FGTaxiRoute contains : " << *(i) << endl; + //cerr << "Offset from end: " << nodes.end() - currNode << endl; + //if (currNode != nodes.end()) + // cerr << "true" << endl; + //else + // cerr << "false" << endl; + + if (currNode == nodes.end()) + return false; + *val = *(currNode); + currNode++; + return true; +}; + +void FGAirRoute::add(const FGAirRoute &other) { + for (constIntVecIterator i = other.nodes.begin() ; + i != other.nodes.end(); i++) + { + nodes.push_back((*i)); + } + distance += other.distance; +} +/*************************************************************************** + * FGAirwayNetwork() + **************************************************************************/ + +FGAirwayNetwork::FGAirwayNetwork() +{ + hasNetwork = false; + foundRoute = false; + totalDistance = 0; + maxDistance = 0; +} + +void FGAirwayNetwork::addAirway(const FGAirway &seg) +{ + segments.push_back(seg); +} + +//void FGAirwayNetwork::addNode(const FGNode &node) +//{ +// nodes.push_back(node); +//} + +/* + void FGAirwayNetwork::addNodes(FGParkingVec *parkings) + { + FGTaxiNode n; + FGParkingVecIterator i = parkings->begin(); + while (i != parkings->end()) + { + n.setIndex(i->getIndex()); + n.setLatitude(i->getLatitude()); + n.setLongitude(i->getLongitude()); + nodes.push_back(n); + + i++; + } + } +*/ + + +void FGAirwayNetwork::init() +{ + hasNetwork = true; + FGAirwayVectorIterator i = segments.begin(); + while(i != segments.end()) { + //cerr << "initializing Airway " << i->getIndex() << endl; + i->setStart(&nodesMap); + i->setEnd (&nodesMap); + //i->setTrackDistance(); + //cerr << "Track distance = " << i->getLength() << endl; + //cerr << "Track ends at" << i->getEnd()->getIndex() << endl; + i++; + } + //exit(1); +} + + +void FGAirwayNetwork::load(SGPath path) +{ + string identStart, identEnd, token, name; + double latStart, lonStart, latEnd, lonEnd; + int type, base, top; + int airwayIndex = 0; + FGNode *n; + + sg_gzifstream in( path.str() ); + if ( !in.is_open() ) { + SG_LOG( SG_GENERAL, SG_ALERT, "Cannot open file: " << path.str() ); + exit(-1); + } + // toss the first two lines of the file + in >> skipeol; + in >> skipeol; + + // read in each remaining line of the file + +#ifdef __MWERKS__ + char c = 0; + while ( in.get(c) && c != '\0' ) { + in.putback(c); +#else + while ( ! in.eof() ) { +#endif + string token; + in >> token; + + if ( token == "99" ) { + return; //in >> skipeol; + } + // Read each line from the database + identStart = token; + in >> latStart >> lonStart >> identEnd >> latEnd >> lonEnd >> type >> base >> top >> name; + /*out << identStart << " " + << latStart << " " + << lonStart << " " + << identEnd << " " + << latEnd << " " + << lonEnd << " " + << type << " " + << base << " " + << top << " " + << name << " " + << endl;*/ + //first determine whether the start and end reference database already exist + //if not we should create them + int startIndex = 0, endIndex=0; + // FGNodeVectorIterator i = nodes.begin(); + // while (i != nodes.end() && (!(i->matches(identStart,latStart, lonStart)))) +// { +// i++; +// startIndex++; +// } +// if (i == nodes.end()) +// { +// nodes.push_back(FGNode(latStart, lonStart, startIndex, identStart)); +// //cout << "Adding node: " << identStart << endl; +// } + +// i = nodes.begin(); +// while (i != nodes.end() && (!(i->matches(identEnd,latEnd, lonEnd)))) { +// i++; +// endIndex++; +// } +// if (i == nodes.end()) { +// nodes.push_back(FGNode(latEnd, lonEnd, endIndex, identEnd)); +// //cout << "Adding node: " << identEnd << endl; +// } + // generate unique IDs for the nodes, consisting of a combination + // of the Navaid identifier + the integer value of the lat/lon position. + // identifier alone will not suffice, because they might not be globally unique. + char buffer[32]; + string startNode, endNode; + // Start + snprintf(buffer, 32, "%s%d%d", identStart.c_str(), (int) latStart, (int) lonStart); + startNode = buffer; + + node_map_iterator itr = nodesMap.find(string(buffer)); + if (itr == nodesMap.end()) { + startIndex = nodes.size(); + n = new FGNode(latStart, lonStart, startIndex, identStart); + nodesMap[string(buffer)] = n; + nodes.push_back(n); + //cout << "Adding node: " << identStart << endl; + } + else { + startIndex = itr->second->getIndex(); + } + // Start + snprintf(buffer, 32, "%s%d%d", identEnd.c_str(), (int) latEnd, (int) lonEnd); + endNode = buffer; + + itr = nodesMap.find(string(buffer)); + if (itr == nodesMap.end()) { + endIndex = nodes.size(); + n = new FGNode(latEnd, lonEnd, endIndex, identEnd); + nodesMap[string(buffer)] = n; + nodes.push_back(n); + //cout << "Adding node: " << identEnd << endl; + } + else { + endIndex = itr->second->getIndex(); + } + + + FGAirway airway; + airway.setIndex ( airwayIndex++ ); + airway.setStartNodeRef ( startNode ); + airway.setEndNodeRef ( endNode ); + airway.setType ( type ); + airway.setBase ( base ); + airway.setTop ( top ); + airway.setName ( name ); + segments.push_back(airway); + //cout << " Adding Airway: " << name << " " << startIndex << " " << endIndex << endl; + } + } + + int FGAirwayNetwork::findNearestNode(double lat, double lon) +{ + double minDist = HUGE_VAL; + double distsqrt, lat2, lon2; + int index; + SGWayPoint first (lon, + lat, + 0); + //cerr << "Lat " << lat << " lon " << lon << endl; + for (FGNodeVectorIterator + itr = nodes.begin(); + itr != nodes.end(); itr++) + { + //double course; + //if ((fabs(lat - ((*itr)->getLatitude())) < 0.001) && + // (fabs(lon - ((*itr)->getLongitude()) < 0.001))) + //cerr << "Warning: nodes are near" << endl; + //SGWayPoint second ((*itr)->getLongitude(), + // (*itr)->getLatitude(), + // 0); + //first.CourseAndDistance(second, &course, &dist); + lat2 = (*itr)->getLatitude(); + lon2 = (*itr)->getLongitude(); + // Note: This equation should adjust for decreasing distance per longitude + // with increasing lat. + distsqrt = + (lat-lat2)*(lat-lat2) + + (lon-lon2)*(lon-lon2); + + if (distsqrt < minDist) + { + minDist = distsqrt; + //cerr << "Test" << endl; + index = (*itr)->getIndex(); + //cerr << "Minimum distance of " << minDist << " for index " << index << endl; + //cerr << (*itr)->getLatitude() << " " << (*itr)->getLongitude() << endl; + } + //cerr << (*itr)->getIndex() << endl; + } + //cerr << " returning " << index << endl; + return index; +} + +FGNode *FGAirwayNetwork::findNode(int idx) +{ + for (FGNodeVectorIterator + itr = nodes.begin(); + itr != nodes.end(); itr++) + { + if ((*itr)->getIndex() == idx) + return (*itr)->getAddress(); + } + return 0; +} + +FGAirRoute FGAirwayNetwork::findShortestRoute(int start, int end) +{ + foundRoute = false; + totalDistance = 0; + FGNode *firstNode = findNode(start); + FGNode *lastNode = findNode(end); + //prevNode = prevPrevNode = -1; + //prevNode = start; + routes.clear(); + traceStack.clear(); + trace(firstNode, end, 0, 0); + FGAirRoute empty; + + if (!foundRoute) + { + SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end ); + cerr << "Failed to find route from waypoint " << start << " to " << end << endl; + //exit(1); + } + sort(routes.begin(), routes.end()); + //for (intVecIterator i = route.begin(); i != route.end(); i++) + // { + // rte->push_back(*i); + // } + + if (routes.begin() != routes.end()) + return *(routes.begin()); + else + return empty; +} + + +void FGAirwayNetwork::trace(FGNode *currNode, int end, int depth, double distance) +{ + traceStack.push_back(currNode->getIndex()); + totalDistance += distance; + //cerr << depth << " "; + //cerr << "Starting trace " << depth << " total distance: " << totalDistance<< endl; + //<< currNode->getIndex() << endl; + + // If the current route matches the required end point we found a valid route + // So we can add this to the routing table + if (currNode->getIndex() == end) + { + cerr << "Found route : " << totalDistance << "" << " " << *(traceStack.end()-1) << endl; + routes.push_back(FGAirRoute(traceStack,totalDistance)); + traceStack.pop_back(); + if (!(foundRoute)) + maxDistance = totalDistance; + else + if (totalDistance < maxDistance) + maxDistance = totalDistance; + foundRoute = true; + totalDistance -= distance; + return; + } + + + // search if the currentNode has been encountered before + // if so, we should step back one level, because it is + // rather rediculous to proceed further from here. + // if the current node has not been encountered before, + // i should point to traceStack.end()-1; and we can continue + // if i is not traceStack.end, the previous node was found, + // and we should return. + // This only works at trace levels of 1 or higher though + if (depth > 0) { + intVecIterator i = traceStack.begin(); + while ((*i) != currNode->getIndex()) { + //cerr << "Route so far : " << (*i) << endl; + i++; + } + if (i != traceStack.end()-1) { + traceStack.pop_back(); + totalDistance -= distance; + return; + } + // If the total distance from start to the current waypoint + // is longer than that of a route we can also stop this trace + // and go back one level. + if ((totalDistance > maxDistance) && foundRoute) + { + cerr << "Stopping rediculously long trace: " << totalDistance << endl; + traceStack.pop_back(); + totalDistance -= distance; + return; + } + } + + //cerr << "2" << endl; + if (currNode->getBeginRoute() != currNode->getEndRoute()) + { + //cerr << "l3l" << endl; + for (FGAirwayPointerVectorIterator + i = currNode->getBeginRoute(); + i != currNode->getEndRoute(); + i++) + { + //cerr << (*i)->getLenght() << endl; + trace((*i)->getEnd(), end, depth+1, (*i)->getLength()); + // { + // // cerr << currNode -> getIndex() << " "; + // route.push_back(currNode->getIndex()); + // return true; + // } + } + } + else + { + //SG_LOG( SG_GENERAL, SG_DEBUG, "4" ); + //cerr << "4" << endl; + } + traceStack.pop_back(); + totalDistance -= distance; + return; +} + diff --git a/src/Navaids/awynet.hxx b/src/Navaids/awynet.hxx new file mode 100755 index 000000000..a2f4dd698 --- /dev/null +++ b/src/Navaids/awynet.hxx @@ -0,0 +1,210 @@ +// airwaynet.hxx - A number of classes to handle taxiway +// assignments by the AI code +// +// Written by Durk Talsma. Based upon the ground netword code, started June 2005. +// +// Copyright (C) 2004 Durk Talsma. +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// +// $Id$ + +#ifndef _AIRWAYNETWORK_HXX_ +#define _AIRWAYNETWORK_HXX_ + +#include STL_STRING +#include +#include +#include +#include + +SG_USING_STD(string); +SG_USING_STD(map); +SG_USING_STD(set); +SG_USING_STD(vector); +SG_USING_STD(fstream); + +#include +#include + + +//#include "parking.hxx" + +class FGAirway; // forward reference + +typedef vector FGAirwayVector; +typedef vector FGAirwayPointerVector; +typedef vector::iterator FGAirwayVectorIterator; +typedef vector::iterator FGAirwayPointerVectorIterator; + +/************************************************************************************** + * class FGNode + *************************************************************************************/ +class FGNode +{ +private: + string ident; + double lat; + double lon; + int index; + FGAirwayPointerVector next; // a vector to all the segments leaving from this node + +public: + FGNode(); + FGNode(double lt, double ln, int idx, string id) { lat = lt; lon = ln; index = idx; ident = id;}; + + void setIndex(int idx) { index = idx;}; + void setLatitude (double val) { lat = val;}; + void setLongitude(double val) { lon = val;}; + //void setLatitude (const string& val) { lat = processPosition(val); }; + //void setLongitude(const string& val) { lon = processPosition(val); }; + void addAirway(FGAirway *segment) { next.push_back(segment); }; + + double getLatitude() { return lat;}; + double getLongitude(){ return lon;}; + + int getIndex() { return index; }; + string getIdent() { return ident; }; + FGNode *getAddress() { return this;}; + FGAirwayPointerVectorIterator getBeginRoute() { return next.begin(); }; + FGAirwayPointerVectorIterator getEndRoute() { return next.end(); }; + + bool matches(string ident, double lat, double lon); +}; + +typedef vector FGNodeVector; +typedef vector::iterator FGNodeVectorIterator; + + +typedef map < string, FGNode *> node_map; +typedef node_map::iterator node_map_iterator; +typedef node_map::const_iterator const_node_map_iterator; + + +/*************************************************************************************** + * class FGAirway + **************************************************************************************/ +class FGAirway +{ +private: + string startNode; + string endNode; + double length; + FGNode *start; + FGNode *end; + int index; + int type; // 1=low altitude; 2=high altitude airway + int base; // base altitude + int top; // top altitude + string name; + +public: + FGAirway(); + FGAirway(FGNode *, FGNode *, int); + + void setIndex (int val) { index = val; }; + void setStartNodeRef (string val) { startNode = val; }; + void setEndNodeRef (string val) { endNode = val; }; + + void setStart(node_map *nodes); + void setEnd (node_map *nodes); + void setType (int tp) { type = tp;}; + void setBase (int val) { base = val;}; + void setTop (int val) { top = val;}; + void setName (string val) { name = val;}; + + void setTrackDistance(); + + FGNode * getEnd() { return end;}; + double getLength() { if (length == 0) setTrackDistance(); return length; }; + int getIndex() { return index; }; + string getName() { return name; }; + + +}; + + +typedef vector intVec; +typedef vector::iterator intVecIterator; +typedef vector::const_iterator constIntVecIterator; + +class FGAirRoute +{ +private: + intVec nodes; + double distance; + intVecIterator currNode; + +public: + FGAirRoute() { distance = 0; currNode = nodes.begin(); }; + FGAirRoute(intVec nds, double dist) { nodes = nds; distance = dist; currNode = nodes.begin();}; + bool operator< (const FGAirRoute &other) const {return distance < other.distance; }; + bool empty () { return nodes.begin() == nodes.end(); }; + bool next(int *val); + + void first() { currNode = nodes.begin(); }; + void add(const FGAirRoute &other); + void add(int node) {nodes.push_back(node);}; + + friend istream& operator >> (istream& in, FGAirRoute& r); +}; + +inline istream& operator >> ( istream& in, FGAirRoute& r ) +{ + int node; + in >> node; + r.nodes.push_back(node); + //getline( in, n.name ); + return in; +} + +typedef vector AirRouteVector; +typedef vector::iterator AirRouteVectorIterator; + +/************************************************************************************** + * class FGAirwayNetwork + *************************************************************************************/ +class FGAirwayNetwork +{ +private: + bool hasNetwork; + node_map nodesMap; + FGNodeVector nodes; + FGAirwayVector segments; + //intVec route; + intVec traceStack; + AirRouteVector routes; + + bool foundRoute; + double totalDistance, maxDistance; + +public: + FGAirwayNetwork(); + + //void addNode (const FGNode& node); + //void addNodes (FGParkingVec *parkings); + void addAirway(const FGAirway& seg); + + void init(); + bool exists() { return hasNetwork; }; + int findNearestNode(double lat, double lon); + FGNode *findNode(int idx); + FGAirRoute findShortestRoute(int start, int end); + void trace(FGNode *, int, int, double dist); + + void load(SGPath path); +}; + +#endif