#include <math.h>
#include <simgear/constants.h>
+#include <iostream>
+SG_USING_STD(cout);
+
#define DCL_PI 3.1415926535f
//#define SG_PI ((SGfloat) M_PI)
#define DCL_DEGREES_TO_RADIANS (DCL_PI/180.0)
correction_factor = cos(origin.lat() * DCL_DEGREES_TO_RADIANS);
}
+FGATCProjection::FGATCProjection(const Point3D& centre) {
+ origin = centre;
+ correction_factor = cos(origin.lat() * DCL_DEGREES_TO_RADIANS);
+}
+
FGATCProjection::~FGATCProjection() {
}
-void FGATCProjection::Init(Point3D centre) {
+void FGATCProjection::Init(const Point3D& centre) {
origin = centre;
correction_factor = cos(origin.lat() * DCL_DEGREES_TO_RADIANS);
}
-Point3D FGATCProjection::ConvertToLocal(Point3D pt) {
+Point3D FGATCProjection::ConvertToLocal(const Point3D& pt) {
double delta_lat = pt.lat() - origin.lat();
double delta_lon = pt.lon() - origin.lon();
return(Point3D(x,y,0.0));
}
-Point3D FGATCProjection::ConvertFromLocal(Point3D pt) {
+Point3D FGATCProjection::ConvertFromLocal(const Point3D& pt) {
double delta_lat = asin(pt.y() / SG_EQUATORIAL_RADIUS_M) * DCL_RADIANS_TO_DEGREES;
double delta_lon = (asin(pt.x() / SG_EQUATORIAL_RADIUS_M) * DCL_RADIANS_TO_DEGREES) / correction_factor;
FGATCAlignedProjection::~FGATCAlignedProjection() {
}
-void FGATCAlignedProjection::Init(Point3D centre, double heading) {
+void FGATCAlignedProjection::Init(const Point3D& centre, double heading) {
origin = centre;
theta = heading * DCL_DEGREES_TO_RADIANS;
correction_factor = cos(origin.lat() * DCL_DEGREES_TO_RADIANS);
}
-Point3D FGATCAlignedProjection::ConvertToLocal(Point3D pt) {
+Point3D FGATCAlignedProjection::ConvertToLocal(const Point3D& pt) {
// convert from lat/lon to orthogonal
double delta_lat = pt.lat() - origin.lat();
double delta_lon = pt.lon() - origin.lon();
double y = sin(delta_lat * DCL_DEGREES_TO_RADIANS) * SG_EQUATORIAL_RADIUS_M;
double x = sin(delta_lon * DCL_DEGREES_TO_RADIANS) * SG_EQUATORIAL_RADIUS_M * correction_factor;
- //cout << "Before alignment, x = " << x << " y = " << y << '\n';
// Align
double xbar = x;
x = x*cos(theta) - y*sin(theta);
y = (xbar*sin(theta)) + (y*cos(theta));
- //cout << "After alignment, x = " << x << " y = " << y << '\n';
- return(Point3D(x,y,0.0));
+ return(Point3D(x,y,pt.elev()));
}
-Point3D FGATCAlignedProjection::ConvertFromLocal(Point3D pt) {
- // de-align
- double x = (pt.x() + pt.y()*sin(theta)) / cos(theta);
- double y = (pt.y() - pt.x()*sin(theta)) / cos(theta);
-
- // convert from orthogonal to lat/lon
- double delta_lat = asin(y / SG_EQUATORIAL_RADIUS_M) * DCL_RADIANS_TO_DEGREES;
- double delta_lon = (asin(x / SG_EQUATORIAL_RADIUS_M) * DCL_RADIANS_TO_DEGREES) / correction_factor;
-
- return(Point3D(origin.lon()+delta_lon, origin.lat()+delta_lat, 0.0));
+Point3D FGATCAlignedProjection::ConvertFromLocal(const Point3D& pt) {
+ //cout << "theta = " << theta << '\n';
+ //cout << "origin = " << origin << '\n';
+ // de-align
+ double thi = theta * -1.0;
+ double x = pt.x()*cos(thi) - pt.y()*sin(thi);
+ double y = (pt.x()*sin(thi)) + (pt.y()*cos(thi));
+
+ // convert from orthogonal to lat/lon
+ double delta_lat = asin(y / SG_EQUATORIAL_RADIUS_M) * DCL_RADIANS_TO_DEGREES;
+ double delta_lon = (asin(x / SG_EQUATORIAL_RADIUS_M) * DCL_RADIANS_TO_DEGREES) / correction_factor;
+
+ return(Point3D(origin.lon()+delta_lon, origin.lat()+delta_lat, pt.elev()));
}