Alex Romosan:

[flightgear.git] / src / ATC / ATCProjection.cxx

diff --git a/src/ATC/ATCProjection.cxx b/src/ATC/ATCProjection.cxx
index 4e3f51f89fe64ec5b26bed9dc46a825d0bdc6838..5c7ed78c6b24b51d8d40bea97fe8cdad05f63932 100644 (file)
--- a/src/ATC/ATCProjection.cxx
+++ b/src/ATC/ATCProjection.cxx
@@ -22,6 +22,9 @@
 #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)
@@ -34,15 +37,20 @@ FGATCProjection::FGATCProjection() {
     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();
 
@@ -52,7 +60,7 @@ Point3D FGATCProjection::ConvertToLocal(Point3D pt) {
     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;
        
@@ -71,13 +79,13 @@ FGATCAlignedProjection::FGATCAlignedProjection() {
 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();
@@ -89,10 +97,12 @@ Point3D FGATCAlignedProjection::ConvertToLocal(Point3D pt) {
     x = x*cos(theta) - y*sin(theta);
     y = (xbar*sin(theta)) + (y*cos(theta));
 
-    return(Point3D(x,y,0.0));
+    return(Point3D(x,y,pt.elev()));
 }
 
-Point3D FGATCAlignedProjection::ConvertFromLocal(Point3D pt) {
+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);
@@ -102,5 +112,5 @@ Point3D FGATCAlignedProjection::ConvertFromLocal(Point3D pt) {
     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));
+    return(Point3D(origin.lon()+delta_lon, origin.lat()+delta_lat, pt.elev()));
 }