Simulator/DroneSimulator/Drone.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;
using static System.Windows.Forms.AxHost;

namespace DroneSimulator
{
  internal class Drone
  {
    public int ID;
    public float Mass; // Масса
    public bool Active; // Живой?
    public float Length; // Длинна лучей
    public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
    public Quaternion Quat; // Основной кватернион
    public float Power = 0; // Тяга всех двигателей
    public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw

    public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
    public float LaserRange; // Имитация: Дальномер

    private const float Gravity = 1.0f;

    private const float TO_GRAD = 180 / MathF.PI;
    private const float TO_RADI = MathF.PI / 180;

    private Thread DroneThread;
    private int Timer;

    private static int CounterID = 0;

    

    public struct DataOut
    {
      public float AccX, AccY, AccZ;
      public float GyrX, GyrY, GyrZ;
      public float PosX, PosY;
      public float LaserRange;

      public byte[] getBytes()
      {
        int size = Marshal.SizeOf(this);
        byte[] arr = new byte[size];

        IntPtr ptr = IntPtr.Zero;
        try
        {
          ptr = Marshal.AllocHGlobal(size);
          Marshal.StructureToPtr(this, ptr, true);
          Marshal.Copy(ptr, arr, 0, size);
        }
        finally
        {
          Marshal.FreeHGlobal(ptr);
        }
        return arr;
      }
    }

    public struct DataIn
    {
      public float MotorUL, MotorUR, MotorDL, MotorDR;

      public void fromBytes(byte[] arr)
      {
        DataIn mem = new DataIn();

        int size = Marshal.SizeOf(mem);
        IntPtr ptr = IntPtr.Zero;
        try
        {
          ptr = Marshal.AllocHGlobal(size);

          Marshal.Copy(arr, 0, ptr, size);

          mem = (DataIn)Marshal.PtrToStructure(ptr, mem.GetType());
        }
        finally
        {
          Marshal.FreeHGlobal(ptr);
        }

        this = mem;
      }
    }

    private void ThreadFunction()
    {
      while (DroneThread != null)
      {
        float time = Environment.TickCount - Timer;
        Timer = Environment.TickCount;
        Action(time / 100);
        Thread.Sleep(1);
      }
    }

    public Drone(int id)
    {
      ID = id;
      Active = false;
      PosXYZ = new Vector3(2000, 1000, 0);
      SpdXYZ = Vector3.Zero;
      AccXYZ = Vector3.Zero;
      Quat = Quaternion.Identity;

      DroneThread = new Thread(new ThreadStart(ThreadFunction));
      Timer = Environment.TickCount;
      DroneThread.Start();
    }

    public int Create(float mass, float len)
    {
      Mass = Range(mass);
      Length = len;

      Active = true;

      return ID;
    }

    public void Close()
    {
      DroneThread = null;
    }

    private float GetAngle(float a1, float a2, float az)
    {
      if (a2 == 0.0f && az == 0.0f)
      {
        if (a1 > 0) return 90;
        if (a1 < 0) return -90;
        return 0;
      }
      return MathF.Atan(a1 / MathF.Sqrt(a2 * a2 + az * az)) * TO_GRAD;
    }

    public void Rotate(float x, float y, float z)
    {
      x = x * MathF.PI / 180;
      y = y * MathF.PI / 180;
      z = -z * MathF.PI / 180;

      Quaternion map = Quat;
      Quaternion spd = new Quaternion(x, y, z, 0);
      Quaternion aq = spd * map;

      map.W -= 0.5f * aq.W;
      map.X -= 0.5f * aq.X;
      map.Y -= 0.5f * aq.Y;
      map.Z -= 0.5f * aq.Z;

      Quat = Quaternion.Normalize(map);
    }

    public Vector4 GetOrientation()
    {
      Quaternion grav = new Quaternion(0, 0, 1, 0);

      grav = (Quat * grav) * Quaternion.Inverse(Quat);

      float yaw = 2 * MathF.Atan2(Quat.Z, Quat.W) * TO_GRAD;
      if (yaw < 0.0f) yaw = 360.0f + yaw;

      return new Vector4(GetAngle(grav.Y, grav.X, grav.Z), GetAngle(-grav.X, grav.Y, grav.Z), yaw, grav.Z);
    }

    public void Action(float time)
    {
      if (!Active) return;

      float flow = Power;

      if (PosXYZ.Z < 100)
      {
        flow += flow * 0.1f; // Воздушная подушка
      }

      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;
      PosXYZ += SpdXYZ * time;

      if (PosXYZ.Z < 0)
      {
        SpdPRY = Vector3.Zero;
        SpdXYZ.X = 0;
        SpdXYZ.Y = 0;
        Quat = Quaternion.Identity;
      }
      else Rotate(SpdPRY.X, SpdPRY.Y, SpdPRY.Z);

      Vector4 ori = GetOrientation();

      if (PosXYZ.Z < 0)
      {
        PosXYZ.Z = 0;

        /*if (SpdXYZ.Z < -5)
        {
          Active = false; // Сильно ударился о землю
        }*/

        /*if (MathF.Abs(ori.X) > 45 || MathF.Abs(ori.Y) > 45)
        {
          Active = false; // Повредил винты при посадке
        }*/

        SpdXYZ.Z = 0;

        Acc = new Vector3(0, 0, 1);
        Gyr = Vector3.Zero;
        LaserRange = 0;
      }
      else
      {
        /*if (ori.W < 0)
        {
          Active = false; // Перевернулся вверх ногами
        }*/

        Quaternion grav = new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0);
        grav = (Quat * grav) * Quaternion.Inverse(Quat);
        Acc = new Vector3(grav.X, grav.Y, grav.Z);

        Gyr = SpdPRY;

        float tilt = (MathF.Abs(ori.X) + MathF.Abs(ori.Y)) * TO_RADI;

        if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt);
        else LaserRange = float.MaxValue;
      }
    }

    private float Range(float pow)
    {
      if (pow > 1) pow = 1;
      if (pow < 0) pow = 0;

      return pow;
    }

    public void SetQadroPow(float ul, float ur, float dl, float dr)
    {
      ul = Range(ul); ur = Range(ur); dl = Range(dl); dr = Range(dr);

      Power = (ul + ur + dl + dr) / 4;

      AccPRY.Y = ((ul + dl) - (ur + dr));
      AccPRY.X = ((ul + ur) - (dl + dr));
      AccPRY.Z = ((ul + dr) - (dl + ur)) / 4;
    }
  }
}