Issue 1090: Adapt the mk-viii instrument code to the new internal runway scheme.

Clean up some unneeded functions as well and make use of standard simgear functions where possible.
This whole thing could need a rewrite...

src/Instrumentation/mk_viii.cxx

@@ -66,8 +66,13 @@
 #include <simgear/constants.h>
 #include <simgear/sg_inlines.h>
 #include <simgear/debug/logstream.hxx>
-#include <simgear/math/sg_geodesy.hxx>
+#include <simgear/math/SGMathFwd.hxx>
+#include <simgear/math/SGLimits.hxx>
+#include <simgear/math/SGGeometryFwd.hxx>
+#include <simgear/math/SGGeodesy.hxx>
 #include <simgear/math/sg_random.h>
+#include <simgear/math/SGLineSegment.hxx>
+#include <simgear/math/SGIntersect.hxx>
 #include <simgear/misc/sg_path.hxx>
 #include <simgear/sound/soundmgr_openal.hxx>
 #include <simgear/sound/sample_group.hxx>
@@ -136,24 +141,6 @@ modify_amplitude (double amplitude, double dB)
   return amplitude * pow(10.0, dB / 20.0);
 }
 
-static double
-heading_add (double h1, double h2)
-{
-  double result = h1 + h2;
-  if (result >= 360)
-    result -= 360;
-  return result;
-}
-
-static double
-heading_substract (double h1, double h2)
-{
-  double result = h1 - h2;
-  if (result < 0)
-    result += 360;
-  return result;
-}
-
 static double
 get_heading_difference (double h1, double h2)
 {
@@ -167,12 +154,6 @@ get_heading_difference (double h1, double h2)
   return fabs(diff);
 }
 
-static double
-get_reciprocal_heading (double h)
-{
-  return heading_add(h, 180);
-}
-
 ///////////////////////////////////////////////////////////////////////////////
 // PropertiesHandler //////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////
@@ -4068,9 +4049,7 @@ MK_VIII::Mode6Handler::test_runway (const FGRunway *_runway)
     SGGeod::fromDeg(mk_data(gps_longitude).get(), mk_data(gps_latitude).get()));
   
   // get distance to threshold
-  double distance, az1, az2;
+  return SGGeodesy::distanceNm(pos, _runway->threshold()) <= 5;
-  SGGeodesy::inverse(pos, _runway->threshold(), az1, az2, distance);
-  return distance * SG_METER_TO_NM <= 5;
 }
 
 bool
@@ -4270,60 +4249,6 @@ MK_VIII::Mode6Handler::update ()
 // TCFHandler /////////////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////
 
-// Gets the difference between the azimuth from @from_lat,@from_lon to
-// @to_lat,@to_lon, and @to_heading, in degrees.
-double
-MK_VIII::TCFHandler::get_azimuth_difference (double from_lat,
-					     double from_lon,
-					     double to_lat,
-					     double to_lon,
-					     double to_heading)
-{
-  double az1, az2, distance;
-  geo_inverse_wgs_84(0, from_lat, from_lon, to_lat, to_lon, &az1, &az2, &distance);
-  return get_heading_difference(az1, to_heading);
-}
-
-// Gets the difference between the azimuth from the current GPS
-// position to the center of @_runway, and the heading of @_runway, in
-// degrees.
-double
-MK_VIII::TCFHandler::get_azimuth_difference (const FGRunway *_runway)
-{
-  return get_azimuth_difference(mk_data(gps_latitude).get(),
-				mk_data(gps_longitude).get(),
-				_runway->latitude(),
-				_runway->longitude(),
-				_runway->headingDeg());
-}
-
-// Selects the most likely intended destination runway of @airport,
-// and returns it in @_runway. For each runway, the difference between
-// the azimuth from the current GPS position to the center of the
-// runway and its heading is computed. The runway having the smallest
-// difference wins.
-//
-// This selection algorithm is not specified in [SPEC], but
-// http://www.egpws.com/general_information/description/runway_select.htm
-// talks about automatic runway selection.
-FGRunway*
-MK_VIII::TCFHandler::select_runway (const FGAirport *airport)
-{
-  FGRunway* _runway = 0;
-  double min_diff = 360;
-  
-  for (unsigned int r=0; r<airport->numRunways(); ++r) {
-    FGRunway* rwy(airport->getRunwayByIndex(r));
-    double diff = get_azimuth_difference(rwy);
-    if (diff < min_diff)
-	  {
-      min_diff = diff;
-      _runway = rwy;
-    }
-  } // of airport runways iteration
-  return _runway;
-}
-
 bool MK_VIII::TCFHandler::AirportFilter::passAirport(FGAirport* aApt) const
 {
   return aApt->hasHardRunwayOfLengthFt(mk->conf.runway_database);
@@ -4343,109 +4268,43 @@ MK_VIII::TCFHandler::update_runway ()
   // large airports, which may have a runway located far away from
   // the airport's reference point.
   AirportFilter filter(mk);
-  FGAirport* apt = FGAirport::findClosest(
+  SGGeod apos = SGGeod::fromDeg(mk_data(gps_longitude).get(), mk_data(gps_latitude).get());
-    SGGeod::fromDeg(mk_data(gps_longitude).get(), mk_data(gps_latitude).get()),
+  FGAirport* apt = FGAirport::findClosest(apos, 30.0, &filter);
-    30.0, &filter);
       
   if (!apt) return;
   
-	  FGRunway* _runway = select_runway(apt);
+	  FGRunway* _runway = apt->findBestRunwayForPos(apos).get();
     
   if (!_runway) return;
 
 	  has_runway = true;
 
-	  runway.center.latitude = _runway->latitude();
+	  runway.center = _runway->pointOnCenterline(_runway->lengthM() * 0.5);
-	  runway.center.longitude = _runway->longitude();
 
 	  runway.elevation = apt->elevation();
 
+          runway.half_width_m = _runway->widthM() * 0.5;
 	  double half_length_m = _runway->lengthM() * 0.5;
 	  runway.half_length = half_length_m * SG_METER_TO_NM;
 
-	  //        b3 ________________ b0
+	  //           ________________
 	  //          |                |
 	  //    h1>>> |  e1<<<<<<<<e0  | <<<h0
 	  //          |________________|
-	  //        b2                  b1
 
 	  // get heading to runway threshold (h0) and end (h1)
 	  runway.edges[0].heading = _runway->headingDeg();
-	  runway.edges[1].heading = get_reciprocal_heading(_runway->headingDeg());
+	  runway.edges[1].heading = _runway->reciprocalRunway()->headingDeg();
-
-	  double az;
 
 	  // get position of runway threshold (e0)
-	  geo_direct_wgs_84(0,
+          runway.edges[0].position = _runway->begin();
-			    runway.center.latitude,
-			    runway.center.longitude,
-			    runway.edges[1].heading,
-			    half_length_m,
-			    &runway.edges[0].position.latitude,
-			    &runway.edges[0].position.longitude,
-			    &az);
 
 	  // get position of runway end (e1)
-	  geo_direct_wgs_84(0,
+          runway.edges[1].position = _runway->end();
-			    runway.center.latitude,
-			    runway.center.longitude,
-			    runway.edges[0].heading,
-			    half_length_m,
-			    &runway.edges[1].position.latitude,
-			    &runway.edges[1].position.longitude,
-			    &az);
 
-	  double half_width_m = _runway->widthM() * 0.5;
+          // get cartesian coordinates of both runway ends
-
+          runway.bias_points[0] = _runway->cart();
-	  // get position of threshold bias area edges (b0 and b1)
+          runway.bias_points[1] = _runway->reciprocalRunway()->cart();
-	  get_bias_area_edges(&runway.edges[0].position,
-			      runway.edges[1].heading,
-			      half_width_m,
-			      &runway.bias_area[0],
-			      &runway.bias_area[1]);
-
-	  // get position of end bias area edges (b2 and b3)
-	  get_bias_area_edges(&runway.edges[1].position,
-			      runway.edges[0].heading,
-			      half_width_m,
-			      &runway.bias_area[2],
-			      &runway.bias_area[3]);
-}
-
-void
-MK_VIII::TCFHandler::get_bias_area_edges (Position *edge,
-					  double reciprocal,
-					  double half_width_m,
-					  Position *bias_edge1,
-					  Position *bias_edge2)
-{
-  double half_bias_width_m = k * SG_NM_TO_METER + half_width_m;
-  double tmp_latitude = 0.0, tmp_longitude = 0.0, az = 0.0;
-
-  geo_direct_wgs_84(0,
-		    edge->latitude,
-		    edge->longitude,
-		    reciprocal,
-		    k * SG_NM_TO_METER,
-		    &tmp_latitude,
-		    &tmp_longitude,
-		    &az);
-  geo_direct_wgs_84(0,
-		    tmp_latitude,
-		    tmp_longitude,
-		    heading_substract(reciprocal, 90),
-		    half_bias_width_m,
-		    &bias_edge1->latitude,
-		    &bias_edge1->longitude,
-		    &az);
-  geo_direct_wgs_84(0,
-		    tmp_latitude,
-		    tmp_longitude,
-		    heading_add(reciprocal, 90),
-		    half_bias_width_m,
-		    &bias_edge2->latitude,
-		    &bias_edge2->longitude,
-		    &az);
 }
 
 // Returns true if the current GPS position is inside the edge
@@ -4457,11 +4316,10 @@ MK_VIII::TCFHandler::get_bias_area_edges (Position *edge,
 bool
 MK_VIII::TCFHandler::is_inside_edge_triangle (RunwayEdge *edge)
 {
-  return get_azimuth_difference(mk_data(gps_latitude).get(),
+  double az = SGGeodesy::courseDeg( SGGeod::fromDeg(mk_data(gps_longitude).get(),
-				mk_data(gps_longitude).get(),
+                                                    mk_data(gps_latitude).get()),
-				edge->position.latitude,
+                                    edge->position);
-				edge->position.longitude,
+  return get_heading_difference(az, edge->heading) <= 45;
-				edge->heading) <= 45;
 }
 
 // Returns true if the current GPS position is inside the bias area of
@@ -4469,30 +4327,10 @@ MK_VIII::TCFHandler::is_inside_edge_triangle (RunwayEdge *edge)
 bool
 MK_VIII::TCFHandler::is_inside_bias_area ()
 {
-  double az1[4];
+  double half_bias_width_m = k * SG_NM_TO_METER + runway.half_width_m;
-  double angles_sum = 0;
+  SGVec3d cpos = SGVec3d::fromGeod(  SGGeod::fromDeg(mk_data(gps_longitude).get(), mk_data(gps_latitude).get()) );
-
+  SGLineSegmentd bias_line = SGLineSegmentd(runway.bias_points[0], runway.bias_points[1]);
-  for (int i = 0; i < 4; i++)
+  return dist(cpos, bias_line) < half_bias_width_m;
-    {
-      double az2, distance;
-      geo_inverse_wgs_84(0,
-			 mk_data(gps_latitude).get(),
-			 mk_data(gps_longitude).get(),
-			 runway.bias_area[i].latitude,
-			 runway.bias_area[i].longitude,
-			 &az1[i], &az2, &distance);
-    }
-
-  for (int i = 0; i < 4; i++)
-    {
-      double angle = az1[i == 3 ? 0 : i + 1] - az1[i];
-      if (angle < -180)
-	angle += 360;
-
-      angles_sum += angle;
-    }
-
-  return angles_sum > 180;
 }
 
 bool
@@ -4502,17 +4340,9 @@ MK_VIII::TCFHandler::is_tcf ()
     {
       if (has_runway)
 	{
-	  double distance, az1, az2;
+	  double distance = SGGeodesy::distanceNm( SGGeod::fromDeg(mk_data(gps_longitude).get(),
-
+                                                                   mk_data(gps_latitude).get()),
-	  geo_inverse_wgs_84(0,
+                                                   runway.center);
-			     mk_data(gps_latitude).get(),
-			     mk_data(gps_longitude).get(),
-			     runway.center.latitude,
-			     runway.center.longitude,
-			     &az1, &az2, &distance);
-
-	  distance *= SG_METER_TO_NM;
-
 	  // distance to the inner envelope edge
 	  double edge_distance = distance - runway.half_length - k;
 
@@ -4576,16 +4406,13 @@ MK_VIII::TCFHandler::is_rfcf ()
 {
   if (has_runway)
     {
-      double distance, az1, az2;
+
-      geo_inverse_wgs_84(0,
+      double distance = SGGeodesy::distanceNm( SGGeod::fromDeg(mk_data(gps_longitude).get(),
-			 mk_data(gps_latitude).get(),
+                                                               mk_data(gps_latitude).get()),
-			 mk_data(gps_longitude).get(),
+                                               runway.center);
-			 runway.center.latitude,
-			 runway.center.longitude,
-			 &az1, &az2, &distance);
 
       double krf = k + mk_data(gps_vertical_figure_of_merit).get() / 200;
-      distance = distance * SG_METER_TO_NM - runway.half_length - krf;
+      distance = distance - runway.half_length - krf;
 
       if (distance <= 5)
 	{

src/Instrumentation/mk_viii.hxx

@@ -1270,13 +1270,7 @@ private:
   {
     typedef struct
     {
-      double latitude;		// latitude in degrees
+      SGGeod	position;	// position of threshold
-      double longitude;		// longitude in degrees
-    } Position;
-
-    typedef struct
-    {
-      Position	position;	// position of threshold
       double	heading;	// runway heading
     } RunwayEdge;
 
@@ -1289,33 +1283,20 @@ private:
 
     struct
     {
-      Position		center;		// center point
+      SGGeod		center;		// center point
       double		elevation;	// elevation in feet
       double		half_length;	// runway half length, in nautical miles
+      double            half_width_m;   // runway half width, in meters
       RunwayEdge	edges[2];	// runway threshold and end
-      Position		bias_area[4];	// vertices of the bias area
+      SGVec3d		bias_points[2];	// vertices of the bias area
     } runway;
 
     double bias;
     double *reference;
     double initial_value;
 
-    double get_azimuth_difference (double from_lat,
-				   double from_lon,
-				   double to_lat,
-				   double to_lon,
-				   double to_heading);
-    double get_azimuth_difference (const FGRunway *_runway);
-
-    FGRunway* select_runway (const FGAirport *airport);
     void update_runway ();
 
-    void get_bias_area_edges (Position *edge,
-			      double reciprocal,
-			      double half_width_m,
-			      Position *bias_edge1,
-			      Position *bias_edge2);
-
     bool is_inside_edge_triangle (RunwayEdge *edge);
     bool is_inside_bias_area ();