diff --git a/src/ATC/ATCutils.cxx b/src/ATC/ATCutils.cxx index e4cfc3e39..0c98fa73b 100644 --- a/src/ATC/ATCutils.cxx +++ b/src/ATC/ATCutils.cxx @@ -22,152 +22,199 @@ #include #include #include -#include +//#include #include "ATCutils.hxx" -// Convert a 2 digit rwy number to a spoken-style string -string convertNumToSpokenString(int n) { - string nums[10] = {"zero", "one", "two", "three", "four", "five", "six", "seven", "eight", "nine"}; - // Basic error/sanity checking - while(n < 0) { - n += 36; - } - while(n > 36) { - n -= 36; - } - if(n == 0) { - n = 36; // Is this right? - } +// Convert any number to spoken digits +string ConvertNumToSpokenDigits(string n) { + //cout << "n = " << n << endl; + string nums[10] = {"zero", "one", "two", "three", "four", "five", "six", "seven", "eight", "nine"}; + string pt = "decimal"; + string str = ""; + + for(unsigned int i=0; i 36) { + n -= 36; + } + if(n == 0) { + n = 36; // Is this right? + } + + string str = ""; + int index = n/10; + str += nums[index]; + n -= (index * 10); + //str += "-"; + str += " "; //Changed this for the benefit of the voice token parser - prefer the "-" in the visual output though. + str += nums[n]; + return(str); } // Return the phonetic letter of a letter represented as an integer 1->26 string GetPhoneticIdent(int i) { -// TODO - Check i is between 1 and 26 and wrap if necessary - switch(i) { - case 1 : return("Alpha"); - case 2 : return("Bravo"); - case 3 : return("Charlie"); - case 4 : return("Delta"); - case 5 : return("Echo"); - case 6 : return("Foxtrot"); - case 7 : return("Golf"); - case 8 : return("Hotel"); - case 9 : return("Indigo"); - case 10 : return("Juliet"); - case 11 : return("Kilo"); - case 12 : return("Lima"); - case 13 : return("Mike"); - case 14 : return("November"); - case 15 : return("Oscar"); - case 16 : return("Papa"); - case 17 : return("Quebec"); - case 18 : return("Romeo"); - case 19 : return("Sierra"); - case 20 : return("Tango"); - case 21 : return("Uniform"); - case 22 : return("Victor"); - case 23 : return("Whiskey"); - case 24 : return("X-ray"); - case 25 : return("Yankee"); - case 26 : return("Zulu"); - } - // We shouldn't get here - return("Error"); + // TODO - Check i is between 1 and 26 and wrap if necessary + switch(i) { + case 1 : return("alpha"); + case 2 : return("bravo"); + case 3 : return("charlie"); + case 4 : return("delta"); + case 5 : return("echo"); + case 6 : return("foxtrot"); + case 7 : return("golf"); + case 8 : return("hotel"); + case 9 : return("india"); + case 10 : return("juliet"); + case 11 : return("kilo"); + case 12 : return("lima"); + case 13 : return("mike"); + case 14 : return("november"); + case 15 : return("oscar"); + case 16 : return("papa"); + case 17 : return("quebec"); + case 18 : return("romeo"); + case 19 : return("sierra"); + case 20 : return("tango"); + case 21 : return("uniform"); + case 22 : return("victor"); + case 23 : return("whiskey"); + case 24 : return("x-ray"); + case 25 : return("yankee"); + case 26 : return("zulu"); + } + // We shouldn't get here + return("Error"); } -// Given two positions, get the HORIZONTAL separation (in meters) +// Given two positions (lat & lon in degrees), get the HORIZONTAL separation (in meters) double dclGetHorizontalSeparation(Point3D pos1, Point3D pos2) { - double x; //East-West separation - double y; //North-South separation - double z; //Horizontal separation - z = sqrt(x^2 + y^2) - - double lat1 = pos1.lat() * SG_DEGREES_TO_RADIANS; - double lon1 = pos1.lon() * SG_DEGREES_TO_RADIANS; - double lat2 = pos2.lat() * SG_DEGREES_TO_RADIANS; - double lon2 = pos2.lon() * SG_DEGREES_TO_RADIANS; - - y = sin(fabs(lat1 - lat2)) * SG_EQUATORIAL_RADIUS_M; - x = sin(fabs(lon1 - lon2)) * SG_EQUATORIAL_RADIUS_M * (cos((lat1 + lat2) / 2.0)); - z = sqrt(x*x + y*y); - - return(z); + double x; //East-West separation + double y; //North-South separation + double z; //Horizontal separation - z = sqrt(x^2 + y^2) + + double lat1 = pos1.lat() * SG_DEGREES_TO_RADIANS; + double lon1 = pos1.lon() * SG_DEGREES_TO_RADIANS; + double lat2 = pos2.lat() * SG_DEGREES_TO_RADIANS; + double lon2 = pos2.lon() * SG_DEGREES_TO_RADIANS; + + y = sin(fabs(lat1 - lat2)) * SG_EQUATORIAL_RADIUS_M; + x = sin(fabs(lon1 - lon2)) * SG_EQUATORIAL_RADIUS_M * (cos((lat1 + lat2) / 2.0)); + z = sqrt(x*x + y*y); + + return(z); } // Given a point and a line, get the HORIZONTAL shortest distance from the point to a point on the line. // Expects to be fed orthogonal co-ordinates, NOT lat & lon ! +// The units of the separation will be those of the input. double dclGetLinePointSeparation(double px, double py, double x1, double y1, double x2, double y2) { - double vecx = x2-x1; - double vecy = y2-y1; - double magline = sqrt(vecx*vecx + vecy*vecy); - double u = ((px-x1)*(x2-x1) + (py-y1)*(y2-y1)) / (magline * magline); - double x0 = x1 + u*(x2-x1); - double y0 = y1 + u*(y2-y1); - vecx = px - x0; - vecy = py - y0; - double d = sqrt(vecx*vecx + vecy*vecy); - if(d < 0) { - d *= -1; - } - return(d); + double vecx = x2-x1; + double vecy = y2-y1; + double magline = sqrt(vecx*vecx + vecy*vecy); + double u = ((px-x1)*(x2-x1) + (py-y1)*(y2-y1)) / (magline * magline); + double x0 = x1 + u*(x2-x1); + double y0 = y1 + u*(y2-y1); + vecx = px - x0; + vecy = py - y0; + double d = sqrt(vecx*vecx + vecy*vecy); + if(d < 0) { + d *= -1; + } + return(d); } -// Given a position (lat/lon/elev), heading, vertical angle, and distance, calculate the new position. -// Assumes that the ground is not hit!!! Expects heading and angle in degrees, distance in meters. +// Given a position (lat/lon/elev), heading and vertical angle (degrees), and distance (meters), calculate the new position. +// This function assumes the world is spherical. If geodetic accuracy is required use the functions is sg_geodesy instead! +// Assumes that the ground is not hit!!! Expects heading and angle in degrees, distance in meters. Point3D dclUpdatePosition(Point3D pos, double heading, double angle, double distance) { - //cout << setprecision(10) << pos.lon() << ' ' << pos.lat() << '\n'; - heading *= DCL_DEGREES_TO_RADIANS; - angle *= DCL_DEGREES_TO_RADIANS; - double lat = pos.lat() * DCL_DEGREES_TO_RADIANS; - double lon = pos.lon() * DCL_DEGREES_TO_RADIANS; - double elev = pos.elev(); - //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n'; - - double horiz_dist = distance * cos(angle); - double vert_dist = distance * sin(angle); - - double north_dist = horiz_dist * cos(heading); - double east_dist = horiz_dist * sin(heading); - - //cout << distance << ' ' << horiz_dist << ' ' << vert_dist << ' ' << north_dist << ' ' << east_dist << '\n'; - - double delta_lat = asin(north_dist / (double)SG_EQUATORIAL_RADIUS_M); - 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. - //cout << delta_lon*DCL_RADIANS_TO_DEGREES << ' ' << delta_lat*DCL_RADIANS_TO_DEGREES << '\n'; - lat += delta_lat; - lon += delta_lon; - elev += vert_dist; - //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n'; - - //cout << setprecision(15) << DCL_DEGREES_TO_RADIANS * DCL_RADIANS_TO_DEGREES << '\n'; - - return(Point3D(lon*DCL_RADIANS_TO_DEGREES, lat*DCL_RADIANS_TO_DEGREES, elev)); + //cout << setprecision(10) << pos.lon() << ' ' << pos.lat() << '\n'; + heading *= DCL_DEGREES_TO_RADIANS; + angle *= DCL_DEGREES_TO_RADIANS; + double lat = pos.lat() * DCL_DEGREES_TO_RADIANS; + double lon = pos.lon() * DCL_DEGREES_TO_RADIANS; + double elev = pos.elev(); + //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n'; + + double horiz_dist = distance * cos(angle); + double vert_dist = distance * sin(angle); + + double north_dist = horiz_dist * cos(heading); + double east_dist = horiz_dist * sin(heading); + + //cout << distance << ' ' << horiz_dist << ' ' << vert_dist << ' ' << north_dist << ' ' << east_dist << '\n'; + + double delta_lat = asin(north_dist / (double)SG_EQUATORIAL_RADIUS_M); + 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. + //cout << delta_lon*DCL_RADIANS_TO_DEGREES << ' ' << delta_lat*DCL_RADIANS_TO_DEGREES << '\n'; + lat += delta_lat; + lon += delta_lon; + elev += vert_dist; + //cout << setprecision(10) << lon*DCL_RADIANS_TO_DEGREES << ' ' << lat*DCL_RADIANS_TO_DEGREES << '\n'; + + //cout << setprecision(15) << DCL_DEGREES_TO_RADIANS * DCL_RADIANS_TO_DEGREES << '\n'; + + return(Point3D(lon*DCL_RADIANS_TO_DEGREES, lat*DCL_RADIANS_TO_DEGREES, elev)); } - -#if 0 -/* Determine location in runway coordinates */ +// Get a heading in degrees from one lat/lon to another. +// This function assumes the world is spherical. If geodetic accuracy is required use the functions is sg_geodesy instead! +// Warning - at the moment we are not checking for identical points - currently it returns 90 in this instance. +double GetHeadingFromTo(Point3D A, Point3D B) { + double latA = A.lat() * DCL_DEGREES_TO_RADIANS; + double lonA = A.lon() * DCL_DEGREES_TO_RADIANS; + double latB = B.lat() * DCL_DEGREES_TO_RADIANS; + double lonB = B.lon() * DCL_DEGREES_TO_RADIANS; + double xdist = sin(lonB - lonA) * (double)SG_EQUATORIAL_RADIUS_M * cos((latA+latB)/2.0); + double ydist = sin(latB - latA) * (double)SG_EQUATORIAL_RADIUS_M; + + if(xdist >= 0) { + if(ydist > 0) { + return(atan(xdist/ydist) * DCL_RADIANS_TO_DEGREES); + } else if (ydist == 0) { + return(90.0); + } else { + return(180.0 - atan(xdist/fabs(ydist)) * DCL_RADIANS_TO_DEGREES); + } + } else { + if(ydist > 0) { + return(360.0 - atan(fabs(xdist)/ydist) * DCL_RADIANS_TO_DEGREES); + } else if (ydist == 0) { + return(270.0); + } else { + return(180.0 + atan(xdist/ydist) * DCL_RADIANS_TO_DEGREES); + } + } +} - Radius_to_rwy = Sea_level_radius + Runway_altitude; - cos_rwy_hdg = cos(Runway_heading*DEG_TO_RAD); - sin_rwy_hdg = sin(Runway_heading*DEG_TO_RAD); - - D_cg_north_of_rwy = Radius_to_rwy*(Latitude - Runway_latitude); - D_cg_east_of_rwy = Radius_to_rwy*cos(Runway_latitude) - *(Longitude - Runway_longitude); - D_cg_above_rwy = Radius_to_vehicle - Radius_to_rwy; - - X_cg_rwy = D_cg_north_of_rwy*cos_rwy_hdg - + D_cg_east_of_rwy*sin_rwy_hdg; - Y_cg_rwy =-D_cg_north_of_rwy*sin_rwy_hdg - + D_cg_east_of_rwy*cos_rwy_hdg; - H_cg_rwy = D_cg_above_rwy; -#endif +// Given a heading (in degrees), bound it from 0 -> 360 +void dclBoundHeading(double &hdg) { + while(hdg < 0.0) { + hdg += 360.0; + } + while(hdg > 360.0) { + hdg -= 360.0; + } +} diff --git a/src/ATC/ATCutils.hxx b/src/ATC/ATCutils.hxx index 6576bf1ac..7655ec85c 100644 --- a/src/ATC/ATCutils.hxx +++ b/src/ATC/ATCutils.hxx @@ -34,8 +34,11 @@ SG_USING_STD(string); * ********************************/ +// Convert any number to spoken digits +string ConvertNumToSpokenDigits(string n); + // Convert a 2 digit rwy number to a spoken-style string -string convertNumToSpokenString(int n); +string ConvertRwyNumToSpokenString(int n); // Return the phonetic letter of a letter represented as an integer 1->26 string GetPhoneticIdent(int i); @@ -47,7 +50,7 @@ string GetPhoneticIdent(int i); * ********************************/ -// Given two positions, get the HORIZONTAL separation +// Given two positions, get the HORIZONTAL separation (in meters) double dclGetHorizontalSeparation(Point3D pos1, Point3D pos2); // Given a point and a line, get the HORIZONTAL shortest distance from the point to a point on the line. @@ -57,3 +60,9 @@ double dclGetLinePointSeparation(double px, double py, double x1, double y1, dou // Given a position (lat/lon/elev), heading, vertical angle, and distance, calculate the new position. // Assumes that the ground is not hit!!! Expects heading and angle in degrees, distance in meters. Point3D dclUpdatePosition(Point3D pos, double heading, double angle, double distance); + +// Get a heading from one lat/lon to another (in degrees) +double GetHeadingFromTo(Point3D A, Point3D B); + +// Given a heading (in degrees), bound it from 0 -> 360 +void dclBoundHeading(double &hdg);