src/ATC/ATCProjection.cxx

   1 #include "ATCProjection.hxx"
   2 #include <math.h>
   3 #include <simgear/constants.h>
   4
   5 #define DCL_PI  3.1415926535f
   6 //#define SG_PI  ((SGfloat) M_PI)
   7 #define DCL_DEGREES_TO_RADIANS  (DCL_PI/180.0)
   8 #define DCL_RADIANS_TO_DEGREES  (180.0/DCL_PI)
   9
  10 FGATCProjection::FGATCProjection() {
  11     origin.setlat(0.0);
  12     origin.setlon(0.0);
  13     origin.setelev(0.0);
  14     correction_factor = cos(origin.lat() * DCL_DEGREES_TO_RADIANS);
  15 }
  16
  17 FGATCProjection::~FGATCProjection() {
  18 }
  19
  20 void FGATCProjection::Init(Point3D centre) {
  21     origin = centre;
  22     correction_factor = cos(origin.lat() * DCL_DEGREES_TO_RADIANS);
  23 }
  24
  25 Point3D FGATCProjection::ConvertToLocal(Point3D pt) {
  26     double delta_lat = pt.lat() - origin.lat();
  27     double delta_lon = pt.lon() - origin.lon();
  28
  29     double y = sin(delta_lat * DCL_DEGREES_TO_RADIANS) * SG_EQUATORIAL_RADIUS_M;
  30     double x = sin(delta_lon * DCL_DEGREES_TO_RADIANS) * SG_EQUATORIAL_RADIUS_M * correction_factor;
  31
  32     return(Point3D(x,y,0.0));
  33 }
  34
  35 Point3D FGATCProjection::ConvertFromLocal(Point3D pt) {
  36     return(Point3D(0,0,0));
  37 }
  38
  39 /**********************************************************************************/
  40
  41 FGATCAlignedProjection::FGATCAlignedProjection() {
  42     origin.setlat(0.0);
  43     origin.setlon(0.0);
  44     origin.setelev(0.0);
  45     correction_factor = cos(origin.lat() * DCL_DEGREES_TO_RADIANS);
  46 }
  47
  48 FGATCAlignedProjection::~FGATCAlignedProjection() {
  49 }
  50
  51 void FGATCAlignedProjection::Init(Point3D centre, double heading) {
  52     origin = centre;
  53     theta = heading * DCL_DEGREES_TO_RADIANS;
  54     correction_factor = cos(origin.lat() * DCL_DEGREES_TO_RADIANS);
  55 }
  56
  57 Point3D FGATCAlignedProjection::ConvertToLocal(Point3D pt) {
  58     // convert from lat/lon to orthogonal
  59     double delta_lat = pt.lat() - origin.lat();
  60     double delta_lon = pt.lon() - origin.lon();
  61     double y = sin(delta_lat * DCL_DEGREES_TO_RADIANS) * SG_EQUATORIAL_RADIUS_M;
  62     double x = sin(delta_lon * DCL_DEGREES_TO_RADIANS) * SG_EQUATORIAL_RADIUS_M * correction_factor;
  63     //cout << "Before alignment, x = " << x << " y = " << y << '\n';
  64
  65     // Align
  66     double xbar = x;
  67     x = x*cos(theta) - y*sin(theta);
  68     y = (xbar*sin(theta)) + (y*cos(theta));
  69     //cout << "After alignment, x = " << x << " y = " << y << '\n';
  70
  71     return(Point3D(x,y,0.0));
  72 }
  73
  74 Point3D FGATCAlignedProjection::ConvertFromLocal(Point3D pt) {
  75     return(Point3D(0,0,0));
  76 }