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.
28 #include <simgear/math/point3d.hxx>
29 #include <simgear/constants.h>
30 #include <simgear/misc/sg_path.hxx>
31 #include <simgear/debug/logstream.hxx>
33 //#include <iomanip.h>
35 #include <Airports/runways.hxx>
36 #include <Main/globals.hxx>
38 #include "ATCutils.hxx"
39 #include "ATCProjection.hxx"
41 static const string nums[10] = {"zero", "one", "two", "three", "four", "five", "six", "seven", "eight", "niner"};
43 // Convert any number to spoken digits
44 string ConvertNumToSpokenDigits(const string &n) {
45 //cout << "n = " << n << endl;
46 static const string pt = "decimal";
49 for(unsigned int i=0; i<n.length(); ++i) {
50 //cout << "n.substr(" << i << ",1 = " << n.substr(i,1) << endl;
51 if(n.substr(i,1) == " ") {
53 } else if(n.substr(i,1) == ".") {
56 str += nums[atoi((n.substr(i,1)).c_str())];
58 if(i != (n.length()-1)) { // ie. don't add a space at the end.
66 // Convert an integer to spoken digits
67 string ConvertNumToSpokenDigits(int n) {
68 std::ostringstream buf;
70 return(ConvertNumToSpokenDigits(buf.str()));
74 // Convert a 2 digit rwy number to a spoken-style string
75 string ConvertRwyNumToSpokenString(int n) {
76 // Basic error/sanity checking
84 n = 36; // Is this right?
92 str += " "; //Changed this for the benefit of the voice token parser - prefer the "-" in the visual output though.
97 // Assumes we get a two-digit string optionally appended with L, R or C
99 // Anything else is not guaranteed to be handled correctly!
100 string ConvertRwyNumToSpokenString(const string &s) {
102 return(ConvertRwyNumToSpokenString(atoi(s.c_str())));
104 string r = ConvertRwyNumToSpokenString(atoi(s.substr(0,2).c_str()));
105 if(s.substr(2,1) == "L") {
107 } else if(s.substr(2,1) == "R") {
109 } else if(s.substr(2,1) == "C") {
112 SG_LOG(SG_ATC, SG_WARN, "WARNING: Unknown suffix " << s.substr(2,1) << " from runway ID " << s << " in ConvertRwyNumToSpokenString(...)");
119 // Return the phonetic letter of a letter represented as an integer 1->26
120 string GetPhoneticIdent(int i) {
121 // TODO - Check i is between 1 and 26 and wrap if necessary
122 return(GetPhoneticIdent(char('a' + (i-1))));
125 // Return the phonetic letter of a character in the range a-z or A-Z.
126 // Currently always returns prefixed by lowercase.
127 string GetPhoneticIdent(char c) {
129 // TODO - Check c is between a and z and wrap if necessary
131 case 'a' : return("alpha");
132 case 'b' : return("bravo");
133 case 'c' : return("charlie");
134 case 'd' : return("delta");
135 case 'e' : return("echo");
136 case 'f' : return("foxtrot");
137 case 'g' : return("golf");
138 case 'h' : return("hotel");
139 case 'i' : return("india");
140 case 'j' : return("juliet");
141 case 'k' : return("kilo");
142 case 'l' : return("lima");
143 case 'm' : return("mike");
144 case 'n' : return("november");
145 case 'o' : return("oscar");
146 case 'p' : return("papa");
147 case 'q' : return("quebec");
148 case 'r' : return("romeo");
149 case 's' : return("sierra");
150 case 't' : return("tango");
151 case 'u' : return("uniform");
152 case 'v' : return("victor");
153 case 'w' : return("whiskey");
154 case 'x' : return("x-ray");
155 case 'y' : return("yankee");
156 case 'z' : return("zulu");
158 // We shouldn't get here
162 // Get the compass direction associated with a heading in degrees
163 // Currently returns 8 direction resolution (N, NE, E etc...)
164 // Might be modified in future to return 4, 8 or 16 resolution but defaulting to 8.
165 string GetCompassDirection(double h) {
166 while(h < 0.0) h += 360.0;
167 while(h > 360.0) h -= 360.0;
168 if(h < 22.5 || h > 337.5) {
170 } else if(h < 67.5) {
171 return("North-East");
172 } else if(h < 112.5) {
174 } else if(h < 157.5) {
175 return("South-East");
176 } else if(h < 202.5) {
178 } else if(h < 247.5) {
179 return("South-West");
180 } else if(h < 292.5) {
183 return("North-West");
187 //================================================================================================================
189 // Given two positions (lat & lon in degrees), get the HORIZONTAL separation (in meters)
190 double dclGetHorizontalSeparation(const Point3D& pos1, const Point3D& pos2) {
191 double x; //East-West separation
192 double y; //North-South separation
193 double z; //Horizontal separation - z = sqrt(x^2 + y^2)
195 double lat1 = pos1.lat() * SG_DEGREES_TO_RADIANS;
196 double lon1 = pos1.lon() * SG_DEGREES_TO_RADIANS;
197 double lat2 = pos2.lat() * SG_DEGREES_TO_RADIANS;
198 double lon2 = pos2.lon() * SG_DEGREES_TO_RADIANS;
200 y = sin(fabs(lat1 - lat2)) * SG_EQUATORIAL_RADIUS_M;
201 x = sin(fabs(lon1 - lon2)) * SG_EQUATORIAL_RADIUS_M * (cos((lat1 + lat2) / 2.0));
207 // Given a point and a line, get the HORIZONTAL shortest distance from the point to a point on the line.
208 // Expects to be fed orthogonal co-ordinates, NOT lat & lon !
209 // The units of the separation will be those of the input.
210 double dclGetLinePointSeparation(double px, double py, double x1, double y1, double x2, double y2) {
213 double magline = sqrt(vecx*vecx + vecy*vecy);
214 double u = ((px-x1)*(x2-x1) + (py-y1)*(y2-y1)) / (magline * magline);
215 double x0 = x1 + u*(x2-x1);
216 double y0 = y1 + u*(y2-y1);
219 double d = sqrt(vecx*vecx + vecy*vecy);
226 // Given a position (lat/lon/elev), heading and vertical angle (degrees), and distance (meters), calculate the new position.
227 // This function assumes the world is spherical. If geodetic accuracy is required use the functions is sg_geodesy instead!
228 // Assumes that the ground is not hit!!! Expects heading and angle in degrees, distance in meters.
229 Point3D dclUpdatePosition(const Point3D& pos, double heading, double angle, double distance) {
230 //cout << setprecision(10) << pos.lon() << ' ' << pos.lat() << '\n';
231 heading *= DCL_DEGREES_TO_RADIANS;
232 angle *= DCL_DEGREES_TO_RADIANS;
233 double lat = pos.lat() * DCL_DEGREES_TO_RADIANS;
234 double lon = pos.lon() * DCL_DEGREES_TO_RADIANS;
235 double elev = pos.elev();
236 //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
238 double horiz_dist = distance * cos(angle);
239 double vert_dist = distance * sin(angle);
241 double north_dist = horiz_dist * cos(heading);
242 double east_dist = horiz_dist * sin(heading);
244 //cout << distance << ' ' << horiz_dist << ' ' << vert_dist << ' ' << north_dist << ' ' << east_dist << '\n';
246 double delta_lat = asin(north_dist / (double)SG_EQUATORIAL_RADIUS_M);
247 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.
248 //cout << delta_lon*DCL_RADIANS_TO_DEGREES << ' ' << delta_lat*DCL_RADIANS_TO_DEGREES << '\n';
252 //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
254 //cout << setprecision(15) << DCL_DEGREES_TO_RADIANS * DCL_RADIANS_TO_DEGREES << '\n';
256 return(Point3D(lon*DCL_RADIANS_TO_DEGREES, lat*DCL_RADIANS_TO_DEGREES, elev));
259 // Get a heading in degrees from one lat/lon to another.
260 // This function assumes the world is spherical. If geodetic accuracy is required use the functions is sg_geodesy instead!
261 // Warning - at the moment we are not checking for identical points - currently it returns 0 in this instance.
262 double GetHeadingFromTo(const Point3D& A, const Point3D& B) {
263 double latA = A.lat() * DCL_DEGREES_TO_RADIANS;
264 double lonA = A.lon() * DCL_DEGREES_TO_RADIANS;
265 double latB = B.lat() * DCL_DEGREES_TO_RADIANS;
266 double lonB = B.lon() * DCL_DEGREES_TO_RADIANS;
267 double xdist = sin(lonB - lonA) * (double)SG_EQUATORIAL_RADIUS_M * cos((latA+latB)/2.0);
268 double ydist = sin(latB - latA) * (double)SG_EQUATORIAL_RADIUS_M;
269 double heading = atan2(xdist, ydist) * DCL_RADIANS_TO_DEGREES;
270 return heading < 0.0 ? heading + 360 : heading;
273 // Given a heading (in degrees), bound it from 0 -> 360
274 void dclBoundHeading(double &hdg) {
283 // smallest difference between two angles in degrees
284 // difference is negative if a1 > a2 and positive if a2 > a1
285 double GetAngleDiff_deg( const double &a1, const double &a2) {
288 while (a3 < 180.0) a3 += 360.0;
289 while (a3 > 180.0) a3 -= 360.0;
295 // Given a Point3D (lon/lat/elev) and an FGRunway struct, determine if the point lies on the runway
296 bool OnRunway(const Point3D& pt, const FGRunway& rwy) {
297 FGATCAlignedProjection ortho;
298 Point3D centre(rwy._lon, rwy._lat, 0.0); // We don't need the elev
299 ortho.Init(centre, rwy._heading);
301 Point3D xyc = ortho.ConvertToLocal(centre);
302 Point3D xyp = ortho.ConvertToLocal(pt);
304 //cout << "Length offset = " << fabs(xyp.y() - xyc.y()) << '\n';
305 //cout << "Width offset = " << fabs(xyp.x() - xyc.x()) << '\n';
307 if((fabs(xyp.y() - xyc.y()) < ((rwy._length/2.0) + 5.0))
308 && (fabs(xyp.x() - xyc.x()) < (rwy._width/2.0))) {