Changes and additions to utility functions to support voice ATC rendering and AI taxiing.
This commit is contained in:
daveluff
parent
2a1c00b8e4
commit
46011ab535
2 changed files with 185 additions and 129 deletions
|
|
@ -22,152 +22,199 @@
|
|||
#include <simgear/math/point3d.hxx>
|
||||
#include <simgear/constants.h>
|
||||
#include <plib/sg.h>
|
||||
#include <iomanip.h>
|
||||
//#include <iomanip.h>
|
||||
|
||||
#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<n.length(); ++i) {
|
||||
//cout << "n.substr(" << i << ",1 = " << n.substr(i,1) << endl;
|
||||
if(n.substr(i,1) == " ") {
|
||||
// do nothing
|
||||
} else if(n.substr(i,1) == ".") {
|
||||
str += pt;
|
||||
} else {
|
||||
str += nums[atoi((n.substr(i,1)).c_str())];
|
||||
}
|
||||
if(i != (n.length()-1)) { // ie. don't add a space at the end.
|
||||
str += " ";
|
||||
}
|
||||
}
|
||||
|
||||
string str = "";
|
||||
int index = n/10;
|
||||
str += nums[index];
|
||||
n -= (index * 10);
|
||||
str += "-";
|
||||
str += nums[n];
|
||||
return(str);
|
||||
return(str);
|
||||
}
|
||||
|
||||
// Convert a 2 digit rwy number to a spoken-style string
|
||||
string ConvertRwyNumToSpokenString(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?
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue