1 // ATCutils.cxx - Utility functions for the ATC / AI system
3 // Written by David Luff, started March 2002.
5 // Copyright (C) 2002 David C Luff - david.luff@nottingham.ac.uk
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.
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.
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.
22 #include <simgear/math/point3d.hxx>
23 #include <simgear/constants.h>
27 #include "ATCutils.hxx"
29 // Convert a 2 digit rwy number to a spoken-style string
30 string convertNumToSpokenString(int n) {
31 string nums[10] = {"zero", "one", "two", "three", "four", "five", "six", "seven", "eight", "nine"};
32 // Basic error/sanity checking
40 n = 36; // Is this right?
52 // Return the phonetic letter of a letter represented as an integer 1->26
53 string GetPhoneticIdent(int i) {
54 // TODO - Check i is between 1 and 26 and wrap if necessary
56 case 1 : return("Alpha");
57 case 2 : return("Bravo");
58 case 3 : return("Charlie");
59 case 4 : return("Delta");
60 case 5 : return("Echo");
61 case 6 : return("Foxtrot");
62 case 7 : return("Golf");
63 case 8 : return("Hotel");
64 case 9 : return("Indigo");
65 case 10 : return("Juliet");
66 case 11 : return("Kilo");
67 case 12 : return("Lima");
68 case 13 : return("Mike");
69 case 14 : return("November");
70 case 15 : return("Oscar");
71 case 16 : return("Papa");
72 case 17 : return("Quebec");
73 case 18 : return("Romeo");
74 case 19 : return("Sierra");
75 case 20 : return("Tango");
76 case 21 : return("Uniform");
77 case 22 : return("Victor");
78 case 23 : return("Whiskey");
79 case 24 : return("X-ray");
80 case 25 : return("Yankee");
81 case 26 : return("Zulu");
83 // We shouldn't get here
87 // Given two positions, get the HORIZONTAL separation (in meters)
88 double dclGetHorizontalSeparation(Point3D pos1, Point3D pos2) {
89 double x; //East-West separation
90 double y; //North-South separation
91 double z; //Horizontal separation - z = sqrt(x^2 + y^2)
93 double lat1 = pos1.lat() * SG_DEGREES_TO_RADIANS;
94 double lon1 = pos1.lon() * SG_DEGREES_TO_RADIANS;
95 double lat2 = pos2.lat() * SG_DEGREES_TO_RADIANS;
96 double lon2 = pos2.lon() * SG_DEGREES_TO_RADIANS;
98 y = sin(fabs(lat1 - lat2)) * SG_EQUATORIAL_RADIUS_M;
99 x = sin(fabs(lon1 - lon2)) * SG_EQUATORIAL_RADIUS_M * (cos((lat1 + lat2) / 2.0));
105 // Given a point and a line, get the HORIZONTAL shortest distance from the point to a point on the line.
106 // Expects to be fed orthogonal co-ordinates, NOT lat & lon !
107 double dclGetLinePointSeparation(double px, double py, double x1, double y1, double x2, double y2) {
110 double magline = sqrt(vecx*vecx + vecy*vecy);
111 double u = ((px-x1)*(x2-x1) + (py-y1)*(y2-y1)) / (magline * magline);
112 double x0 = x1 + u*(x2-x1);
113 double y0 = y1 + u*(y2-y1);
116 double d = sqrt(vecx*vecx + vecy*vecy);
123 // Given a position (lat/lon/elev), heading, vertical angle, and distance, calculate the new position.
124 // Assumes that the ground is not hit!!! Expects heading and angle in degrees, distance in meters.
125 Point3D dclUpdatePosition(Point3D pos, double heading, double angle, double distance) {
126 //cout << setprecision(10) << pos.lon() << ' ' << pos.lat() << '\n';
127 heading *= DCL_DEGREES_TO_RADIANS;
128 angle *= DCL_DEGREES_TO_RADIANS;
129 double lat = pos.lat() * DCL_DEGREES_TO_RADIANS;
130 double lon = pos.lon() * DCL_DEGREES_TO_RADIANS;
131 double elev = pos.elev();
132 //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
134 double horiz_dist = distance * cos(angle);
135 double vert_dist = distance * sin(angle);
137 double north_dist = horiz_dist * cos(heading);
138 double east_dist = horiz_dist * sin(heading);
140 //cout << distance << ' ' << horiz_dist << ' ' << vert_dist << ' ' << north_dist << ' ' << east_dist << '\n';
142 double delta_lat = asin(north_dist / (double)SG_EQUATORIAL_RADIUS_M);
143 double delta_lon = asin(east_dist / (double)SG_EQUATORIAL_RADIUS_M) * (1.0 / cos(lat)); // I suppose really we should use the average of the original and new lat but we'll assume that this will be good enough.
144 //cout << delta_lon*DCL_RADIANS_TO_DEGREES << ' ' << delta_lat*DCL_RADIANS_TO_DEGREES << '\n';
148 //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
150 //cout << setprecision(15) << DCL_DEGREES_TO_RADIANS * DCL_RADIANS_TO_DEGREES << '\n';
152 return(Point3D(lon*DCL_RADIANS_TO_DEGREES, lat*DCL_RADIANS_TO_DEGREES, elev));
157 /* Determine location in runway coordinates */
159 Radius_to_rwy = Sea_level_radius + Runway_altitude;
160 cos_rwy_hdg = cos(Runway_heading*DEG_TO_RAD);
161 sin_rwy_hdg = sin(Runway_heading*DEG_TO_RAD);
163 D_cg_north_of_rwy = Radius_to_rwy*(Latitude - Runway_latitude);
164 D_cg_east_of_rwy = Radius_to_rwy*cos(Runway_latitude)
165 *(Longitude - Runway_longitude);
166 D_cg_above_rwy = Radius_to_vehicle - Radius_to_rwy;
168 X_cg_rwy = D_cg_north_of_rwy*cos_rwy_hdg
169 + D_cg_east_of_rwy*sin_rwy_hdg;
170 Y_cg_rwy =-D_cg_north_of_rwy*sin_rwy_hdg
171 + D_cg_east_of_rwy*cos_rwy_hdg;
172 H_cg_rwy = D_cg_above_rwy;