]> git.mxchange.org Git - flightgear.git/blob - src/ATC/ATCutils.cxx
ignore resets for now because every z/Z key press would trigger a call to NOAA. We...
[flightgear.git] / src / ATC / ATCutils.cxx
1 // ATCutils.cxx - Utility functions for the ATC / AI system
2 //
3 // Written by David Luff, started March 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., 675 Mass Ave, Cambridge, MA 02139, USA.
20
21 #include <math.h>
22 #include <simgear/math/point3d.hxx>
23 #include <simgear/constants.h>
24 #include <simgear/misc/sg_path.hxx>
25 #include <simgear/debug/logstream.hxx>
26 #include <plib/sg.h>
27 //#include <iomanip.h>
28
29 #include <Airports/runways.hxx>
30 #include <Main/globals.hxx>
31
32 #include "ATCutils.hxx"
33 #include "ATCProjection.hxx"
34
35 // Convert any number to spoken digits
36 string ConvertNumToSpokenDigits(string n) {
37         //cout << "n = " << n << endl;
38         string nums[10] = {"zero", "one", "two", "three", "four", "five", "six", "seven", "eight", "nine"};
39         string pt = "decimal";
40         string str = "";
41         
42         for(unsigned int i=0; i<n.length(); ++i) {
43                 //cout << "n.substr(" << i << ",1 = " << n.substr(i,1) << endl;
44                 if(n.substr(i,1) == " ") {
45                         // do nothing
46                 } else if(n.substr(i,1) == ".") {
47                         str += pt;
48                 } else {
49                         str += nums[atoi((n.substr(i,1)).c_str())];
50                 }
51                 if(i != (n.length()-1)) {       // ie. don't add a space at the end.
52                         str += " ";
53                 }
54         }
55         return(str);
56 }
57
58
59 // Convert an integer to spoken digits
60 string ConvertNumToSpokenDigits(int n) {
61         char buf[12];   // should be big enough!!
62         sprintf(buf, "%i", n);
63         string tempstr1 = buf;
64         return(ConvertNumToSpokenDigits(tempstr1));
65 }
66
67
68 // Convert a 2 digit rwy number to a spoken-style string
69 string ConvertRwyNumToSpokenString(int n) {
70         string nums[10] = {"zero", "one", "two", "three", "four", "five", "six", "seven", "eight", "nine"};
71         // Basic error/sanity checking
72         while(n < 0) {
73                 n += 36;
74         }
75         while(n > 36) {
76                 n -= 36;
77         }
78         if(n == 0) {
79                 n = 36; // Is this right?
80         }
81         
82         string str = "";
83         int index = n/10;
84         str += nums[index];
85         n -= (index * 10);
86         //str += "-";
87         str += " ";             //Changed this for the benefit of the voice token parser - prefer the "-" in the visual output though.
88         str += nums[n];
89         return(str);
90 }
91
92 // Assumes we get a two-digit string optionally appended with L, R or C
93 // eg 01 07L 29R 36
94 // Anything else is not guaranteed to be handled correctly!
95 string ConvertRwyNumToSpokenString(string s) {
96         if(s.size() < 3) {
97                 return(ConvertRwyNumToSpokenString(atoi(s.c_str())));
98         } else {
99                 string r = ConvertRwyNumToSpokenString(atoi(s.substr(0,2).c_str()));
100                 if(s.substr(2,1) == "L") {
101                         r += " left";
102                 } else if(s.substr(2,1) == "R") {
103                         r += " right";
104                 } else if(s.substr(2,1) == "C") {
105                         r += " center";
106                 } else {
107                         SG_LOG(SG_ATC, SG_WARN, "WARNING: Unknown suffix " << s.substr(2,1) << " from runway ID " << s << " in ConvertRwyNumToSpokenString(...)");
108                 }
109                 return(r);
110         }
111 }
112         
113
114 // Return the phonetic letter of a letter represented as an integer 1->26
115 string GetPhoneticIdent(int i) {
116         // TODO - Check i is between 1 and 26 and wrap if necessary
117         return(GetPhoneticIdent(char('a' + (i-1))));
118 }
119
120 // Return the phonetic letter of a character in the range a-z or A-Z.
121 // Currently always returns prefixed by lowercase.
122 string GetPhoneticIdent(char c) {
123         c = tolower(c);
124         // TODO - Check c is between a and z and wrap if necessary
125         switch(c) {
126         case 'a' : return("alpha");
127         case 'b' : return("bravo");
128         case 'c' : return("charlie");
129         case 'd' : return("delta");
130         case 'e' : return("echo");
131         case 'f' : return("foxtrot");
132         case 'g' : return("golf");
133         case 'h' : return("hotel");
134         case 'i' : return("india");
135         case 'j' : return("juliet");
136         case 'k' : return("kilo");
137         case 'l' : return("lima");
138         case 'm' : return("mike");
139         case 'n' : return("november");
140         case 'o' : return("oscar");
141         case 'p' : return("papa");
142         case 'q' : return("quebec");
143         case 'r' : return("romeo");
144         case 's' : return("sierra");
145         case 't' : return("tango");
146         case 'u' : return("uniform");
147         case 'v' : return("victor");
148         case 'w' : return("whiskey");
149         case 'x' : return("x-ray");
150         case 'y' : return("yankee");
151         case 'z' : return("zulu");
152         }
153         // We shouldn't get here
154         return("Error");
155 }
156
157 // Get the compass direction associated with a heading in degrees
158 // Currently returns 8 direction resolution (N, NE, E etc...)
159 // Might be modified in future to return 4, 8 or 16 resolution but defaulting to 8. 
160 string GetCompassDirection(double h) {
161         while(h < 0.0) h += 360.0;
162         while(h > 360.0) h -= 360.0;
163         if(h < 22.5 || h > 337.5) {
164                 return("North");
165         } else if(h < 67.5) {
166                 return("North-East");
167         } else if(h < 112.5) {
168                 return("East");
169         } else if(h < 157.5) {
170                 return("South-East");
171         } else if(h < 202.5) {
172                 return("South");
173         } else if(h < 247.5) {
174                 return("South-West");
175         } else if(h < 292.5) {
176                 return("West");
177         } else {
178                 return("North-West");
179         }
180 }
181
182 //================================================================================================================
183
184 // Given two positions (lat & lon in degrees), get the HORIZONTAL separation (in meters)
185 double dclGetHorizontalSeparation(Point3D pos1, Point3D pos2) {
186         double x;       //East-West separation
187         double y;       //North-South separation
188         double z;       //Horizontal separation - z = sqrt(x^2 + y^2)
189         
190         double lat1 = pos1.lat() * SG_DEGREES_TO_RADIANS;
191         double lon1 = pos1.lon() * SG_DEGREES_TO_RADIANS;
192         double lat2 = pos2.lat() * SG_DEGREES_TO_RADIANS;
193         double lon2 = pos2.lon() * SG_DEGREES_TO_RADIANS;
194         
195         y = sin(fabs(lat1 - lat2)) * SG_EQUATORIAL_RADIUS_M;
196         x = sin(fabs(lon1 - lon2)) * SG_EQUATORIAL_RADIUS_M * (cos((lat1 + lat2) / 2.0));
197         z = sqrt(x*x + y*y);
198         
199         return(z);
200 }
201
202 // Given a point and a line, get the HORIZONTAL shortest distance from the point to a point on the line.
203 // Expects to be fed orthogonal co-ordinates, NOT lat & lon !
204 // The units of the separation will be those of the input.
205 double dclGetLinePointSeparation(double px, double py, double x1, double y1, double x2, double y2) {
206         double vecx = x2-x1;
207         double vecy = y2-y1;
208         double magline = sqrt(vecx*vecx + vecy*vecy);
209         double u = ((px-x1)*(x2-x1) + (py-y1)*(y2-y1)) / (magline * magline);
210         double x0 = x1 + u*(x2-x1);
211         double y0 = y1 + u*(y2-y1);
212         vecx = px - x0;
213         vecy = py - y0;
214         double d = sqrt(vecx*vecx + vecy*vecy);
215         if(d < 0) {
216                 d *= -1;
217         }
218         return(d);
219 }
220
221 // Given a position (lat/lon/elev), heading and vertical angle (degrees), and distance (meters), calculate the new position.
222 // This function assumes the world is spherical.  If geodetic accuracy is required use the functions is sg_geodesy instead!
223 // Assumes that the ground is not hit!!!  Expects heading and angle in degrees, distance in meters. 
224 Point3D dclUpdatePosition(Point3D pos, double heading, double angle, double distance) {
225         //cout << setprecision(10) << pos.lon() << ' ' << pos.lat() << '\n';
226         heading *= DCL_DEGREES_TO_RADIANS;
227         angle *= DCL_DEGREES_TO_RADIANS;
228         double lat = pos.lat() * DCL_DEGREES_TO_RADIANS;
229         double lon = pos.lon() * DCL_DEGREES_TO_RADIANS;
230         double elev = pos.elev();
231         //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
232         
233         double horiz_dist = distance * cos(angle);
234         double vert_dist = distance * sin(angle);
235         
236         double north_dist = horiz_dist * cos(heading);
237         double east_dist = horiz_dist * sin(heading);
238         
239         //cout << distance << ' ' << horiz_dist << ' ' << vert_dist << ' ' << north_dist << ' ' << east_dist << '\n';
240         
241         double delta_lat = asin(north_dist / (double)SG_EQUATORIAL_RADIUS_M);
242         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.
243         //cout << delta_lon*DCL_RADIANS_TO_DEGREES << ' ' << delta_lat*DCL_RADIANS_TO_DEGREES << '\n';
244         lat += delta_lat;
245         lon += delta_lon;
246         elev += vert_dist;
247         //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
248         
249         //cout << setprecision(15) << DCL_DEGREES_TO_RADIANS * DCL_RADIANS_TO_DEGREES << '\n';
250         
251         return(Point3D(lon*DCL_RADIANS_TO_DEGREES, lat*DCL_RADIANS_TO_DEGREES, elev));
252 }
253
254 // Get a heading in degrees from one lat/lon to another.
255 // This function assumes the world is spherical.  If geodetic accuracy is required use the functions is sg_geodesy instead!
256 // Warning - at the moment we are not checking for identical points - currently it returns 90 in this instance.
257 double GetHeadingFromTo(Point3D A, Point3D B) {
258         double latA = A.lat() * DCL_DEGREES_TO_RADIANS;
259         double lonA = A.lon() * DCL_DEGREES_TO_RADIANS;
260         double latB = B.lat() * DCL_DEGREES_TO_RADIANS;
261         double lonB = B.lon() * DCL_DEGREES_TO_RADIANS;
262         double xdist = sin(lonB - lonA) * (double)SG_EQUATORIAL_RADIUS_M * cos((latA+latB)/2.0);
263         double ydist = sin(latB - latA) * (double)SG_EQUATORIAL_RADIUS_M;
264         
265         if(xdist >= 0) {
266                 if(ydist > 0) {
267                         return(atan(xdist/ydist) * DCL_RADIANS_TO_DEGREES);
268                 } else if (ydist == 0) {
269                         return(90.0);
270                 } else {
271                         return(180.0 - atan(xdist/fabs(ydist)) * DCL_RADIANS_TO_DEGREES);
272                 }
273         } else {
274                 if(ydist > 0) {
275                         return(360.0 - atan(fabs(xdist)/ydist) * DCL_RADIANS_TO_DEGREES);
276                 } else if (ydist == 0) {
277                         return(270.0);
278                 } else {
279                         return(180.0 + atan(xdist/ydist) * DCL_RADIANS_TO_DEGREES);
280                 }
281         }
282 }
283
284 // Given a heading (in degrees), bound it from 0 -> 360
285 void dclBoundHeading(double &hdg) {
286         while(hdg < 0.0) {
287                 hdg += 360.0;
288         }
289         while(hdg > 360.0) {
290                 hdg -= 360.0;
291         }
292 }
293
294 // smallest difference between two angles in degrees
295 // difference is negative if a1 > a2 and positive if a2 > a1
296 double GetAngleDiff_deg( const double &a1, const double &a2) {
297   
298   double a3 = a2 - a1;
299   while (a3 < 180.0) a3 += 360.0;
300   while (a3 > 180.0) a3 -= 360.0;
301
302   return a3;
303 }
304
305 //================================================================================================================
306
307 // Airport stuff.  The next two functions are straight copies of their fg.... equivalents
308 // in fg_init.cxx, and are just here temporarily until some rationalisation occurs.
309 // find basic airport location info from airport database
310 bool dclFindAirportID( const string& id, FGAirport *a ) {
311     FGAirport result;
312
313     if ( id.length() ) {
314         SG_LOG( SG_GENERAL, SG_INFO, "Searching for airport code = " << id );
315
316         result = globals->get_airports()->search( id );
317         if ( result.id.empty() ) {
318             SG_LOG( SG_GENERAL, SG_ALERT,
319                     "Failed to find " << id << " in basic.dat.gz" );
320             return false;
321         }
322     } else {
323         return false;
324     }
325
326     *a = result;
327
328     SG_LOG( SG_GENERAL, SG_INFO,
329             "Position for " << id << " is ("
330             << a->longitude << ", "
331             << a->latitude << ")" );
332
333     return true;
334 }
335
336 // get airport elevation
337 double dclGetAirportElev( const string& id ) {
338     FGAirport a;
339     // double lon, lat;
340
341     SG_LOG( SG_ATC, SG_INFO,
342             "Finding elevation for airport: " << id );
343
344     if ( dclFindAirportID( id, &a ) ) {
345         return a.elevation * SG_FEET_TO_METER;
346     } else {
347         return -9999.0;
348     }
349 }
350
351 // get airport position
352 Point3D dclGetAirportPos( const string& id ) {
353     FGAirport a;
354     // double lon, lat;
355
356     SG_LOG( SG_ATC, SG_INFO,
357             "Finding position for airport: " << id );
358
359     if ( dclFindAirportID( id, &a ) ) {
360         return Point3D(a.longitude, a.latitude, a.elevation);
361     } else {
362         return Point3D(0.0, 0.0, -9999.0);
363     }
364 }       
365
366 // Runway stuff
367 // Given a Point3D (lon/lat/elev) and an FGRunway struct, determine if the point lies on the runway
368 bool OnRunway(Point3D pt, const FGRunway& rwy) {
369         FGATCAlignedProjection ortho;
370         Point3D centre(rwy.lon, rwy.lat, 0.0);  // We don't need the elev
371         ortho.Init(centre, rwy.heading);
372         
373         Point3D xyc = ortho.ConvertToLocal(centre);
374         Point3D xyp = ortho.ConvertToLocal(pt);
375         
376         //cout << "Length offset = " << fabs(xyp.y() - xyc.y()) << '\n';
377         //cout << "Width offset = " << fabs(xyp.x() - xyc.x()) << '\n';
378         
379         if((fabs(xyp.y() - xyc.y()) < ((rwy.length/2.0) + 5.0)) 
380                 && (fabs(xyp.x() - xyc.x()) < (rwy.width/2.0))) {
381                 return(true);
382         }
383         
384         return(false);
385 }
386