init

2025-09-12 15:20:28 +08:00
commit 3257a14c32
449 changed files with 388780 additions and 0 deletions
--- a/processors/timeline_agents/behavior_planned_route.txt
+++ b/processors/timeline_agents/behavior_planned_route.txt
@@ -0,0 +1,124 @@
+# ****************************************************************************
+# 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.
+# ****************************************************************************
+
+//TODO use default values from higher level context (processor or parent behavior that holds default)
+
+
+#assumes aDraw duration & layer is set
+script void DrawRoute(WsfDraw aDraw, WsfRoute aRoute)
+   if (aRoute.IsValid())
+   {
+      aDraw.SetColor(0,1,1);   //teal?
+      aDraw.SetLineSize(2);
+      aDraw.SetLineStyle("solid");
+      aDraw.BeginPolyline();
+         for (int i=0; i<aRoute.Size(); i=i+1)
+         {
+            aDraw.Vertex(aRoute.Waypoint(i).Location());
+         }
+      aDraw.End();
+
+      aDraw.SetColor(1.0,0.3,0.3);   //pink?
+      aDraw.SetPointSize(4);
+      aDraw.BeginPoints();
+         for (int i=0; i<aRoute.Size(); i=i+1)
+         {
+            aDraw.Vertex(aRoute.Waypoint(i).Location());
+         }
+      aDraw.End();
+   }
+end_script
+
+
+behavior planned_route
+
+   script_debug_writes off
+
+   script_variables
+      bool     mDrawRoute              = false;
+      WsfDraw  mDraw                   = WsfDraw();
+      double   cDEFAULT_SPEED          = 450.0 * MATH.MPS_PER_NMPH();
+      double   cDEFAULT_ACCEL          =   7.5 * Earth.ACCEL_OF_GRAVITY(); // 7.5 G  (m/s^2)
+   end_script_variables
+
+   precondition
+      writeln_d("precondition planned_route");
+      return true;
+   end_precondition
+
+   on_new_execute
+      PLATFORM.Comment("route");
+   end_on_new_execute
+
+   execute
+      writeln_d(PLATFORM.Name(), " executing planned_route, T=", TIME_NOW);
+      #only command the platform to do something different if its not currently flying a route
+      WsfMover aMover = PLATFORM.Mover();
+      if (aMover.IsValid()) {
+         if (aMover.IsExtrapolating()) {
+            WsfGeoPoint pt = PLATFORM.Location();
+            WsfRoute ro = aMover.DefaultRoute().Copy(); #now we have a modifiable route
+            if (!ro.IsValid())
+               return;
+
+            writeln_d("flying route, name: ", ro.Name(), ", type: ", ro.Type());
+            WsfGeoPoint close = ro.LocationAtDistance(ro.DistanceAlongRoute(pt));
+            if (!close.IsValid()) {
+               return;
+            }
+            close.SetAltitudeAGL(pt.Altitude());
+            if (mDrawRoute)
+            {
+               mDraw.BeginLines();
+                  mDraw.Vertex(pt);
+                  mDraw.Vertex(close);
+               mDraw.End();
+            }
+            double d1 = ro.DistanceFromRoute(pt);
+            double d2 = pt.GroundRangeTo(close);
+            double d3 = -1;
+            Array<double> turnRad = aMover.PropertyDouble("turn_radius");
+            if (turnRad.Size() > 0) {
+               d3 = 2*turnRad[0];
+            }
+            int i = 0;
+            for (; i < ro.Size(); i = i+1)
+            {
+               WsfWaypoint wpt = ro.Waypoint(i);
+               WsfGeoPoint rpt = wpt.Location();
+               //check if we are close to an existing waypoint, if so... break & fly at that one
+               if (rpt.GroundRangeTo(close) < 926) {
+                  break;
+               }
+               double dist = ro.DistanceAlongRoute(rpt);
+               if (dist > d1) {
+                  if (d2 > d3) {
+                     ro.Insert(i, WsfWaypoint.Create(close, wpt.Speed()));
+                  }
+                  break;
+               }
+            }
+            if (i >= ro.Size()) {
+               i = ro.Size() - 1;
+            }
+            //go at default speed; this gets overwritten if route waypoint has defined a speed
+            PLATFORM.GoToSpeed(cDEFAULT_SPEED, cDEFAULT_ACCEL, true);
+            PLATFORM.FollowRoute(ro, i);
+         }
+      }
+
+      if (mDrawRoute)
+      {
+         mDraw.SetDuration(PROCESSOR.UpdateInterval());
+         mDraw.SetLayer("behavior_planned_route");
+         DrawRoute(mDraw, PLATFORM.Route());
+      }
+   end_execute
+
+end_behavior