src/ATC/ATCProjection.cxx

   1 // ATCProjection.cxx - A convienience projection class for the ATC/AI system.
   2 //
   3 // Written by David Luff, started 2002.
   4 //
   5 // Copyright (C) 2002  David C Luff - david.luff@nottingham.ac.uk
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  20
  21 #include "ATCProjection.hxx"
  22 #include <math.h>
  23 #include <simgear/constants.h>
  24
  25 #include <iostream>
  26 SG_USING_STD(cout);
  27
  28 #define DCL_PI  3.1415926535f
  29 //#define SG_PI  ((SGfloat) M_PI)
  30 #define DCL_DEGREES_TO_RADIANS  (DCL_PI/180.0)
  31 #define DCL_RADIANS_TO_DEGREES  (180.0/DCL_PI)
  32
  33 FGATCProjection::FGATCProjection() {
  34     origin.setlat(0.0);
  35     origin.setlon(0.0);
  36     origin.setelev(0.0);
  37     correction_factor = cos(origin.lat() * DCL_DEGREES_TO_RADIANS);
  38 }
  39
  40 FGATCProjection::FGATCProjection(Point3D centre) {
  41     origin = centre;
  42     correction_factor = cos(origin.lat() * DCL_DEGREES_TO_RADIANS);
  43 }
  44
  45 FGATCProjection::~FGATCProjection() {
  46 }
  47
  48 void FGATCProjection::Init(Point3D centre) {
  49     origin = centre;
  50     correction_factor = cos(origin.lat() * DCL_DEGREES_TO_RADIANS);
  51 }
  52
  53 Point3D FGATCProjection::ConvertToLocal(Point3D pt) {
  54     double delta_lat = pt.lat() - origin.lat();
  55     double delta_lon = pt.lon() - origin.lon();
  56
  57     double y = sin(delta_lat * DCL_DEGREES_TO_RADIANS) * SG_EQUATORIAL_RADIUS_M;
  58     double x = sin(delta_lon * DCL_DEGREES_TO_RADIANS) * SG_EQUATORIAL_RADIUS_M * correction_factor;
  59
  60     return(Point3D(x,y,0.0));
  61 }
  62
  63 Point3D FGATCProjection::ConvertFromLocal(Point3D pt) {
  64         double delta_lat = asin(pt.y() / SG_EQUATORIAL_RADIUS_M) * DCL_RADIANS_TO_DEGREES;
  65         double delta_lon = (asin(pt.x() / SG_EQUATORIAL_RADIUS_M) * DCL_RADIANS_TO_DEGREES) / correction_factor;
  66
  67     return(Point3D(origin.lon()+delta_lon, origin.lat()+delta_lat, 0.0));
  68 }
  69
  70 /**********************************************************************************/
  71
  72 FGATCAlignedProjection::FGATCAlignedProjection() {
  73     origin.setlat(0.0);
  74     origin.setlon(0.0);
  75     origin.setelev(0.0);
  76     correction_factor = cos(origin.lat() * DCL_DEGREES_TO_RADIANS);
  77 }
  78
  79 FGATCAlignedProjection::~FGATCAlignedProjection() {
  80 }
  81
  82 void FGATCAlignedProjection::Init(Point3D centre, double heading) {
  83     origin = centre;
  84     theta = heading * DCL_DEGREES_TO_RADIANS;
  85     correction_factor = cos(origin.lat() * DCL_DEGREES_TO_RADIANS);
  86 }
  87
  88 Point3D FGATCAlignedProjection::ConvertToLocal(Point3D pt) {
  89     // convert from lat/lon to orthogonal
  90     double delta_lat = pt.lat() - origin.lat();
  91     double delta_lon = pt.lon() - origin.lon();
  92     double y = sin(delta_lat * DCL_DEGREES_TO_RADIANS) * SG_EQUATORIAL_RADIUS_M;
  93     double x = sin(delta_lon * DCL_DEGREES_TO_RADIANS) * SG_EQUATORIAL_RADIUS_M * correction_factor;
  94
  95     // Align
  96     double xbar = x;
  97     x = x*cos(theta) - y*sin(theta);
  98     y = (xbar*sin(theta)) + (y*cos(theta));
  99
 100     return(Point3D(x,y,pt.elev()));
 101 }
 102
 103 Point3D FGATCAlignedProjection::ConvertFromLocal(Point3D pt) {
 104         //cout << "theta = " << theta << '\n';
 105         //cout << "origin = " << origin << '\n';
 106     // de-align
 107     double thi = theta * -1.0;
 108     double x = pt.x()*cos(thi) - pt.y()*sin(thi);
 109     double y = (pt.x()*sin(thi)) + (pt.y()*cos(thi));
 110
 111     // convert from orthogonal to lat/lon
 112     double delta_lat = asin(y / SG_EQUATORIAL_RADIUS_M) * DCL_RADIANS_TO_DEGREES;
 113     double delta_lon = (asin(x / SG_EQUATORIAL_RADIUS_M) * DCL_RADIANS_TO_DEGREES) / correction_factor;
 114
 115     return(Point3D(origin.lon()+delta_lon, origin.lat()+delta_lat, pt.elev()));
 116 }