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         switch(i) {
 118         case 1 : return("alpha");
 119         case 2 : return("bravo");
 120         case 3 : return("charlie");
 121         case 4 : return("delta");
 122         case 5 : return("echo");
 123         case 6 : return("foxtrot");
 124         case 7 : return("golf");
 125         case 8 : return("hotel");
 126         case 9 : return("india");
 127         case 10 : return("juliet");
 128         case 11 : return("kilo");
 129         case 12 : return("lima");
 130         case 13 : return("mike");
 131         case 14 : return("november");
 132         case 15 : return("oscar");
 133         case 16 : return("papa");
 134         case 17 : return("quebec");
 135         case 18 : return("romeo");
 136         case 19 : return("sierra");
 137         case 20 : return("tango");
 138         case 21 : return("uniform");
 139         case 22 : return("victor");
 140         case 23 : return("whiskey");
 141         case 24 : return("x-ray");
 142         case 25 : return("yankee");
 143         case 26 : return("zulu");
 144         }
 145         // We shouldn't get here
 146         return("Error");
 147 }
 148
 149 //================================================================================================================
 150
 151 // Given two positions (lat & lon in degrees), get the HORIZONTAL separation (in meters)
 152 double dclGetHorizontalSeparation(Point3D pos1, Point3D pos2) {
 153         double x;       //East-West separation
 154         double y;       //North-South separation
 155         double z;       //Horizontal separation - z = sqrt(x^2 + y^2)
 156
 157         double lat1 = pos1.lat() * SG_DEGREES_TO_RADIANS;
 158         double lon1 = pos1.lon() * SG_DEGREES_TO_RADIANS;
 159         double lat2 = pos2.lat() * SG_DEGREES_TO_RADIANS;
 160         double lon2 = pos2.lon() * SG_DEGREES_TO_RADIANS;
 161
 162         y = sin(fabs(lat1 - lat2)) * SG_EQUATORIAL_RADIUS_M;
 163         x = sin(fabs(lon1 - lon2)) * SG_EQUATORIAL_RADIUS_M * (cos((lat1 + lat2) / 2.0));
 164         z = sqrt(x*x + y*y);
 165
 166         return(z);
 167 }
 168
 169 // Given a point and a line, get the HORIZONTAL shortest distance from the point to a point on the line.
 170 // Expects to be fed orthogonal co-ordinates, NOT lat & lon !
 171 // The units of the separation will be those of the input.
 172 double dclGetLinePointSeparation(double px, double py, double x1, double y1, double x2, double y2) {
 173         double vecx = x2-x1;
 174         double vecy = y2-y1;
 175         double magline = sqrt(vecx*vecx + vecy*vecy);
 176         double u = ((px-x1)*(x2-x1) + (py-y1)*(y2-y1)) / (magline * magline);
 177         double x0 = x1 + u*(x2-x1);
 178         double y0 = y1 + u*(y2-y1);
 179         vecx = px - x0;
 180         vecy = py - y0;
 181         double d = sqrt(vecx*vecx + vecy*vecy);
 182         if(d < 0) {
 183                 d *= -1;
 184         }
 185         return(d);
 186 }
 187
 188 // Given a position (lat/lon/elev), heading and vertical angle (degrees), and distance (meters), calculate the new position.
 189 // This function assumes the world is spherical.  If geodetic accuracy is required use the functions is sg_geodesy instead!
 190 // Assumes that the ground is not hit!!!  Expects heading and angle in degrees, distance in meters.
 191 Point3D dclUpdatePosition(Point3D pos, double heading, double angle, double distance) {
 192         //cout << setprecision(10) << pos.lon() << ' ' << pos.lat() << '\n';
 193         heading *= DCL_DEGREES_TO_RADIANS;
 194         angle *= DCL_DEGREES_TO_RADIANS;
 195         double lat = pos.lat() * DCL_DEGREES_TO_RADIANS;
 196         double lon = pos.lon() * DCL_DEGREES_TO_RADIANS;
 197         double elev = pos.elev();
 198         //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
 199
 200         double horiz_dist = distance * cos(angle);
 201         double vert_dist = distance * sin(angle);
 202
 203         double north_dist = horiz_dist * cos(heading);
 204         double east_dist = horiz_dist * sin(heading);
 205
 206         //cout << distance << ' ' << horiz_dist << ' ' << vert_dist << ' ' << north_dist << ' ' << east_dist << '\n';
 207
 208         double delta_lat = asin(north_dist / (double)SG_EQUATORIAL_RADIUS_M);
 209         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.
 210         //cout << delta_lon*DCL_RADIANS_TO_DEGREES << ' ' << delta_lat*DCL_RADIANS_TO_DEGREES << '\n';
 211         lat += delta_lat;
 212         lon += delta_lon;
 213         elev += vert_dist;
 214         //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
 215
 216         //cout << setprecision(15) << DCL_DEGREES_TO_RADIANS * DCL_RADIANS_TO_DEGREES << '\n';
 217
 218         return(Point3D(lon*DCL_RADIANS_TO_DEGREES, lat*DCL_RADIANS_TO_DEGREES, elev));
 219 }
 220
 221 // Get a heading in degrees from one lat/lon to another.
 222 // This function assumes the world is spherical.  If geodetic accuracy is required use the functions is sg_geodesy instead!
 223 // Warning - at the moment we are not checking for identical points - currently it returns 90 in this instance.
 224 double GetHeadingFromTo(Point3D A, Point3D B) {
 225         double latA = A.lat() * DCL_DEGREES_TO_RADIANS;
 226         double lonA = A.lon() * DCL_DEGREES_TO_RADIANS;
 227         double latB = B.lat() * DCL_DEGREES_TO_RADIANS;
 228         double lonB = B.lon() * DCL_DEGREES_TO_RADIANS;
 229         double xdist = sin(lonB - lonA) * (double)SG_EQUATORIAL_RADIUS_M * cos((latA+latB)/2.0);
 230         double ydist = sin(latB - latA) * (double)SG_EQUATORIAL_RADIUS_M;
 231
 232         if(xdist >= 0) {
 233                 if(ydist > 0) {
 234                         return(atan(xdist/ydist) * DCL_RADIANS_TO_DEGREES);
 235                 } else if (ydist == 0) {
 236                         return(90.0);
 237                 } else {
 238                         return(180.0 - atan(xdist/fabs(ydist)) * DCL_RADIANS_TO_DEGREES);
 239                 }
 240         } else {
 241                 if(ydist > 0) {
 242                         return(360.0 - atan(fabs(xdist)/ydist) * DCL_RADIANS_TO_DEGREES);
 243                 } else if (ydist == 0) {
 244                         return(270.0);
 245                 } else {
 246                         return(180.0 + atan(xdist/ydist) * DCL_RADIANS_TO_DEGREES);
 247                 }
 248         }
 249 }
 250
 251 // Given a heading (in degrees), bound it from 0 -> 360
 252 void dclBoundHeading(double &hdg) {
 253         while(hdg < 0.0) {
 254                 hdg += 360.0;
 255         }
 256         while(hdg > 360.0) {
 257                 hdg -= 360.0;
 258         }
 259 }
 260
 261 // smallest difference between two angles in degrees
 262 // difference is negative if a1 > a2 and positive if a2 > a1
 263 double GetAngleDiff_deg( const double &a1, const double &a2) {
 264
 265   double a3 = a2 - a1;
 266   while (a3 < 180.0) a3 += 360.0;
 267   while (a3 > 180.0) a3 -= 360.0;
 268
 269   return a3;
 270 }
 271
 272 //================================================================================================================
 273
 274 // Airport stuff.  The next two functions are straight copies of their fg.... equivalents
 275 // in fg_init.cxx, and are just here temporarily until some rationalisation occurs.
 276 // find basic airport location info from airport database
 277 bool dclFindAirportID( const string& id, FGAirport *a ) {
 278     FGAirport result;
 279
 280     if ( id.length() ) {
 281         SG_LOG( SG_GENERAL, SG_INFO, "Searching for airport code = " << id );
 282
 283         result = globals->get_airports()->search( id );
 284         if ( result.id.empty() ) {
 285             SG_LOG( SG_GENERAL, SG_ALERT,
 286                     "Failed to find " << id << " in basic.dat.gz" );
 287             return false;
 288         }
 289     } else {
 290         return false;
 291     }
 292
 293     *a = result;
 294
 295     SG_LOG( SG_GENERAL, SG_INFO,
 296             "Position for " << id << " is ("
 297             << a->longitude << ", "
 298             << a->latitude << ")" );
 299
 300     return true;
 301 }
 302
 303 // get airport elevation
 304 double dclGetAirportElev( const string& id ) {
 305     FGAirport a;
 306     // double lon, lat;
 307
 308     SG_LOG( SG_GENERAL, SG_INFO,
 309             "Finding elevation for airport: " << id );
 310
 311     if ( dclFindAirportID( id, &a ) ) {
 312         return a.elevation;
 313     } else {
 314         return -9999.0;
 315     }
 316 }
 317
 318 // Runway stuff
 319 // Given a Point3D (lon/lat/elev) and an FGRunway struct, determine if the point lies on the runway
 320 bool OnRunway(Point3D pt, const FGRunway& rwy) {
 321         FGATCAlignedProjection ortho;
 322         Point3D centre(rwy.lon, rwy.lat, 0.0);  // We don't need the elev
 323         ortho.Init(centre, rwy.heading);
 324
 325         Point3D xyc = ortho.ConvertToLocal(centre);
 326         Point3D xyp = ortho.ConvertToLocal(pt);
 327
 328         //cout << "Length offset = " << fabs(xyp.y() - xyc.y()) << '\n';
 329         //cout << "Width offset = " << fabs(xyp.x() - xyc.x()) << '\n';
 330
 331         if((fabs(xyp.y() - xyc.y()) < ((rwy.length/2.0) + 5.0))
 332                 && (fabs(xyp.x() - xyc.x()) < (rwy.width/2.0))) {
 333                 return(true);
 334         }
 335
 336         return(false);
 337 }
 338