--- /dev/null
+// 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 <config.h>
+#endif
+
+#ifdef _MSC_VER
+# define _USE_MATH_DEFINES
+#endif
+#include <math.h>
+#include <algorithm>
+
+#include <simgear/compiler.h>
+
+//#include <plib/sg.h>
+//#include <plib/ul.h>
+
+//#include <Environment/environment_mgr.hxx>
+//#include <Environment/environment.hxx>
+//#include <simgear/misc/sg_path.hxx>
+//#include <simgear/props/props.hxx>
+//#include <simgear/structure/subsystem_mgr.hxx>
+#include <simgear/debug/logstream.hxx>
+#include <simgear/misc/sgstream.hxx>
+#include <simgear/route/waypoint.hxx>
+//#include <Main/globals.hxx>
+//#include <Main/fg_props.hxx>
+//#include <Airports/runways.hxx>
+
+//#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;
+}
+
--- /dev/null
+// 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 <fstream>
+#include <set>
+#include <map>
+#include <vector>
+
+SG_USING_STD(string);
+SG_USING_STD(map);
+SG_USING_STD(set);
+SG_USING_STD(vector);
+SG_USING_STD(fstream);
+
+#include <simgear/misc/sg_path.hxx>
+#include <simgear/misc/sgstream.hxx>
+
+
+//#include "parking.hxx"
+
+class FGAirway; // forward reference
+
+typedef vector<FGAirway> FGAirwayVector;
+typedef vector<FGAirway *> FGAirwayPointerVector;
+typedef vector<FGAirway>::iterator FGAirwayVectorIterator;
+typedef vector<FGAirway*>::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<FGNode *> FGNodeVector;
+typedef vector<FGNode *>::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<int> intVec;
+typedef vector<int>::iterator intVecIterator;
+typedef vector<int>::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<FGAirRoute> AirRouteVector;
+typedef vector<FGAirRoute>::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
projects / flightgear.git / commitdiff