Дофига исправлений

DroneSimulator/Drone.cs

@@ -9,6 +9,7 @@ namespace DroneSimulator
     public float Mass; // Масса
     public bool Active; // Живой?
     public float Length; // Длинна лучей
+    public float Dynamic; // Физика поведения
     public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
     public Quaternion Quat; // Основной кватернион
     public float Power = 0; // Тяга всех двигателей
@@ -76,6 +77,20 @@ namespace DroneSimulator
       public float LaserRange;
     }
 
+    public DataOut GetDataOut()
+    {
+      DataOut data = new DataOut();
+
+      data.AccX = Acc.X; data.AccY = Acc.Y; data.AccZ = Acc.Z;
+      data.GyrX = Gyr.X; data.GyrY = Gyr.Y; data.GyrZ = Gyr.Z;
+
+      data.PosX = PosXYZ.X; data.PosY = PosXYZ.Y;
+
+      data.LaserRange = LaserRange;
+
+      return data;
+    }
+
     public struct DataIn
     {
       public float MotorUL, MotorUR, MotorDL, MotorDR;
@@ -99,7 +114,7 @@ namespace DroneSimulator
       {
         float time = Environment.TickCount - Timer;
         Timer = Environment.TickCount;
-        Action(time / 100);
+        Action(time / 1000);
         Thread.Sleep(1);
       }
     }
@@ -108,7 +123,7 @@ namespace DroneSimulator
     {
       ID = id;
       Active = false;
-      PosXYZ = new Vector3(2000, 1000, 0);
+      PosXYZ = Vector3.Zero;
       SpdXYZ = Vector3.Zero;
       AccXYZ = Vector3.Zero;
       Quat = Quaternion.Identity;
@@ -118,9 +133,10 @@ namespace DroneSimulator
       DroneThread.Start();
     }
 
-    public int Create(float mass, float len)
+    public int Create(float mass, float len, float dynamic)
     {
       Mass = Range(mass);
+      Dynamic = dynamic * 100;
       Length = len;
 
       Active = true;
@@ -180,19 +196,23 @@ namespace DroneSimulator
 
       float flow = Power;
 
-      if (PosXYZ.Z < 100)
+      if (PosXYZ.Z < 0.3f)
       {
         flow += flow * 0.1f; // Воздушная подушка
       }
 
+      SpdPRY += AccPRY * ((time * Dynamic) / (Mass * Length));
+
+      float dyn = Dynamic / 10;
+
       Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat;
-      AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) / Mass;
-
-      SpdPRY += AccPRY * time / (Mass * Length);
-
-      SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time;
+      AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) * (dyn / Mass);
+      
+      SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity * dyn)) * time;
       PosXYZ += SpdXYZ * time;
 
+      AccXYZ /= dyn;
+
       if (PosXYZ.Z < 0)
       {
         SpdPRY = Vector3.Zero;
@@ -200,7 +220,7 @@ namespace DroneSimulator
         SpdXYZ.Y = 0;
         Quat = Quaternion.Identity;
       }
-      else Rotate(SpdPRY.X, SpdPRY.Y, SpdPRY.Z);
+      else Rotate(SpdPRY.X * time, SpdPRY.Y * time, SpdPRY.Z * time);
 
       Vector4 ori = GetOrientation();
 
@@ -213,7 +233,7 @@ namespace DroneSimulator
           Active = false; // Сильно ударился о землю
         }*/
 
-        /*if (MathF.Abs(ori.X) > 45 || MathF.Abs(ori.Y) > 45)
+        /*if (MathF.Abs(ori.X) > 20 || MathF.Abs(ori.Y) > 20)
         {
           Active = false; // Повредил винты при посадке
         }*/
@@ -226,13 +246,13 @@ namespace DroneSimulator
       }
       else
       {
-        /*if (ori.W < 0)
+        if (ori.W < 0)
         {
-          Active = false; // Перевернулся вверх ногами
-        }*/
+          //Active = false; // Перевернулся вверх ногами
+        }
 
         Quaternion grav = new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0);
-        grav = (Quat * grav) * Quaternion.Inverse(Quat);
+        //grav = (Quat * grav) * Quaternion.Inverse(Quat); // Инерциальный акселерометр (тест)
         Acc = new Vector3(grav.X, grav.Y, grav.Z);
 
         Gyr = SpdPRY;