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