1 // Utility functions for the ATC / AI system
4 #include <simgear/math/point3d.hxx>
5 #include <simgear/constants.h>
8 // Given two positions, get the HORIZONTAL separation (in meters)
9 double dclGetHorizontalSeparation(Point3D pos1, Point3D pos2) {
10 double x; //East-West separation
11 double y; //North-South separation
12 double z; //Horizontal separation - z = sqrt(x^2 + y^2)
14 double lat1 = pos1.lat() * SG_DEGREES_TO_RADIANS;
15 double lon1 = pos1.lon() * SG_DEGREES_TO_RADIANS;
16 double lat2 = pos2.lat() * SG_DEGREES_TO_RADIANS;
17 double lon2 = pos2.lon() * SG_DEGREES_TO_RADIANS;
19 y = sin(fabs(lat1 - lat2)) * SG_EQUATORIAL_RADIUS_M;
20 x = sin(fabs(lon1 - lon2)) * SG_EQUATORIAL_RADIUS_M * (cos((lat1 + lat2) / 2.0));
26 // Given a position (lat/lon/elev), heading, vertical angle, and distance, calculate the new position.
27 // Assumes that the ground is not hit!!! Expects heading and angle in degrees, distance in meters.
28 Point3D dclUpdatePosition(Point3D pos, double heading, double angle, double distance) {
29 double lat = pos.lat() * SG_DEGREES_TO_RADIANS;
30 double lon = pos.lon() * SG_DEGREES_TO_RADIANS;
31 double elev = pos.elev();
33 double horiz_dist = distance * cos(angle);
34 double vert_dist = distance * sin(angle);
36 double north_dist = horiz_dist * cos(heading);
37 double east_dist = horiz_dist * sin(heading);
39 lat += asin(north_dist / SG_EQUATORIAL_RADIUS_M);
40 lon += asin(east_dist / 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.
43 return(Point3D(lon*SG_RADIANS_TO_DEGREES, lat*SG_RADIANS_TO_DEGREES, elev));
48 /* Determine location in runway coordinates */
50 Radius_to_rwy = Sea_level_radius + Runway_altitude;
51 cos_rwy_hdg = cos(Runway_heading*DEG_TO_RAD);
52 sin_rwy_hdg = sin(Runway_heading*DEG_TO_RAD);
54 D_cg_north_of_rwy = Radius_to_rwy*(Latitude - Runway_latitude);
55 D_cg_east_of_rwy = Radius_to_rwy*cos(Runway_latitude)
56 *(Longitude - Runway_longitude);
57 D_cg_above_rwy = Radius_to_vehicle - Radius_to_rwy;
59 X_cg_rwy = D_cg_north_of_rwy*cos_rwy_hdg
60 + D_cg_east_of_rwy*sin_rwy_hdg;
61 Y_cg_rwy =-D_cg_north_of_rwy*sin_rwy_hdg
62 + D_cg_east_of_rwy*cos_rwy_hdg;
63 H_cg_rwy = D_cg_above_rwy;