From 96df6689e6799875ff14659fa3bd633806afd4cf Mon Sep 17 00:00:00 2001
From: Bertrand Coconnier <bcoconni@users.sourceforge.net>
Date: Sat, 10 Jun 2017 21:13:20 +0200
Subject: [PATCH] Added the computation of the wake of all AI aircrafts.

Wake computations are now performed for all AI aircrafts within a range lower or equal to the value indicated by the property /fdm/ai-wake/max-radius-nm.

These computations are triggered by the property /fdm/ai-wake/enabled (it is disabled by default).

The result of the wake computations is not yet used by the FDMs so do not expect the user aircraft to react to the AI wake.
---
 src/FDM/fdm_shell.cxx | 37 ++++++++++++++++++++++++++++++++++++-
 src/FDM/fdm_shell.hxx |  5 +++++
 src/FDM/flight.cxx    |  3 ++-
 src/FDM/flight.hxx    | 12 +++++++++---
 4 files changed, 52 insertions(+), 5 deletions(-)

diff --git a/src/FDM/fdm_shell.cxx b/src/FDM/fdm_shell.cxx
index b4ff7a6b2..75e2b3a86 100644
--- a/src/FDM/fdm_shell.cxx
+++ b/src/FDM/fdm_shell.cxx
@@ -34,6 +34,8 @@
 #include <Main/globals.hxx>
 #include <Main/fg_props.hxx>
 #include <Scenery/scenery.hxx>
+#include "AIModel/AIManager.hxx"
+#include "AIModel/AIAircraft.hxx"
 
 // all the FDMs, since we are the factory method
 #ifdef ENABLE_SP_FDM
@@ -88,6 +90,10 @@ void FDMShell::init()
   _data_logging     = _props->getNode("/sim/temp/fdm-data-logging",         true);
   _replay_master    = _props->getNode("/sim/freeze/replay-state",           true);
 
+  // AI aerodynamic wake interaction
+  _max_radius_nm    = _props->getNode("fdm/ai-wake/max-radius-nm",          true);
+  _ai_wake_enabled  = _props->getNode("fdm/ai-wake/enabled",                true);
+
   createImplementation();
 }
 
@@ -100,6 +106,12 @@ void FDMShell::postinit()
     {
         SG_LOG(SG_FLIGHT, SG_ALERT, "Failed to save initial FDM property state");
     }
+
+    _ai_mgr = dynamic_cast<FGAIManager*>(globals->get_subsystem("ai-model"));
+    if (_ai_mgr)
+        SG_LOG(SG_FLIGHT, SG_INFO, "FDM connection to the AI manager: SUCCESS");
+    else
+        SG_LOG(SG_FLIGHT, SG_DEV_ALERT, "FDM connection to the AI manager: FAILED");
 }
 
 void FDMShell::shutdown()
@@ -184,7 +196,30 @@ void FDMShell::update(double dt)
     return; // still waiting
   }
 
-// pull environmental data in, since the FDMs are lazy
+  // AI aerodynamic wake interaction
+  if (_ai_wake_enabled->getBoolValue()) {
+      for (FGAIBase* base : _ai_mgr->get_ai_list()) {
+          try {
+              if (base->isa(FGAIBase::otAircraft) ) {
+                  SGVec3d pos = _impl->getCartPosition();
+                  const SGSharedPtr<FGAIAircraft> aircraft = dynamic_cast<FGAIAircraft*>(base);
+                  double range = _ai_mgr->calcRangeFt(pos, aircraft)*SG_FEET_TO_METER;
+
+                  if (!aircraft->onGround() && aircraft->getSpeed() > 0.0
+                      && range < _max_radius_nm->getDoubleValue()*SG_NM_TO_METER) {
+                      _impl->add_ai_wake(aircraft);
+                  }
+              }
+          } catch (sg_exception& e) {
+              SG_LOG(SG_FLIGHT, SG_WARN, "caught exception updating AI model:"
+                     << base->_getName()<< ", which will be killed."
+                     "\n\tError:" << e.getFormattedMessage());
+              base->setDie(true);
+          }
+      }
+  }
+
+  // pull environmental data in, since the FDMs are lazy
   _impl->set_Velocities_Local_Airmass(
           _wind_north->getDoubleValue(),
           _wind_east->getDoubleValue(),
diff --git a/src/FDM/fdm_shell.hxx b/src/FDM/fdm_shell.hxx
index 1b4a368e5..f9047fe65 100644
--- a/src/FDM/fdm_shell.hxx
+++ b/src/FDM/fdm_shell.hxx
@@ -28,6 +28,7 @@
 
 // forward decls
 class FGInterface;
+class FGAIManager;
 
 /**
  * Wrap an FDM implementation in a subsystem with standard semantics
@@ -68,6 +69,10 @@ private:
   SGPropertyNode_ptr _density_slugft, _data_logging, _replay_master;
     
   SGPropertyNode_ptr _initialFdmProperties;
+
+    SGSharedPtr<FGAIManager> _ai_mgr;
+    SGPropertyNode_ptr _max_radius_nm;
+    SGPropertyNode_ptr _ai_wake_enabled;
 };
 
 #endif // of FG_FDM_SHELL_HXX
diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx
index 7141dbc18..692d34323 100644
--- a/src/FDM/flight.cxx
+++ b/src/FDM/flight.cxx
@@ -228,6 +228,8 @@ FGInterface::common_init ()
         set_Climb_Rate( fgGetDouble("/sim/presets/vertical-speed-fps") );
     }
 
+    reset_wake_group();
+
     SG_LOG( SG_FLIGHT, SG_INFO, "End common FDM init" );
 }
 
@@ -927,4 +929,3 @@ FGInterface::release_wire(void)
 {
   ground_cache.release_wire();
 }
-
diff --git a/src/FDM/flight.hxx b/src/FDM/flight.hxx
index 5eebb5799..348e8098b 100644
--- a/src/FDM/flight.hxx
+++ b/src/FDM/flight.hxx
@@ -80,12 +80,14 @@
 #include <simgear/structure/subsystem_mgr.hxx>
 #include <simgear/props/tiedpropertylist.hxx>
 #include <FDM/groundcache.hxx>
+#include <FDM/AIWake/AIWakeGroup.hxx>
 
 namespace simgear {
 class BVHMaterial;
 }
 
 class SGIOChannel;
+class FGAIAircraft;
 
 /**
  * A little helper class to update the track if
@@ -125,8 +127,6 @@ private:
 
 // This is based heavily on LaRCsim/ls_generic.h
 class FGInterface : public SGSubsystem {
-
-private:
   
     // Has the init() method been called.  This is used to delay
     // initialization until scenery can be loaded and we know the true
@@ -210,6 +210,8 @@ private:
     // the ground cache object itself.
     FGGroundCache ground_cache;
 
+    AIWakeGroup wake_group;
+
     void set_A_X_pilot(double x)
     { _set_Accels_Pilot_Body(x, _state.a_pilot_body_v[1], _state.a_pilot_body_v[2]); }
     
@@ -224,7 +226,6 @@ protected:
 
     int _calc_multiloop (double dt);
 
-public:
 
 				// deliberately not virtual so that
 				// FGInterface constructor will call
@@ -751,6 +752,11 @@ public:
     // Tell the cache code that it does no longer need to care for
     // the wire end position.
     void release_wire(void);
+
+    // Manages the AI wake computations.
+    void add_ai_wake(FGAIAircraft* ai) { wake_group.AddAI(ai); }
+    void reset_wake_group(void) { wake_group.gc(); }
+    const AIWakeGroup& get_wake_group(void) { return wake_group; }
 };
 
 #endif // _FLIGHT_HXX