src/ATC/ATCutils.cxx

   1 // Utility functions for the ATC / AI system
   2
   3 #include <math.h>
   4 #include <simgear/math/point3d.hxx>
   5 #include <simgear/constants.h>
   6 #include <plib/sg.h>
   7
   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)
  13
  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;
  18
  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));
  21     z = sqrt(x*x + y*y);
  22
  23     return(z);
  24 }
  25
  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();
  32
  33     double horiz_dist = distance * cos(angle);
  34     double vert_dist = distance * sin(angle);
  35
  36     double north_dist = horiz_dist * cos(heading);
  37     double east_dist = horiz_dist * sin(heading);
  38
  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.
  41     elev += vert_dist;
  42
  43     return(Point3D(lon*SG_RADIANS_TO_DEGREES, lat*SG_RADIANS_TO_DEGREES, elev));
  44 }
  45
  46
  47 #if 0
  48 /* Determine location in runway coordinates */
  49
  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);
  53
  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;
  58
  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;
  64 #endif