bool FGAIAircraft::aiTrafficVisible() {
- double userLatitude = fgGetDouble("/position/latitude-deg");
- double userLongitude = fgGetDouble("/position/longitude-deg");
- double course, distance;
-
- SGWayPoint current(pos.getLongitudeDeg(), pos.getLatitudeDeg(), 0);
- SGWayPoint user (userLongitude, userLatitude, 0);
-
- user.CourseAndDistance(current, &course, &distance);
-
- return ((distance * SG_METER_TO_NM) <= TRAFFICTOAIDISTTODIE);
+ SGGeod userPos(SGGeod::fromDeg(fgGetDouble("/position/longitude-deg"),
+ fgGetDouble("/position/latitude-deg")));
+
+ return (SGGeodesy::distanceNm(userPos, pos) <= TRAFFICTOAIDISTTODIE);
}
char buffer[10];
snprintf (buffer, 10, "%d", node);
FGTaxiNode *tn = apt->getDynamics()->getGroundNetwork()->findNode(node);
- waypoint* wpt = createOnGround(ac, buffer, tn->geod(), apt->getElevation(), ac->getPerformance()->vTaxi());
+ waypoint* wpt = createOnGround(ac, buffer, tn->getGeod(), apt->getElevation(), ac->getPerformance()->vTaxi());
wpt->routeIndex = route;
waypoints.push_back(wpt);
}
char buffer[10];
snprintf (buffer, 10, "%d", node);
FGTaxiNode *tn = gn->findNode(node);
- waypoint* wpt = createOnGround(ac, buffer, tn->geod(), apt->getElevation(), ac->getPerformance()->vTaxi());
+ waypoint* wpt = createOnGround(ac, buffer, tn->getGeod(), apt->getElevation(), ac->getPerformance()->vTaxi());
wpt->routeIndex = route;
waypoints.push_back(wpt);
}
intVec nodes;
int tmpNode, prevNode;
-
- SGWayPoint first (deplon,
- deplat,
- 100);
- SGWayPoint sec (arrlon,
- arrlat,
- 100);
- double course, distance;
- first.CourseAndDistance(sec, &course, &distance);
- distance *= SG_METER_TO_NM;
-
- SGVec3d a = SGVec3d::fromGeoc(SGGeoc::fromDegM(deplon, deplat, 1));
- SGVec3d b = SGVec3d::fromGeoc(SGGeoc::fromDegM(arrlon, arrlat, 1));
+ SGGeoc dep(SGGeoc::fromDegM(deplon, deplat, 100.0));
+ SGGeoc arr(SGGeoc::fromDegM(arrlon, arrlat, 100.0));
+
+ SGVec3d a = SGVec3d::fromGeoc(dep);
+ SGVec3d b = SGVec3d::fromGeoc(arr);
SGVec3d _cross = cross(b, a);
double angle = sgACos(dot(a, b));
tmpNode = 0;
for (double ang = 0.0; ang < angle; ang += 0.05)
- {
+ {
sgdVec3 newPos;
sgdMat4 matrix;
//cerr << "Angle = " << ang << endl;
sgdMakeRotMat4(matrix, ang, _cross.sg());
for(int j = 0; j < 3; j++)
- {
- newPos[j] =0.0;
- for (int k = 0; k<3; k++)
- {
- newPos[j] += matrix[j][k]*a[k];
- }
- }
+ {
+ newPos[j] =0.0;
+ for (int k = 0; k<3; k++)
+ {
+ newPos[j] += matrix[j][k]*a[k];
+ }
+ }
+
//cerr << "1"<< endl;
SGGeoc geoc = SGGeoc::fromCart(SGVec3d(newPos[0], newPos[1], newPos[2]));
prevNode = tmpNode;
tmpNode = globals->get_airwaynet()->findNearestNode(midlat, midlon);
- double nodelat = globals->get_airwaynet()->findNode(tmpNode)->getLatitude ();
- double nodelon = globals->get_airwaynet()->findNode(tmpNode)->getLongitude ();
- SGWayPoint curr(midlat,
- midlon,
- 100);
- SGWayPoint node(nodelat,
- nodelon,
- 100);
- curr.CourseAndDistance(node, &course, &distance);
- if ((distance < 25000) && (tmpNode != prevNode))
- nodes.push_back(tmpNode);
+ FGNode* node = globals->get_airwaynet()->findNode(tmpNode);
+ SGGeoc nodePos(SGGeoc::fromGeod(node->getPosition()));
+
+ if ((tmpNode != prevNode) && (SGGeodesy::distanceM(geoc, nodePos) < 25000)) {
+ nodes.push_back(tmpNode);
+ }
}
intVecIterator i = nodes.begin();
*************************************************************************/
+void FGTaxiNode::setLatitude (double val)
+{
+ geod.setLatitudeDeg(val);
+}
+
+void FGTaxiNode::setLongitude(double val)
+{
+ geod.setLongitudeDeg(val);
+}
+
+void FGTaxiNode::setLatitude (const string& val)
+{
+ geod.setLatitudeDeg(processPosition(val));
+}
+
+void FGTaxiNode::setLongitude(const string& val)
+{
+ geod.setLongitudeDeg(processPosition(val));
+}
+
void FGTaxiNode::sortEndSegments(bool byLength)
{
if (byLength)
#include <simgear/compiler.h>
#include <simgear/math/sg_geodesy.hxx>
-using std::string;
-using std::vector;
-
class FGTaxiSegment;
-typedef vector<FGTaxiSegment*> FGTaxiSegmentVector;
+typedef std::vector<FGTaxiSegment*> FGTaxiSegmentVector;
typedef FGTaxiSegmentVector::iterator FGTaxiSegmentVectorIterator;
bool sortByHeadingDiff(FGTaxiSegment *a, FGTaxiSegment *b);
bool sortByLength (FGTaxiSegment *a, FGTaxiSegment *b);
-double processPosition(const string& pos);
class FGTaxiNode
{
private:
- double lat;
- double lon;
+ SGGeod geod;
int index;
bool isOnRunway;
public:
FGTaxiNode() :
- lat (0.0),
- lon (0.0),
index(0),
isOnRunway(false),
holdType(0),
};
FGTaxiNode(const FGTaxiNode &other) :
- lat(other.lat),
- lon(other.lon),
+ geod(other.geod),
index(other.index),
isOnRunway(other.isOnRunway),
holdType(other.holdType),
FGTaxiNode &operator =(const FGTaxiNode &other)
{
- lat = other.lat;
- lon = other.lon;
+ geod = other.geod;
index = other.index;
isOnRunway = other.isOnRunway;
holdType = other.holdType;
};
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 setLatitude (double val);
+ void setLongitude(double val);
+ void setLatitude (const std::string& val);
+ void setLongitude(const std::string& val);
void addSegment(FGTaxiSegment *segment) { next.push_back(segment); };
void setHoldPointType(int val) { holdType = val; };
void setOnRunway(bool val) { isOnRunway = val; };
FGTaxiSegment *getPreviousSegment() { return previousSeg; };
double getPathScore() { return pathScore; };
- double getLatitude() { return lat;};
- double getLongitude(){ return lon;};
+ double getLatitude() { return geod.getLatitudeDeg();};
+ double getLongitude(){ return geod.getLongitudeDeg();};
- SGGeod geod() const { return SGGeod::fromDeg(lon, lat); }
+ const SGGeod& getGeod() const { return geod; }
int getIndex() { return index; };
int getHoldPointType() { return holdType; };
};
-typedef vector<FGTaxiNode*> FGTaxiNodeVector;
+typedef std::vector<FGTaxiNode*> FGTaxiNodeVector;
typedef FGTaxiNodeVector::iterator FGTaxiNodeVectorIterator;
#endif
// doing this.
void FGTaxiSegment::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->getGeod(), end->getGeod());
}
}
int FGGroundNetwork::findNearestNode(const SGGeod& aGeod)
-{
- return findNearestNode(aGeod.getLatitudeDeg(), aGeod.getLongitudeDeg());
-}
-
-int FGGroundNetwork::findNearestNode(double lat, double lon)
{
double minDist = HUGE_VAL;
- double dist;
- int index;
- SGWayPoint first (lon,
- lat,
- 0);
+ int index = -1;
- for (FGTaxiNodeVectorIterator
- itr = nodes.begin();
- itr != nodes.end(); itr++)
+ for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end(); itr++)
+ {
+ double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
+ if (d < minDist)
{
- double course;
- SGWayPoint second ((*itr)->getLongitude(),
- (*itr)->getLatitude(),
- 0);
- first.CourseAndDistance(second, &course, &dist);
- if (dist < minDist)
- {
- minDist = dist;
- index = (*itr)->getIndex();
- //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
- }
+ minDist = d;
+ index = (*itr)->getIndex();
+ //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
}
+ }
+
return index;
}
+int FGGroundNetwork::findNearestNode(double lat, double lon)
+{
+ return findNearestNode(SGGeod::fromDeg(lon, lat));
+}
+
FGTaxiNode *FGGroundNetwork::findNode(int idx)
{ /*
for (FGTaxiNodeVectorIterator
double mindist = HUGE_VAL;
if (activeTraffic.size())
{
- double course, dist, bearing, minbearing;
- SGWayPoint curr (lon,
- lat,
- alt);
+ double course, dist, bearing, minbearing, az2;
+ SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
//TrafficVector iterator closest;
closest = current;
for (TrafficVectorIterator i = activeTraffic.begin();
i != activeTraffic.end(); i++)
{
- if (i != current) {
- //SGWayPoint curr (lon,
- // lat,
- // alt);
- SGWayPoint other (i->getLongitude (),
- i->getLatitude (),
- i->getAltitude ());
- other.CourseAndDistance(curr, &course, &dist);
- bearing = fabs(heading-course);
+ if (i == current) {
+ continue;
+ }
+
+ SGGeod other(SGGeod::fromDegM(i->getLongitude(),
+ i->getLatitude(), i->getAltitude()));
+ SGGeodesy::inverse(other, curr, course, az2, dist);
+ bearing = fabs(heading-course);
if (bearing > 180)
bearing = 360-bearing;
if ((dist < mindist) && (bearing < 60.0))
closest = i;
minbearing = bearing;
}
- }
}
//Check traffic at the tower controller
if (towerController->hasActiveTraffic())
i != towerController->getActiveTraffic().end(); i++)
{
//cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
- //SGWayPoint curr (lon,
- // lat,
- // alt);
- SGWayPoint other (i->getLongitude (),
- i->getLatitude (),
- i->getAltitude ());
- other.CourseAndDistance(curr, &course, &dist);
- bearing = fabs(heading-course);
+ SGGeod other(SGGeod::fromDegM(i->getLongitude(),
+ i->getLatitude(), i->getAltitude()));
+ SGGeodesy::inverse(other, curr, course, az2, dist);
+ bearing = fabs(heading-course);
if (bearing > 180)
- bearing = 360-bearing;
+ bearing = 360-bearing;
if ((dist < mindist) && (bearing < 60.0))
{
mindist = dist;
// Finally, check UserPosition
double userLatitude = fgGetDouble("/position/latitude-deg");
double userLongitude = fgGetDouble("/position/longitude-deg");
- SGWayPoint user (userLongitude,
- userLatitude,
- alt); // Alt is not really important here.
- user.CourseAndDistance(curr, &course, &dist);
+ SGGeod user(SGGeod::fromDeg(userLatitude,userLongitude));
+ SGGeodesy::inverse(user, curr, course, az2, dist);
+
bearing = fabs(heading-course);
if (bearing > 180)
bearing = 360-bearing;
}
current = i;
current->setHoldPosition(false);
- SGWayPoint curr (lon,
- lat,
- alt);
- double course, dist, bearing, minbearing;
+ SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
+
for (i = activeTraffic.begin();
i != activeTraffic.end(); i++)
{
int node = current->crosses(this, *i);
if (node != -1)
{
+ FGTaxiNode* taxiNode = findNode(node);
+
// Determine whether it's save to continue or not.
// If we have a crossing route, there are two possibilities:
// 1) This is an interestion
// 2) This is oncoming two-way traffic, using the same taxiway.
//cerr << "Hold check 1 : " << id << " has common node " << node << endl;
- SGWayPoint nodePos(findNode(node)->getLongitude (),
- findNode(node)->getLatitude (),
- alt);
-
- SGWayPoint other (i->getLongitude (),
- i->getLatitude (),
- i->getAltitude ());
- //other.CourseAndDistance(curr, &course, &dist);
- bool needsToWait;
+
+ SGGeod other(SGGeod::fromDegM(i->getLongitude(), i->getLatitude(), i->getAltitude()));
+ bool needsToWait;
bool opposing;
if (current->isOpposing(this, *i, node))
{
else
{
opposing = false;
- other.CourseAndDistance(nodePos, &course, &dist);
- if (dist > 200) // 2.0*i->getRadius())
+ if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
{
needsToWait = false;
//cerr << "Hold check 3 : " << id <<" Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue "
//cerr << "Hold check 4: " << id << " Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
}
}
- curr.CourseAndDistance(nodePos, &course, &dist);
- if (!(i->hasHoldPosition()))
+
+ double dist = SGGeodesy::distanceM(curr, taxiNode->getGeod());
+ if (!(i->hasHoldPosition()))
{
if ((dist < 200) && //2.5*current->getRadius()) &&
{
}
+FGNode::FGNode(double lt, double ln, int idx, std::string id) :
+ ident(id),
+ geod(SGGeod::fromDeg(ln, lt)),
+ index(idx)
+{
+}
+
bool FGNode::matches(string id, double lt, double ln)
{
if ((ident == id) &&
- (fabs(lt - lat) < 1.0) &&
- (fabs(ln - lon) < 1.0))
+ (fabs(lt - geod.getLatitudeDeg()) < 1.0) &&
+ (fabs(ln - 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());
}
/***************************************************************************
{
private:
std::string ident;
- double lat;
- double lon;
+ SGGeod geod;
int index;
FGAirwayPointerVector next; // a vector to all the segments leaving from this node
public:
FGNode();
- FGNode(double lt, double ln, int idx, std::string id) { lat = lt; lon = ln; index = idx; ident = id;};
+ FGNode(double lt, double ln, int idx, std::string id);
void setIndex(int idx) { index = idx;};
- void setLatitude (double val) { lat = val;};
- void setLongitude(double val) { lon = val;};
- //void setLatitude (const std::string& val) { lat = processPosition(val); };
- //void setLongitude(const std::string& val) { lon = processPosition(val); };
void addAirway(FGAirway *segment) { next.push_back(segment); };
- double getLatitude() { return lat;};
- double getLongitude(){ return lon;};
+ double getLatitude() { return geod.getLatitudeDeg();};
+ double getLongitude(){ return geod.getLongitudeDeg();};
+
+ const SGGeod& getPosition() {return geod;}
int getIndex() { return index; };
std::string getIdent() { return ident; };
projects / flightgear.git / commitdiff