src/ATCDCL/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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  20
  21 #ifdef HAVE_CONFIG_H
  22 #  include <config.h>
  23 #endif
  24
  25 #include <sstream>
  26 #include <cstdlib>
  27
  28 #include <simgear/math/SGMath.hxx>
  29 #include <simgear/constants.h>
  30 #include <simgear/misc/sg_path.hxx>
  31 #include <simgear/debug/logstream.hxx>
  32
  33 #include <Airports/runways.hxx>
  34 #include <Main/globals.hxx>
  35
  36 #include "ATCutils.hxx"
  37 #include "ATCProjection.hxx"
  38
  39 static const string nums[10] = {"zero", "one", "two", "three", "four",
  40                                 "five", "six", "seven", "eight", "niner"};
  41
  42 static const string letters[LTRS] = {
  43     "alpha",    "bravo",    "charlie",   "delta",     "echo",
  44     "foxtrot",  "golf",     "hotel",     "india",     "juliet",
  45     "kilo",     "lima",     "mike",      "november",  "oscar",
  46     "papa",     "quebec",   "romeo",     "sierra",    "tango",
  47     "uniform",  "victor",   "whiskey",   "xray",      "yankee",    "zulu"
  48 };
  49
  50 // Convert any number to spoken digits
  51 string ConvertNumToSpokenDigits(const string &n) {
  52         //cout << "n = " << n << endl;
  53         static const string pt = "decimal";
  54         string str = "";
  55
  56         for(unsigned int i=0; i<n.length(); ++i) {
  57                 //cout << "n.substr(" << i << ",1 = " << n.substr(i,1) << endl;
  58                 if(n.substr(i,1) == " ") {
  59                         // do nothing
  60                 } else if(n.substr(i,1) == ".") {
  61                         str += pt;
  62                 } else {
  63                         str += nums[atoi((n.substr(i,1)).c_str())];
  64                 }
  65                 if(i != (n.length()-1)) {       // ie. don't add a space at the end.
  66                         str += " ";
  67                 }
  68         }
  69         return(str);
  70 }
  71
  72 // Convert an integer to a decimal numeral string
  73 string decimalNumeral(const int& n) {
  74         std::ostringstream buf;
  75         buf << n;
  76         return buf.str();
  77 }
  78
  79 // Convert an integer to spoken digits
  80 string ConvertNumToSpokenDigits(const int& n) {
  81         return ConvertNumToSpokenDigits(decimalNumeral(n));
  82 }
  83
  84
  85 // Assumes we get a string of digits optionally appended with L, R or C
  86 // eg 1 7L 29R 36
  87 // Anything else is not guaranteed to be handled correctly!
  88 string ConvertRwyNumToSpokenString(const string &rwy) {
  89   string rslt;
  90   for (size_t ii = 0; ii < rwy.length(); ii++){
  91     if (rslt.length()) rslt += " ";
  92     string ch = rwy.substr(ii,1);
  93     if (isdigit(ch[0])) rslt += ConvertNumToSpokenDigits(atoi(ch.c_str()));
  94     else if (ch == "R") rslt += "right";
  95     else if (ch == "C") rslt += "center";
  96     else if (ch == "L") rslt += "left";
  97     else {
  98       rslt += GetPhoneticLetter(ch[0]);
  99       SG_LOG(SG_ATC, SG_WARN, "WARNING: Unknown suffix '" << ch
 100           << "' in runway " << rwy << " in ConvertRwyNumToSpokenString(...)");
 101     }
 102   }
 103   return rslt;
 104 }
 105
 106
 107 // Return the phonetic letter of a letter represented as an integer 1->26
 108 string GetPhoneticLetter(const int i) {
 109         return(letters[i % LTRS]);
 110 }
 111
 112 // Return the phonetic letter of a character in the range a-z or A-Z.
 113 // Currently always returns prefixed by lowercase.
 114 string GetPhoneticLetter(const char c) {
 115         return GetPhoneticLetter(int(tolower(c) - 'a'));
 116 }
 117
 118 // Get the compass direction associated with a heading in degrees
 119 // Currently returns 8 direction resolution (N, NE, E etc...)
 120 // Might be modified in future to return 4, 8 or 16 resolution but defaulting to 8.
 121 string GetCompassDirection(double h) {
 122         while(h < 0.0) h += 360.0;
 123         while(h > 360.0) h -= 360.0;
 124         if(h < 22.5 || h > 337.5) {
 125                 return("North");
 126         } else if(h < 67.5) {
 127                 return("North-East");
 128         } else if(h < 112.5) {
 129                 return("East");
 130         } else if(h < 157.5) {
 131                 return("South-East");
 132         } else if(h < 202.5) {
 133                 return("South");
 134         } else if(h < 247.5) {
 135                 return("South-West");
 136         } else if(h < 292.5) {
 137                 return("West");
 138         } else {
 139                 return("North-West");
 140         }
 141 }
 142
 143 //================================================================================================================
 144
 145 // Given two positions (lat & lon in degrees), get the HORIZONTAL separation (in meters)
 146 double dclGetHorizontalSeparation(const SGGeod& pos1, const SGGeod& pos2) {
 147         double x;       //East-West separation
 148         double y;       //North-South separation
 149         double z;       //Horizontal separation - z = sqrt(x^2 + y^2)
 150
 151         double lat1 = pos1.getLatitudeRad();
 152         double lon1 = pos1.getLongitudeRad();
 153         double lat2 = pos2.getLatitudeRad();
 154         double lon2 = pos2.getLongitudeRad();
 155
 156         y = sin(fabs(lat1 - lat2)) * SG_EQUATORIAL_RADIUS_M;
 157         x = sin(fabs(lon1 - lon2)) * SG_EQUATORIAL_RADIUS_M * (cos((lat1 + lat2) / 2.0));
 158         z = sqrt(x*x + y*y);
 159
 160         return(z);
 161 }
 162
 163 // Given a point and a line, get the HORIZONTAL shortest distance from the point to a point on the line.
 164 // Expects to be fed orthogonal co-ordinates, NOT lat & lon !
 165 // The units of the separation will be those of the input.
 166 double dclGetLinePointSeparation(double px, double py, double x1, double y1, double x2, double y2) {
 167         double vecx = x2-x1;
 168         double vecy = y2-y1;
 169         double magline = sqrt(vecx*vecx + vecy*vecy);
 170         double u = ((px-x1)*(x2-x1) + (py-y1)*(y2-y1)) / (magline * magline);
 171         double x0 = x1 + u*(x2-x1);
 172         double y0 = y1 + u*(y2-y1);
 173         vecx = px - x0;
 174         vecy = py - y0;
 175         double d = sqrt(vecx*vecx + vecy*vecy);
 176         if(d < 0) {
 177                 d *= -1;
 178         }
 179         return(d);
 180 }
 181
 182 // Given a position (lat/lon/elev), heading and vertical angle (degrees), and distance (meters), calculate the new position.
 183 // This function assumes the world is spherical.  If geodetic accuracy is required use the functions is sg_geodesy instead!
 184 // Assumes that the ground is not hit!!!  Expects heading and angle in degrees, distance in meters.
 185 SGGeod dclUpdatePosition(const SGGeod& pos, double heading, double angle, double distance) {
 186     // FIXME: use SGGeodesy instead ...
 187
 188         //cout << setprecision(10) << pos.lon() << ' ' << pos.lat() << '\n';
 189         heading *= DCL_DEGREES_TO_RADIANS;
 190         angle *= DCL_DEGREES_TO_RADIANS;
 191         double lat = pos.getLatitudeRad();
 192         double lon = pos.getLongitudeRad();
 193         double elev = pos.getElevationM();
 194         //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
 195
 196         double horiz_dist = distance * cos(angle);
 197         double vert_dist = distance * sin(angle);
 198
 199         double north_dist = horiz_dist * cos(heading);
 200         double east_dist = horiz_dist * sin(heading);
 201
 202         //cout << distance << ' ' << horiz_dist << ' ' << vert_dist << ' ' << north_dist << ' ' << east_dist << '\n';
 203
 204         double delta_lat = asin(north_dist / (double)SG_EQUATORIAL_RADIUS_M);
 205         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.
 206         //cout << delta_lon*DCL_RADIANS_TO_DEGREES << ' ' << delta_lat*DCL_RADIANS_TO_DEGREES << '\n';
 207         lat += delta_lat;
 208         lon += delta_lon;
 209         elev += vert_dist;
 210         //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
 211
 212         //cout << setprecision(15) << DCL_DEGREES_TO_RADIANS * DCL_RADIANS_TO_DEGREES << '\n';
 213
 214         return SGGeod::fromRadM(lon, lat, elev);
 215 }
 216
 217 // Get a heading in degrees from one lat/lon to another.
 218 // This function assumes the world is spherical.  If geodetic accuracy is required use the functions is sg_geodesy instead!
 219 // Warning - at the moment we are not checking for identical points - currently it returns 0 in this instance.
 220 double GetHeadingFromTo(const SGGeod& A, const SGGeod& B) {
 221         double latA = A.getLatitudeRad();
 222         double lonA = A.getLongitudeRad();
 223         double latB = B.getLatitudeRad();
 224         double lonB = B.getLongitudeRad();
 225         double xdist = sin(lonB - lonA) * (double)SG_EQUATORIAL_RADIUS_M * cos((latA+latB)/2.0);
 226         double ydist = sin(latB - latA) * (double)SG_EQUATORIAL_RADIUS_M;
 227         double heading = atan2(xdist, ydist) * DCL_RADIANS_TO_DEGREES;
 228         return heading < 0.0 ? heading + 360 : heading;
 229 }
 230
 231 // Given a heading (in degrees), bound it from 0 -> 360
 232 void dclBoundHeading(double &hdg) {
 233         while(hdg < 0.0) {
 234                 hdg += 360.0;
 235         }
 236         while(hdg > 360.0) {
 237                 hdg -= 360.0;
 238         }
 239 }
 240
 241 // smallest difference between two angles in degrees
 242 // difference is negative if a1 > a2 and positive if a2 > a1
 243 double GetAngleDiff_deg( const double &a1, const double &a2) {
 244
 245   double a3 = a2 - a1;
 246   while (a3 < 180.0) a3 += 360.0;
 247   while (a3 > 180.0) a3 -= 360.0;
 248
 249   return a3;
 250 }
 251
 252 // Runway stuff
 253 // Given (lon/lat/elev) and an FGRunway struct, determine if the point lies on the runway
 254 bool OnRunway(const SGGeod& pt, const FGRunwayBase* rwy) {
 255         FGATCAlignedProjection ortho;
 256         SGGeod centre = SGGeod::fromDegM(rwy->longitude(), rwy->latitude(), 0); // We don't need the elev
 257         ortho.Init(centre, rwy->headingDeg());
 258
 259         SGVec3d xyc = ortho.ConvertToLocal(centre);
 260         SGVec3d xyp = ortho.ConvertToLocal(pt);
 261
 262         //cout << "Length offset = " << fabs(xyp.y() - xyc.y()) << '\n';
 263         //cout << "Width offset = " << fabs(xyp.x() - xyc.x()) << '\n';
 264
 265         if((fabs(xyp.y() - xyc.y()) < ((rwy->lengthFt()/2.0) + 5.0))
 266                 && (fabs(xyp.x() - xyc.x()) < (rwy->widthFt()/2.0))) {
 267                 return(true);
 268         }
 269
 270         return(false);
 271 }
 272