//
// 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.
+// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
//
// $Id$
#include <math.h>
#include <algorithm>
+#include <iostream>
#include <simgear/compiler.h>
#include <simgear/debug/logstream.hxx>
#include <simgear/misc/sgstream.hxx>
#include <simgear/route/waypoint.hxx>
+#include <simgear/misc/sg_path.hxx>
#include "awynet.hxx"
-SG_USING_STD(sort);
+using std::sort;
+using std::string;
+using std::cerr;
+using std::endl;
/**************************************************************************
* FGNode
{
}
-bool FGNode::matches(string id, double lt, double ln)
+FGNode::FGNode(const SGGeod& aPos, int idx, std::string id) :
+ ident(id),
+ geod(aPos),
+ index(idx)
+{
+ cart = SGVec3d::fromGeod(geod);
+}
+
+bool FGNode::matches(std::string id, const SGGeod& aPos)
{
if ((ident == id) &&
- (fabs(lt - lat) < 1.0) &&
- (fabs(ln - lon) < 1.0))
+ (fabs(aPos.getLatitudeDeg() - geod.getLatitudeDeg()) < 1.0) &&
+ (fabs(aPos.getLongitudeDeg() - geod.getLongitudeDeg()) < 1.0))
return true;
else
return false;
// 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);
-
+ length = SGGeodesy::distanceM(start->getPosition(), end->getPosition());
}
/***************************************************************************
//}
/*
- void FGAirwayNetwork::addNodes(FGParkingVec *parkings)
+ void FGAirwayNetwork::addNodes(FGParkingList *parkings)
{
FGTaxiNode n;
- FGParkingVecIterator i = parkings->begin();
+ FGParkingList::iterator i = parkings->begin();
while (i != parkings->end())
{
n.setIndex(i->getIndex());
}
-void FGAirwayNetwork::load(SGPath path)
+void FGAirwayNetwork::load(const SGPath& path)
{
- string identStart, identEnd, token, name;
+ std::string identStart, identEnd, token, name;
double latStart, lonStart, latEnd, lonEnd;
int type, base, top;
int airwayIndex = 0;
sg_gzifstream in( path.str() );
if ( !in.is_open() ) {
- SG_LOG( SG_GENERAL, SG_ALERT, "Cannot open file: " << path.str() );
+ SG_LOG( SG_NAVAID, SG_ALERT, "Cannot open file: " << path.str() );
exit(-1);
}
// toss the first two lines of the file
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;
// Read each line from the database
identStart = token;
in >> latStart >> lonStart >> identEnd >> latEnd >> lonEnd >> type >> base >> top >> name;
+ in >> skipeol;
/*out << identStart << " "
<< latStart << " "
<< lonStart << " "
node_map_iterator itr = nodesMap.find(string(buffer));
if (itr == nodesMap.end()) {
startIndex = nodes.size();
- n = new FGNode(latStart, lonStart, startIndex, identStart);
+ SGGeod startPos(SGGeod::fromDeg(lonStart, latStart));
+ n = new FGNode(startPos, startIndex, identStart);
nodesMap[string(buffer)] = n;
nodes.push_back(n);
//cout << "Adding node: " << identStart << endl;
itr = nodesMap.find(string(buffer));
if (itr == nodesMap.end()) {
endIndex = nodes.size();
- n = new FGNode(latEnd, lonEnd, endIndex, identEnd);
+ SGGeod endPos(SGGeod::fromDeg(lonEnd, latEnd));
+ n = new FGNode(endPos, endIndex, identEnd);
nodesMap[string(buffer)] = n;
nodes.push_back(n);
//cout << "Adding node: " << identEnd << endl;
}
}
- int FGAirwayNetwork::findNearestNode(double lat, double lon)
+int FGAirwayNetwork::findNearestNode(const SGGeod& aPos)
{
double minDist = HUGE_VAL;
- double distsqrt, lat2, lon2;
- int index;
- SGWayPoint first (lon,
- lat,
- 0);
+ int index = -1;
+ SGVec3d cart = SGVec3d::fromGeod(aPos);
+
//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;
- }
+ double d2 = distSqr(cart, (*itr)->getCart());
+ if (d2 < minDist)
+ {
+ minDist = d2;
+ index = (*itr)->getIndex();
+ }
//cerr << (*itr)->getIndex() << endl;
}
//cerr << " returning " << index << endl;
foundRoute = false;
totalDistance = 0;
FGNode *firstNode = findNode(start);
- FGNode *lastNode = findNode(end);
+ //FGNode *lastNode = findNode(end);
//prevNode = prevPrevNode = -1;
//prevNode = start;
routes.clear();
if (!foundRoute)
{
- SG_LOG( SG_GENERAL, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
+ SG_LOG( SG_NAVAID, SG_INFO, "Failed to find route from waypoint " << start << " to " << end );
cerr << "Failed to find route from waypoint " << start << " to " << end << endl;
//exit(1);
}
}
else
{
- //SG_LOG( SG_GENERAL, SG_DEBUG, "4" );
+ //SG_LOG( SG_NAVAID, SG_DEBUG, "4" );
//cerr << "4" << endl;
}
traceStack.pop_back();
projects / flightgear.git / blobdiff