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 <plib/sg.h>
  25 //#include <iomanip.h>
  26
  27 #include "ATCutils.hxx"
  28
  29 // Convert any number to spoken digits
  30 string ConvertNumToSpokenDigits(string n) {
  31         //cout << "n = " << n << endl;
  32         string nums[10] = {"zero", "one", "two", "three", "four", "five", "six", "seven", "eight", "nine"};
  33         string pt = "decimal";
  34         string str = "";
  35
  36         for(unsigned int i=0; i<n.length(); ++i) {
  37                 //cout << "n.substr(" << i << ",1 = " << n.substr(i,1) << endl;
  38                 if(n.substr(i,1) == " ") {
  39                         // do nothing
  40                 } else if(n.substr(i,1) == ".") {
  41                         str += pt;
  42                 } else {
  43                         str += nums[atoi((n.substr(i,1)).c_str())];
  44                 }
  45                 if(i != (n.length()-1)) {       // ie. don't add a space at the end.
  46                         str += " ";
  47                 }
  48         }
  49
  50         return(str);
  51 }
  52
  53 // Convert a 2 digit rwy number to a spoken-style string
  54 string ConvertRwyNumToSpokenString(int n) {
  55         string nums[10] = {"zero", "one", "two", "three", "four", "five", "six", "seven", "eight", "nine"};
  56         // Basic error/sanity checking
  57         while(n < 0) {
  58                 n += 36;
  59         }
  60         while(n > 36) {
  61                 n -= 36;
  62         }
  63         if(n == 0) {
  64                 n = 36; // Is this right?
  65         }
  66
  67         string str = "";
  68         int index = n/10;
  69         str += nums[index];
  70         n -= (index * 10);
  71         //str += "-";
  72         str += " ";             //Changed this for the benefit of the voice token parser - prefer the "-" in the visual output though.
  73         str += nums[n];
  74         return(str);
  75 }
  76
  77 // Return the phonetic letter of a letter represented as an integer 1->26
  78 string GetPhoneticIdent(int i) {
  79         // TODO - Check i is between 1 and 26 and wrap if necessary
  80         switch(i) {
  81         case 1 : return("alpha");
  82         case 2 : return("bravo");
  83         case 3 : return("charlie");
  84         case 4 : return("delta");
  85         case 5 : return("echo");
  86         case 6 : return("foxtrot");
  87         case 7 : return("golf");
  88         case 8 : return("hotel");
  89         case 9 : return("india");
  90         case 10 : return("juliet");
  91         case 11 : return("kilo");
  92         case 12 : return("lima");
  93         case 13 : return("mike");
  94         case 14 : return("november");
  95         case 15 : return("oscar");
  96         case 16 : return("papa");
  97         case 17 : return("quebec");
  98         case 18 : return("romeo");
  99         case 19 : return("sierra");
 100         case 20 : return("tango");
 101         case 21 : return("uniform");
 102         case 22 : return("victor");
 103         case 23 : return("whiskey");
 104         case 24 : return("x-ray");
 105         case 25 : return("yankee");
 106         case 26 : return("zulu");
 107         }
 108         // We shouldn't get here
 109         return("Error");
 110 }
 111
 112 // Given two positions (lat & lon in degrees), get the HORIZONTAL separation (in meters)
 113 double dclGetHorizontalSeparation(Point3D pos1, Point3D pos2) {
 114         double x;       //East-West separation
 115         double y;       //North-South separation
 116         double z;       //Horizontal separation - z = sqrt(x^2 + y^2)
 117
 118         double lat1 = pos1.lat() * SG_DEGREES_TO_RADIANS;
 119         double lon1 = pos1.lon() * SG_DEGREES_TO_RADIANS;
 120         double lat2 = pos2.lat() * SG_DEGREES_TO_RADIANS;
 121         double lon2 = pos2.lon() * SG_DEGREES_TO_RADIANS;
 122
 123         y = sin(fabs(lat1 - lat2)) * SG_EQUATORIAL_RADIUS_M;
 124         x = sin(fabs(lon1 - lon2)) * SG_EQUATORIAL_RADIUS_M * (cos((lat1 + lat2) / 2.0));
 125         z = sqrt(x*x + y*y);
 126
 127         return(z);
 128 }
 129
 130 // Given a point and a line, get the HORIZONTAL shortest distance from the point to a point on the line.
 131 // Expects to be fed orthogonal co-ordinates, NOT lat & lon !
 132 // The units of the separation will be those of the input.
 133 double dclGetLinePointSeparation(double px, double py, double x1, double y1, double x2, double y2) {
 134         double vecx = x2-x1;
 135         double vecy = y2-y1;
 136         double magline = sqrt(vecx*vecx + vecy*vecy);
 137         double u = ((px-x1)*(x2-x1) + (py-y1)*(y2-y1)) / (magline * magline);
 138         double x0 = x1 + u*(x2-x1);
 139         double y0 = y1 + u*(y2-y1);
 140         vecx = px - x0;
 141         vecy = py - y0;
 142         double d = sqrt(vecx*vecx + vecy*vecy);
 143         if(d < 0) {
 144                 d *= -1;
 145         }
 146         return(d);
 147 }
 148
 149 // Given a position (lat/lon/elev), heading and vertical angle (degrees), and distance (meters), calculate the new position.
 150 // This function assumes the world is spherical.  If geodetic accuracy is required use the functions is sg_geodesy instead!
 151 // Assumes that the ground is not hit!!!  Expects heading and angle in degrees, distance in meters.
 152 Point3D dclUpdatePosition(Point3D pos, double heading, double angle, double distance) {
 153         //cout << setprecision(10) << pos.lon() << ' ' << pos.lat() << '\n';
 154         heading *= DCL_DEGREES_TO_RADIANS;
 155         angle *= DCL_DEGREES_TO_RADIANS;
 156         double lat = pos.lat() * DCL_DEGREES_TO_RADIANS;
 157         double lon = pos.lon() * DCL_DEGREES_TO_RADIANS;
 158         double elev = pos.elev();
 159         //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
 160
 161         double horiz_dist = distance * cos(angle);
 162         double vert_dist = distance * sin(angle);
 163
 164         double north_dist = horiz_dist * cos(heading);
 165         double east_dist = horiz_dist * sin(heading);
 166
 167         //cout << distance << ' ' << horiz_dist << ' ' << vert_dist << ' ' << north_dist << ' ' << east_dist << '\n';
 168
 169         double delta_lat = asin(north_dist / (double)SG_EQUATORIAL_RADIUS_M);
 170         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.
 171         //cout << delta_lon*DCL_RADIANS_TO_DEGREES << ' ' << delta_lat*DCL_RADIANS_TO_DEGREES << '\n';
 172         lat += delta_lat;
 173         lon += delta_lon;
 174         elev += vert_dist;
 175         //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
 176
 177         //cout << setprecision(15) << DCL_DEGREES_TO_RADIANS * DCL_RADIANS_TO_DEGREES << '\n';
 178
 179         return(Point3D(lon*DCL_RADIANS_TO_DEGREES, lat*DCL_RADIANS_TO_DEGREES, elev));
 180 }
 181
 182 // Get a heading in degrees from one lat/lon to another.
 183 // This function assumes the world is spherical.  If geodetic accuracy is required use the functions is sg_geodesy instead!
 184 // Warning - at the moment we are not checking for identical points - currently it returns 90 in this instance.
 185 double GetHeadingFromTo(Point3D A, Point3D B) {
 186         double latA = A.lat() * DCL_DEGREES_TO_RADIANS;
 187         double lonA = A.lon() * DCL_DEGREES_TO_RADIANS;
 188         double latB = B.lat() * DCL_DEGREES_TO_RADIANS;
 189         double lonB = B.lon() * DCL_DEGREES_TO_RADIANS;
 190         double xdist = sin(lonB - lonA) * (double)SG_EQUATORIAL_RADIUS_M * cos((latA+latB)/2.0);
 191         double ydist = sin(latB - latA) * (double)SG_EQUATORIAL_RADIUS_M;
 192
 193         if(xdist >= 0) {
 194                 if(ydist > 0) {
 195                         return(atan(xdist/ydist) * DCL_RADIANS_TO_DEGREES);
 196                 } else if (ydist == 0) {
 197                         return(90.0);
 198                 } else {
 199                         return(180.0 - atan(xdist/fabs(ydist)) * DCL_RADIANS_TO_DEGREES);
 200                 }
 201         } else {
 202                 if(ydist > 0) {
 203                         return(360.0 - atan(fabs(xdist)/ydist) * DCL_RADIANS_TO_DEGREES);
 204                 } else if (ydist == 0) {
 205                         return(270.0);
 206                 } else {
 207                         return(180.0 + atan(xdist/ydist) * DCL_RADIANS_TO_DEGREES);
 208                 }
 209         }
 210 }
 211
 212 // Given a heading (in degrees), bound it from 0 -> 360
 213 void dclBoundHeading(double &hdg) {
 214         while(hdg < 0.0) {
 215                 hdg += 360.0;
 216         }
 217         while(hdg > 360.0) {
 218                 hdg -= 360.0;
 219         }
 220 }