init

processors/quantum_agents/aiww/behavior_formation.txt

@@ -0,0 +1,173 @@
+# ****************************************************************************
+# CUI
+#
+# The Advanced Framework for Simulation, Integration, and Modeling (AFSIM)
+#
+# The use, dissemination or disclosure of data in this file is subject to
+# limitation or restriction. See accompanying README and LICENSE for details.
+# ****************************************************************************
+
+
+
+
+
+behavior formation
+
+   script_debug_writes off
+
+   script_variables
+
+      //**********************************************************************//
+      //** debugging parameters                                             **//
+      //**********************************************************************//
+      bool  mDrawFormationData =  false;
+
+      //**********************************************************************//
+      //** formation flying parameters, for offset and tightness            **//
+      //**********************************************************************//
+      Array<string> mLeadNames = Array<string>();
+      double   mFormationPositionX =  -500;     #meters in front of of package
+      double   mFormationPositionY =  3000;  #meters off right wing of package
+      double   mGoodFormationRatio =  0.15;  #percentage of total offset
+      double   mFormationLookAhead = 30.0;   #seconds
+      double   mFormationAltitude  = -1.0;   #used if positive
+
+      //**********************************************************************//
+      //********* VARIABLES BELOW THIS LINE ARE NOT FOR USER EDITING *********//
+      //**********************************************************************//
+      WsfDraw  mDraw           =  WsfDraw();
+      bool     mLastInPosition =  false;  #boolean flag, utility, don't change
+
+   end_script_variables
+
+
+   ### find escorted platform, return it
+   script WsfPlatform GetFormationLead()
+      WsfPlatform leadPlatform;
+      foreach ( string sLeadName in mLeadNames )
+      {
+         leadPlatform = WsfSimulation.FindPlatform(sLeadName);
+         if (leadPlatform.IsValid() )
+         {
+            break;
+         }
+      }
+      return leadPlatform;
+   end_script
+
+
+   precondition
+      writeln_d("precondition formation");
+
+      WsfPlatform leadPlatform = GetFormationLead();
+      if (!leadPlatform.IsValid() )
+      {
+         return Failure("no listed lead for formation found!");
+      }
+
+      writeln_d("--- agent ", PLATFORM.Name(), " is in formation with ", leadPlatform.Name());
+      return true;
+   end_precondition
+
+
+
+   script bool AmInFormationPosition(WsfGeoPoint formationPoint, double radius)
+      //make it fly to the inner 50% of the acceptable radius on re-entry
+      if (!mLastInPosition)
+      {
+         radius = 0.5 * radius;
+      }
+      mLastInPosition = false;
+      double range = PLATFORM.GroundRangeTo(formationPoint);
+      if (range <= radius)
+      {
+         mLastInPosition = true;
+      }
+      return mLastInPosition;
+   end_script
+
+
+
+   execute
+      writeln_d("executing formation.");
+
+      //make sure escorted platform exists, find it, save it
+      WsfPlatform leadPlatform = GetFormationLead();
+      if (!leadPlatform.IsValid() )
+      {
+         writeln_d("--- fighter ", PLATFORM.Name(), " no valid platform to escort!");
+         return;
+      }
+
+      //check if I am approximately in my formation position
+      WsfGeoPoint formationPoint = leadPlatform.Location();
+
+      if (mFormationAltitude > 0)
+      {
+        formationPoint.Set(formationPoint.Latitude(), formationPoint.Longitude(), mFormationAltitude);
+      }
+
+      if (mFormationPositionX > 0)
+      {
+         formationPoint.Extrapolate(leadPlatform.Heading(), mFormationPositionX);
+      }
+      else
+      {
+         formationPoint.Extrapolate(leadPlatform.Heading()+180, -mFormationPositionX);
+      }
+
+      if (mFormationPositionY > 0)
+      {
+         formationPoint.Extrapolate(leadPlatform.Heading()+90.0, mFormationPositionY);
+      }
+      else
+      {
+         formationPoint.Extrapolate(leadPlatform.Heading()-90.0, -mFormationPositionY);
+      }
+
+      if (!formationPoint.IsValid())
+      {
+         writeln_d("--- fighter ", PLATFORM.Name(), " invalid formation point from ", leadPlatform.Name());
+         return;
+      }
+
+      if (mDrawFormationData == true)
+      {
+         mDraw.SetLayer("behavior_formation");
+         mDraw.SetDuration(PROCESSOR.UpdateInterval());
+
+         // draw line to desired formation position
+         mDraw.SetColor(0, 1, 0);   //green
+         mDraw.SetLineSize(4);
+         mDraw.BeginLines();
+            mDraw.Vertex(leadPlatform.Location());
+            mDraw.Vertex(formationPoint);
+         mDraw.End();
+      }
+
+      double radius = mGoodFormationRatio * leadPlatform.SlantRangeTo(formationPoint);
+
+      double formAlt   = leadPlatform.Altitude();
+      double formSpeed = leadPlatform.Speed();
+      double formYaw   = leadPlatform.Heading();
+      if (formAlt < PLATFORM.Altitude())
+      {
+         formAlt = PLATFORM.Altitude();
+      }
+      WsfGeoPoint tgt = WsfGeoPoint.Construct(formationPoint.Latitude(), formationPoint.Longitude(), formAlt);
+      double lookAheadDistance  = mFormationLookAhead * leadPlatform.Speed();
+      tgt.Extrapolate(formYaw, lookAheadDistance);
+
+      if (! AmInFormationPosition(formationPoint, radius))
+      {
+         //fly towards formation position, extrapolated ahead X seconds, in order to catch up to the point in X seconds
+         formSpeed = PLATFORM.GroundRangeTo(tgt) / mFormationLookAhead;
+      }
+
+     #extern bool FlyTarget (WsfPlatform, WsfGeoPoint,    double);
+      FlyTarget (PLATFORM, tgt, formSpeed);
+
+   end_execute
+
+end_behavior
+