Ground network distance tracking code. AIAircraft taxiing at airports

[flightgear.git] / src / AIModel / AIBase.cxx

diff --git a/src/AIModel/AIBase.cxx b/src/AIModel/AIBase.cxx
index 43a001cb94266ca3b24d470cd827e9df29045eb8..1fe6a7e0ebbb581082cea26c03ecb61ec03dbba9 100644 (file)
--- a/src/AIModel/AIBase.cxx
+++ b/src/AIModel/AIBase.cxx
@@ -30,10 +30,12 @@
 #include <plib/ssg.h>
 
 #include <simgear/math/point3d.hxx>
+#include <simgear/math/polar3d.hxx>
 #include <simgear/math/sg_geodesy.hxx>
 #include <simgear/misc/sg_path.hxx>
 #include <simgear/scene/model/location.hxx>
 #include <simgear/scene/model/model.hxx>
+#include <simgear/scene/model/personality.hxx>
 #include <simgear/debug/logstream.hxx>
 #include <simgear/props/props.hxx>
 
@@ -49,14 +51,14 @@ const double FGAIBase::e = 2.71828183;
 const double FGAIBase::lbs_to_slugs = 0.031080950172;   //conversion factor
 
 
-FGAIBase::FGAIBase(object_type ot)
-  : fp( NULL ),
+FGAIBase::FGAIBase(object_type ot) :
     props( NULL ),
     manager( NULL ),
+    fp( NULL ),
     _refID( _newAIModelID() ),
     _otype(ot)
 {
-    tgt_heading = hdg = tgt_altitude = tgt_speed = 0.0;
+    tgt_heading = hdg = tgt_altitude_ft = tgt_speed = 0.0;
     tgt_roll = roll = tgt_pitch = tgt_yaw = tgt_vs = vs = pitch = 0.0;
     bearing = elevation = range = rdot = 0.0;
     x_shift = y_shift = rotation = 0.0;
@@ -161,6 +163,8 @@ ssgBranch * FGAIBase::load3DModel(const string& fg_root,
   // some more code here to check whether a model with this name has already been loaded
   // if not load it, otherwise, get the memory pointer and do something like
   // SetModel as in ATC/AIEntity.cxx
+  ssgBranch *personality_branch = new SGPersonalityBranch;
+
   model = manager->getModel(path);
   if (!(model)) {
       model = sgLoad3DModel(fg_root,
@@ -169,8 +173,9 @@ ssgBranch * FGAIBase::load3DModel(const string& fg_root,
                             sim_time_sec);
       manager->setModel(path, model);
   }
+  personality_branch->addKid( model );
 
-  return model;
+  return personality_branch;
 }
 
 bool FGAIBase::isa( object_type otype ) {
@@ -229,7 +234,7 @@ void FGAIBase::bind() {
    props->setDoubleValue("controls/flight/target-roll", roll);
 
    props->setStringValue("controls/flight/longitude-mode", "alt");
-   props->setDoubleValue("controls/flight/target-alt", altitude);
+   props->setDoubleValue("controls/flight/target-alt", altitude_ft);
    props->setDoubleValue("controls/flight/target-pitch", pitch);
 
    props->setDoubleValue("controls/flight/target-spd", speed);
@@ -288,8 +293,8 @@ double FGAIBase::UpdateRadar(FGAIManager* manager)
      double user_altitude  = manager->get_user_altitude();
      double user_heading   = manager->get_user_heading();
      double user_pitch     = manager->get_user_pitch();
-     double user_yaw       = manager->get_user_yaw();
-     double user_speed     = manager->get_user_speed();
+     //double user_yaw       = manager->get_user_yaw();
+     //double user_speed     = manager->get_user_speed();
 
      // calculate range to target in feet and nautical miles
      double range_ft = sqrt( range_ft2 );
@@ -312,13 +317,26 @@ double FGAIBase::UpdateRadar(FGAIManager* manager)
         }
      }
 
+     // This is an alternate way to compute bearing and distance which
+     // agrees with the original scheme within about 0.1 degrees.
+     //
+     // Point3D start( user_longitude * SGD_DEGREES_TO_RADIANS,
+     //                user_latitude * SGD_DEGREES_TO_RADIANS, 0 );
+     // Point3D dest( pos.getLongitudeRad(), pos.getLatitudeRad(), 0 );
+     // double gc_bearing, gc_range;
+     // calc_gc_course_dist( start, dest, &gc_bearing, &gc_range );
+     // gc_range *= SG_METER_TO_NM;
+     // gc_bearing *= SGD_RADIANS_TO_DEGREES;
+     // printf("orig b = %.3f %.2f  gc b= %.3f, %.2f\n",
+     //        bearing, range, gc_bearing, gc_range);
+
      // calculate look left/right to target, without yaw correction
      horiz_offset = bearing - user_heading;
      if (horiz_offset > 180.0) horiz_offset -= 360.0;
      if (horiz_offset < -180.0) horiz_offset += 360.0;
 
      // calculate elevation to target
-     elevation = atan2( altitude - user_altitude, range_ft ) * SG_RADIANS_TO_DEGREES;
+     elevation = atan2( altitude_ft - user_altitude, range_ft ) * SG_RADIANS_TO_DEGREES;
 
      // calculate look up/down to target
      vert_offset = elevation - user_pitch;
@@ -335,14 +353,14 @@ double FGAIBase::UpdateRadar(FGAIManager* manager)
 */
 
      // now correct look left/right for yaw
-     horiz_offset += user_yaw;
+     // horiz_offset += user_yaw; // FIXME: WHY WOULD WE WANT TO ADD IN SIDE-SLIP HERE?
 
      // calculate values for radar display
      y_shift = range * cos( horiz_offset * SG_DEGREES_TO_RADIANS);
      x_shift = range * sin( horiz_offset * SG_DEGREES_TO_RADIANS);
      rotation = hdg - user_heading;
      if (rotation < 0.0) rotation += 360.0;
-     ht_diff = altitude - user_altitude;
+     ht_diff = altitude_ft - user_altitude;
 
    }
 
@@ -396,7 +414,7 @@ void FGAIBase::_setVS_fps( double _vs ) {
 }
 
 double FGAIBase::_getAltitude() const {
-    return altitude;
+    return altitude_ft;
 }
 void FGAIBase::_setAltitude( double _alt ) {
     setAltitude( _alt );
@@ -414,6 +432,8 @@ void FGAIBase::CalculateMach() {
     // Calculate rho at altitude, using standard atmosphere
     // For the temperature T and the pressure p,
 
+    double altitude = altitude_ft;
+
     if (altitude < 36152) {            // curve fits for the troposphere
       T = 59 - 0.00356 * altitude;
       p = 2116 * pow( ((T + 459.7) / 518.6) , 5.256);