src/ATCDCL/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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  20
  21 #ifdef HAVE_CONFIG_H
  22 #  include <config.h>
  23 #endif
  24
  25 #include "ATCProjection.hxx"
  26 #include <math.h>
  27 #include <simgear/constants.h>
  28
  29 FGATCAlignedProjection::FGATCAlignedProjection() {
  30     _origin.setLatitudeRad(0);
  31     _origin.setLongitudeRad(0);
  32     _origin.setElevationM(0);
  33     _correction_factor = cos(_origin.getLatitudeRad());
  34 }
  35
  36 FGATCAlignedProjection::FGATCAlignedProjection(const SGGeod& centre, double heading) {
  37     _origin = centre;
  38     _theta = heading * SG_DEGREES_TO_RADIANS;
  39     _correction_factor = cos(_origin.getLatitudeRad());
  40 }
  41
  42 FGATCAlignedProjection::~FGATCAlignedProjection() {
  43 }
  44
  45 void FGATCAlignedProjection::Init(const SGGeod& centre, double heading) {
  46     _origin = centre;
  47     _theta = heading * SG_DEGREES_TO_RADIANS;
  48     _correction_factor = cos(_origin.getLatitudeRad());
  49 }
  50
  51 SGVec3d FGATCAlignedProjection::ConvertToLocal(const SGGeod& pt) {
  52     // convert from lat/lon to orthogonal
  53     double delta_lat = pt.getLatitudeRad() - _origin.getLatitudeRad();
  54     double delta_lon = pt.getLongitudeRad() - _origin.getLongitudeRad();
  55     double y = sin(delta_lat) * SG_EQUATORIAL_RADIUS_M;
  56     double x = sin(delta_lon) * SG_EQUATORIAL_RADIUS_M * _correction_factor;
  57
  58     // Align
  59     if(_theta != 0.0) {
  60         double xbar = x;
  61         x = x*cos(_theta) - y*sin(_theta);
  62         y = (xbar*sin(_theta)) + (y*cos(_theta));
  63     }
  64
  65     return SGVec3d(x, y, pt.getElevationM());
  66 }
  67
  68 SGGeod FGATCAlignedProjection::ConvertFromLocal(const SGVec3d& pt) {
  69     // de-align
  70     double thi = _theta * -1.0;
  71     double x = pt.x()*cos(thi) - pt.y()*sin(thi);
  72     double y = (pt.x()*sin(thi)) + (pt.y()*cos(thi));
  73
  74     // convert from orthogonal to lat/lon
  75     double delta_lat = asin(y / SG_EQUATORIAL_RADIUS_M);
  76     double delta_lon = asin(x / SG_EQUATORIAL_RADIUS_M) / _correction_factor;
  77
  78     return SGGeod::fromRadM(_origin.getLongitudeRad()+delta_lon, _origin.getLatitudeRad()+delta_lat, pt.z());
  79 }