Melchior FRANZ:

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

diff --git a/src/ATC/ATCProjection.cxx b/src/ATC/ATCProjection.cxx
index 94293502a1d96cf41946d57b314d5471fcb3ede9..257ea544483cc492a51381f5beccdcc6f59e8aa8 100644 (file)
--- a/src/ATC/ATCProjection.cxx
+++ b/src/ATC/ATCProjection.cxx
@@ -1,7 +1,30 @@
+// ATCProjection.cxx - A convienience projection class for the ATC/AI system.
+//
+// Written by David Luff, started 2002.
+//
+// Copyright (C) 2002  David C Luff - david.luff@nottingham.ac.uk
+//
+// 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.
+
 #include "ATCProjection.hxx"
 #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)
@@ -33,7 +56,10 @@ Point3D FGATCProjection::ConvertToLocal(Point3D pt) {
 }
 
 Point3D FGATCProjection::ConvertFromLocal(Point3D pt) {
-    return(Point3D(0,0,0));
+       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;
+       
+    return(Point3D(origin.lon()+delta_lon, origin.lat()+delta_lat, 0.0));
 }
 
 /**********************************************************************************/
@@ -60,17 +86,26 @@ Point3D FGATCAlignedProjection::ConvertToLocal(Point3D pt) {
     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) {
-    return(Point3D(0,0,0));
+       //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()));
 }