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 a 2 digit rwy number to a spoken-style string
  30 string convertNumToSpokenString(int n) {
  31     string nums[10] = {"zero", "one", "two", "three", "four", "five", "six", "seven", "eight", "nine"};
  32     // Basic error/sanity checking
  33     while(n < 0) {
  34         n += 36;
  35     }
  36     while(n > 36) {
  37         n -= 36;
  38     }
  39     if(n == 0) {
  40         n = 36; // Is this right?
  41     }
  42
  43     string str = "";
  44     int index = n/10;
  45     str += nums[index];
  46     n -= (index * 10);
  47     str += "-";
  48     str += nums[n];
  49     return(str);
  50 }
  51
  52 // Return the phonetic letter of a letter represented as an integer 1->26
  53 string GetPhoneticIdent(int i) {
  54 // TODO - Check i is between 1 and 26 and wrap if necessary
  55     switch(i) {
  56     case 1 : return("Alpha");
  57     case 2 : return("Bravo");
  58     case 3 : return("Charlie");
  59     case 4 : return("Delta");
  60     case 5 : return("Echo");
  61     case 6 : return("Foxtrot");
  62     case 7 : return("Golf");
  63     case 8 : return("Hotel");
  64     case 9 : return("Indigo");
  65     case 10 : return("Juliet");
  66     case 11 : return("Kilo");
  67     case 12 : return("Lima");
  68     case 13 : return("Mike");
  69     case 14 : return("November");
  70     case 15 : return("Oscar");
  71     case 16 : return("Papa");
  72     case 17 : return("Quebec");
  73     case 18 : return("Romeo");
  74     case 19 : return("Sierra");
  75     case 20 : return("Tango");
  76     case 21 : return("Uniform");
  77     case 22 : return("Victor");
  78     case 23 : return("Whiskey");
  79     case 24 : return("X-ray");
  80     case 25 : return("Yankee");
  81     case 26 : return("Zulu");
  82     }
  83     // We shouldn't get here
  84     return("Error");
  85 }
  86
  87 // Given two positions, get the HORIZONTAL separation (in meters)
  88 double dclGetHorizontalSeparation(Point3D pos1, Point3D pos2) {
  89     double x;   //East-West separation
  90     double y;   //North-South separation
  91     double z;   //Horizontal separation - z = sqrt(x^2 + y^2)
  92
  93     double lat1 = pos1.lat() * SG_DEGREES_TO_RADIANS;
  94     double lon1 = pos1.lon() * SG_DEGREES_TO_RADIANS;
  95     double lat2 = pos2.lat() * SG_DEGREES_TO_RADIANS;
  96     double lon2 = pos2.lon() * SG_DEGREES_TO_RADIANS;
  97
  98     y = sin(fabs(lat1 - lat2)) * SG_EQUATORIAL_RADIUS_M;
  99     x = sin(fabs(lon1 - lon2)) * SG_EQUATORIAL_RADIUS_M * (cos((lat1 + lat2) / 2.0));
 100     z = sqrt(x*x + y*y);
 101
 102     return(z);
 103 }
 104
 105 // Given a point and a line, get the HORIZONTAL shortest distance from the point to a point on the line.
 106 // Expects to be fed orthogonal co-ordinates, NOT lat & lon !
 107 double dclGetLinePointSeparation(double px, double py, double x1, double y1, double x2, double y2) {
 108     double vecx = x2-x1;
 109     double vecy = y2-y1;
 110     double magline = sqrt(vecx*vecx + vecy*vecy);
 111     double u = ((px-x1)*(x2-x1) + (py-y1)*(y2-y1)) / (magline * magline);
 112     double x0 = x1 + u*(x2-x1);
 113     double y0 = y1 + u*(y2-y1);
 114     vecx = px - x0;
 115     vecy = py - y0;
 116     double d = sqrt(vecx*vecx + vecy*vecy);
 117     if(d < 0) {
 118         d *= -1;
 119     }
 120     return(d);
 121 }
 122
 123 // Given a position (lat/lon/elev), heading, vertical angle, and distance, calculate the new position.
 124 // Assumes that the ground is not hit!!!  Expects heading and angle in degrees, distance in meters.
 125 Point3D dclUpdatePosition(Point3D pos, double heading, double angle, double distance) {
 126     //cout << setprecision(10) << pos.lon() << ' ' << pos.lat() << '\n';
 127     heading *= DCL_DEGREES_TO_RADIANS;
 128     angle *= DCL_DEGREES_TO_RADIANS;
 129     double lat = pos.lat() * DCL_DEGREES_TO_RADIANS;
 130     double lon = pos.lon() * DCL_DEGREES_TO_RADIANS;
 131     double elev = pos.elev();
 132     //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
 133
 134     double horiz_dist = distance * cos(angle);
 135     double vert_dist = distance * sin(angle);
 136
 137     double north_dist = horiz_dist * cos(heading);
 138     double east_dist = horiz_dist * sin(heading);
 139
 140     //cout << distance << ' ' << horiz_dist << ' ' << vert_dist << ' ' << north_dist << ' ' << east_dist << '\n';
 141
 142     double delta_lat = asin(north_dist / (double)SG_EQUATORIAL_RADIUS_M);
 143     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.
 144     //cout << delta_lon*DCL_RADIANS_TO_DEGREES << ' ' << delta_lat*DCL_RADIANS_TO_DEGREES << '\n';
 145     lat += delta_lat;
 146     lon += delta_lon;
 147     elev += vert_dist;
 148     //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n';
 149
 150     //cout << setprecision(15) << DCL_DEGREES_TO_RADIANS * DCL_RADIANS_TO_DEGREES << '\n';
 151
 152     return(Point3D(lon*DCL_RADIANS_TO_DEGREES, lat*DCL_RADIANS_TO_DEGREES, elev));
 153 }
 154
 155
 156 #if 0
 157 /* Determine location in runway coordinates */
 158
 159         Radius_to_rwy = Sea_level_radius + Runway_altitude;
 160         cos_rwy_hdg = cos(Runway_heading*DEG_TO_RAD);
 161         sin_rwy_hdg = sin(Runway_heading*DEG_TO_RAD);
 162
 163         D_cg_north_of_rwy = Radius_to_rwy*(Latitude - Runway_latitude);
 164         D_cg_east_of_rwy = Radius_to_rwy*cos(Runway_latitude)
 165                 *(Longitude - Runway_longitude);
 166         D_cg_above_rwy  = Radius_to_vehicle - Radius_to_rwy;
 167
 168         X_cg_rwy = D_cg_north_of_rwy*cos_rwy_hdg
 169           + D_cg_east_of_rwy*sin_rwy_hdg;
 170         Y_cg_rwy =-D_cg_north_of_rwy*sin_rwy_hdg
 171           + D_cg_east_of_rwy*cos_rwy_hdg;
 172         H_cg_rwy = D_cg_above_rwy;
 173 #endif