]> git.mxchange.org Git - flightgear.git/blob - src/ATCDCL/ATCProjection.cxx
A generic, configurable autobrake system. Not fully features yet, but works quite...
[flightgear.git] / 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 }