update

DroneSimulator/RealMode.cs

@@ -4,6 +4,7 @@ using System.Linq;
 using System.Text;
 using System.Numerics;
 using System.Reflection;
+using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
 
 namespace DroneSimulator
 {
@@ -263,7 +264,83 @@ namespace DroneSimulator
 
     internal class OpticalFlow
     {
+      public static bool Enable;
+      public static float MaxHeight;
+      public static uint Freq;
+      public static float Noise;
+      public static float Lateness;
+      public static float Error;
+      public static uint Wait;
+      public static float Lens;
 
+      private uint last = 0;
+
+      private Random rand = new Random();
+
+      private const int count = 1000;
+      private Vector2[] laten = new Vector2[count];
+      private uint index = 0;
+
+      public uint delay = 0;
+
+      public uint timer = 0;
+      public Vector2 result;
+      public void Update(Vector2 value, float Range, uint time)
+      {
+        if (!RealSimulation)
+        {
+          result = value;
+          timer = time;
+          return;
+        }
+
+        if (!Enable)
+        {
+          result = Vector2.NaN;
+          return;
+        }
+
+        value *= Lens;
+
+        if (rand.Next(0, 1000) < (Error * 10))
+        {
+          value = Vector2.Zero;
+          delay = time + Wait;
+        }
+        else if (delay > time)
+        {
+          value = Vector2.Zero;
+        }
+
+        if (Range > MaxHeight) value = Vector2.Zero;
+        else
+        {
+          int noise = (int)(Noise * 1000);
+          value.X += ((float)rand.Next(-noise, noise)) / 1000;
+          value.Y += ((float)rand.Next(-noise, noise)) / 1000;
+        }
+
+        uint clock = (uint)(Lateness * 1000);
+
+        uint tick = time - last;
+        last = time;
+        while (tick != 0)
+        {
+          tick--;
+          laten[index++] = value;
+          if (index >= clock) index = 0;
+        }
+
+        value = laten[index];
+
+        uint freq = 1000 / Freq;
+
+        if (timer + freq < time)
+        {
+          result = value;
+          timer = time;
+        }
+      }
     }
 
     internal class Range