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>
24 #include <simgear/misc/sg_path.hxx>
25 #include <simgear/debug/logstream.hxx>
27 //#include <iomanip.h>
29 #include <Airports/runways.hxx>
30 #include <Main/globals.hxx>
32 #include "ATCutils.hxx"
33 #include "ATCProjection.hxx"
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";
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) == " ") {
46 } else if(n.substr(i,1) == ".") {
49 str += nums[atoi((n.substr(i,1)).c_str())];
51 if(i != (n.length()-1)) { // ie. don't add a space at the end.
59 // Convert a 2 digit rwy number to a spoken-style string
60 string ConvertRwyNumToSpokenString(int n) {
61 string nums[10] = {"zero", "one", "two", "three", "four", "five", "six", "seven", "eight", "nine"};
62 // Basic error/sanity checking
70 n = 36; // Is this right?
78 str += " "; //Changed this for the benefit of the voice token parser - prefer the "-" in the visual output though.
83 // Assumes we get a two-digit string optionally appended with R or L
85 // Anything else is not guaranteed to be handled correctly!
86 string ConvertRwyNumToSpokenString(string s) {
88 return(ConvertRwyNumToSpokenString(atoi(s.c_str())));
90 string r = ConvertRwyNumToSpokenString(atoi(s.substr(0,2).c_str()));
91 return(r += (s.substr(2,1) == "L" ? " left" : " right")); // Warning - not much error checking there!
96 // Return the phonetic letter of a letter represented as an integer 1->26
97 string GetPhoneticIdent(int i) {
98 // TODO - Check i is between 1 and 26 and wrap if necessary
100 case 1 : return("alpha");
101 case 2 : return("bravo");
102 case 3 : return("charlie");
103 case 4 : return("delta");
104 case 5 : return("echo");
105 case 6 : return("foxtrot");
106 case 7 : return("golf");
107 case 8 : return("hotel");
108 case 9 : return("india");
109 case 10 : return("juliet");
110 case 11 : return("kilo");
111 case 12 : return("lima");
112 case 13 : return("mike");
113 case 14 : return("november");
114 case 15 : return("oscar");
115 case 16 : return("papa");
116 case 17 : return("quebec");
117 case 18 : return("romeo");
118 case 19 : return("sierra");
119 case 20 : return("tango");
120 case 21 : return("uniform");
121 case 22 : return("victor");
122 case 23 : return("whiskey");
123 case 24 : return("x-ray");
124 case 25 : return("yankee");
125 case 26 : return("zulu");
127 // We shouldn't get here
131 // Given two positions (lat & lon in degrees), get the HORIZONTAL separation (in meters)
132 double dclGetHorizontalSeparation(Point3D pos1, Point3D pos2) {
133 double x; //East-West separation
134 double y; //North-South separation
135 double z; //Horizontal separation - z = sqrt(x^2 + y^2)
137 double lat1 = pos1.lat() * SG_DEGREES_TO_RADIANS;
138 double lon1 = pos1.lon() * SG_DEGREES_TO_RADIANS;
139 double lat2 = pos2.lat() * SG_DEGREES_TO_RADIANS;
140 double lon2 = pos2.lon() * SG_DEGREES_TO_RADIANS;
142 y = sin(fabs(lat1 - lat2)) * SG_EQUATORIAL_RADIUS_M;
143 x = sin(fabs(lon1 - lon2)) * SG_EQUATORIAL_RADIUS_M * (cos((lat1 + lat2) / 2.0));
149 // Given a point and a line, get the HORIZONTAL shortest distance from the point to a point on the line.
150 // Expects to be fed orthogonal co-ordinates, NOT lat & lon !
151 // The units of the separation will be those of the input.
152 double dclGetLinePointSeparation(double px, double py, double x1, double y1, double x2, double y2) {
155 double magline = sqrt(vecx*vecx + vecy*vecy);
156 double u = ((px-x1)*(x2-x1) + (py-y1)*(y2-y1)) / (magline * magline);
157 double x0 = x1 + u*(x2-x1);
158 double y0 = y1 + u*(y2-y1);
161 double d = sqrt(vecx*vecx + vecy*vecy);
168 // Given a position (lat/lon/elev), heading and vertical angle (degrees), and distance (meters), calculate the new position.
169 // This function assumes the world is spherical. If geodetic accuracy is required use the functions is sg_geodesy instead!
170 // Assumes that the ground is not hit!!! Expects heading and angle in degrees, distance in meters.
171 Point3D dclUpdatePosition(Point3D pos, double heading, double angle, double distance) {
172 //cout << setprecision(10) << pos.lon() << ' ' << pos.lat() << '\n';
173 heading *= DCL_DEGREES_TO_RADIANS;
174 angle *= DCL_DEGREES_TO_RADIANS;
175 double lat = pos.lat() * DCL_DEGREES_TO_RADIANS;
176 double lon = pos.lon() * DCL_DEGREES_TO_RADIANS;
177 double elev = pos.elev();
178 //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
180 double horiz_dist = distance * cos(angle);
181 double vert_dist = distance * sin(angle);
183 double north_dist = horiz_dist * cos(heading);
184 double east_dist = horiz_dist * sin(heading);
186 //cout << distance << ' ' << horiz_dist << ' ' << vert_dist << ' ' << north_dist << ' ' << east_dist << '\n';
188 double delta_lat = asin(north_dist / (double)SG_EQUATORIAL_RADIUS_M);
189 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.
190 //cout << delta_lon*DCL_RADIANS_TO_DEGREES << ' ' << delta_lat*DCL_RADIANS_TO_DEGREES << '\n';
194 //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
196 //cout << setprecision(15) << DCL_DEGREES_TO_RADIANS * DCL_RADIANS_TO_DEGREES << '\n';
198 return(Point3D(lon*DCL_RADIANS_TO_DEGREES, lat*DCL_RADIANS_TO_DEGREES, elev));
201 // Get a heading in degrees from one lat/lon to another.
202 // This function assumes the world is spherical. If geodetic accuracy is required use the functions is sg_geodesy instead!
203 // Warning - at the moment we are not checking for identical points - currently it returns 90 in this instance.
204 double GetHeadingFromTo(Point3D A, Point3D B) {
205 double latA = A.lat() * DCL_DEGREES_TO_RADIANS;
206 double lonA = A.lon() * DCL_DEGREES_TO_RADIANS;
207 double latB = B.lat() * DCL_DEGREES_TO_RADIANS;
208 double lonB = B.lon() * DCL_DEGREES_TO_RADIANS;
209 double xdist = sin(lonB - lonA) * (double)SG_EQUATORIAL_RADIUS_M * cos((latA+latB)/2.0);
210 double ydist = sin(latB - latA) * (double)SG_EQUATORIAL_RADIUS_M;
214 return(atan(xdist/ydist) * DCL_RADIANS_TO_DEGREES);
215 } else if (ydist == 0) {
218 return(180.0 - atan(xdist/fabs(ydist)) * DCL_RADIANS_TO_DEGREES);
222 return(360.0 - atan(fabs(xdist)/ydist) * DCL_RADIANS_TO_DEGREES);
223 } else if (ydist == 0) {
226 return(180.0 + atan(xdist/ydist) * DCL_RADIANS_TO_DEGREES);
231 // Given a heading (in degrees), bound it from 0 -> 360
232 void dclBoundHeading(double &hdg) {
241 // Airport stuff. The next two functions are straight copies of their fg.... equivalents
242 // in fg_init.cxx, and are just here temporarily until some rationalisation occurs.
243 // find basic airport location info from airport database
244 bool dclFindAirportID( const string& id, FGAirport *a ) {
246 SGPath path( globals->get_fg_root() );
247 path.append( "Airports" );
248 path.append( "simple.mk4" );
249 FGAirports airports( path.c_str() );
251 SG_LOG( SG_GENERAL, SG_INFO, "Searching for airport code = " << id );
253 if ( ! airports.search( id, a ) ) {
254 SG_LOG( SG_GENERAL, SG_ALERT,
255 "Failed to find " << id << " in " << path.str() );
262 SG_LOG( SG_GENERAL, SG_INFO,
263 "Position for " << id << " is ("
264 << a->longitude << ", "
265 << a->latitude << ")" );
270 // get airport elevation
271 double dclGetAirportElev( const string& id ) {
275 SG_LOG( SG_GENERAL, SG_INFO,
276 "Finding elevation for airport: " << id );
278 if ( dclFindAirportID( id, &a ) ) {
286 // Given a Point3D (lon/lat/elev) and an FGRunway struct, determine if the point lies on the runway
287 bool OnRunway(Point3D pt, FGRunway* rwy) {
288 FGATCAlignedProjection ortho;
289 Point3D centre(rwy->lon, rwy->lat, 0.0); // We don't need the elev
290 ortho.Init(centre, rwy->heading);
292 Point3D xyc = ortho.ConvertToLocal(centre);
293 Point3D xyp = ortho.ConvertToLocal(pt);
295 //cout << "Length offset = " << fabs(xyp.y() - xyc.y()) << '\n';
296 //cout << "Width offset = " << fabs(xyp.x() - xyc.x()) << '\n';
298 if((fabs(xyp.y() - xyc.y()) < ((rwy->length/2.0) + 5.0))
299 && (fabs(xyp.x() - xyc.x()) < (rwy->width/2.0))) {