Чистка кода от мусора

DroneClient/Drone.cs

@@ -1,67 +1,62 @@
-using System;
+using System.Runtime.InteropServices;
-using System.Collections.Generic;
-using System.Linq;
-using System.Runtime.InteropServices;
-using System.Text;
-using System.Threading.Tasks;
 
 namespace DroneClient
 {
-  internal class Drone
+    internal class Drone
-  {
-    public struct DataOut
     {
-      public float AccX, AccY, AccZ;
+        public struct DataOut
-      public float GyrX, GyrY, GyrZ;
+        {
-      public float PosX, PosY;
+            public float AccX, AccY, AccZ;
-      public float LaserRange;
+            public float GyrX, GyrY, GyrZ;
+            public float PosX, PosY;
+            public float LaserRange;
+        }
+
+        public struct DataIn
+        {
+            public float MotorUL, MotorUR, MotorDL, MotorDR;
+        }
+
+        public static byte[] getBytes(object data)
+        {
+            int size = Marshal.SizeOf(data);
+            byte[] arr = new byte[size];
+
+            IntPtr ptr = IntPtr.Zero;
+            try
+            {
+                ptr = Marshal.AllocHGlobal(size);
+                Marshal.StructureToPtr(data, ptr, true);
+                Marshal.Copy(ptr, arr, 0, size);
+            }
+            finally
+            {
+                Marshal.FreeHGlobal(ptr);
+            }
+            return arr;
+        }
+
+        public static object fromBytes(byte[] arr, Type type)
+        {
+            object mem = new object();
+
+            int size = Marshal.SizeOf(type);
+            IntPtr ptr = IntPtr.Zero;
+            try
+            {
+                ptr = Marshal.AllocHGlobal(size);
+
+                Marshal.Copy(arr, 0, ptr, size);
+
+                mem = Marshal.PtrToStructure(ptr, type);
+            }
+            finally
+            {
+                Marshal.FreeHGlobal(ptr);
+            }
+
+            return mem;
+        }
+
     }
-
-    public struct DataIn
-    {
-      public float MotorUL, MotorUR, MotorDL, MotorDR;  
-    }
-
-    public static byte[] getBytes(object data)
-    {
-      int size = Marshal.SizeOf(data);
-      byte[] arr = new byte[size];
-
-      IntPtr ptr = IntPtr.Zero;
-      try
-      {
-        ptr = Marshal.AllocHGlobal(size);
-        Marshal.StructureToPtr(data, ptr, true);
-        Marshal.Copy(ptr, arr, 0, size);
-      }
-      finally
-      {
-        Marshal.FreeHGlobal(ptr);
-      }
-      return arr;
-    }
-
-    public static object fromBytes(byte[] arr, Type type)
-    {
-      object mem = new object();
-
-      int size = Marshal.SizeOf(type);
-      IntPtr ptr = IntPtr.Zero;
-      try
-      {
-        ptr = Marshal.AllocHGlobal(size);
-
-        Marshal.Copy(arr, 0, ptr, size);
-
-        mem = Marshal.PtrToStructure(ptr, type);
-      }
-      finally
-      {
-        Marshal.FreeHGlobal(ptr);
-      }
-
-      return mem;
-    }
-
-  }
 }

DroneClient/NetClient.cs

@@ -1,108 +1,103 @@
-using System;
+using System.Net;
-using System.Collections.Generic;
-using System.Linq;
-using System.Text;
-using System.Threading.Tasks;
 using System.Net.Sockets;
-using System.Net;
 
 namespace DroneSimulator
 {
-  internal class NetClient
+    internal class NetClient
-  {
-    public class ConnectData
     {
-      public bool Connect;
+        public class ConnectData
+        {
+            public bool Connect;
 
-      public Socket? Server;
+            public Socket? Server;
+        }
+
+        public class ReceiveData
+        {
+            public byte[]? Buffer;
+            public int Size;
+
+            public Socket? Server;
+        }
+
+        private class ServerData
+        {
+            public const int size = 1024;
+            public byte[] buffer = new byte[size];
+        }
+
+        private bool Connected = false;
+        private Socket? ServerSocket = null;
+        private ServerData DataServer = new ServerData();
+
+        public delegate void ClientCallback(object o);
+
+        private ClientCallback? ConnectionCallback;
+        private ClientCallback? ReceiveCallback;
+
+        public enum ClientState { Error, Connected, Stop };
+
+        public ClientState Connect(string Addr, int Port, ClientCallback Connection, ClientCallback Receive)
+        {
+            if (Connected)
+            {
+                try { ServerSocket?.Shutdown(SocketShutdown.Both); } catch { }
+                ServerSocket?.Close();
+                Connected = false;
+                return ClientState.Stop;
+            }
+
+            ConnectionCallback = Connection;
+            ReceiveCallback = Receive;
+
+            IPEndPoint ep = new IPEndPoint(IPAddress.Parse(Addr), Port);
+            ServerSocket = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
+
+            try { ServerSocket.Connect(ep); }
+            catch { ServerSocket.Close(); return ClientState.Error; }
+
+            Connected = true;
+
+            ConnectionCallback(new ConnectData { Connect = true, Server = ServerSocket });
+
+            ReceiveData receiveData = new ReceiveData { Buffer = DataServer.buffer, Size = ServerData.size, Server = ServerSocket };
+
+            try { ServerSocket.BeginReceive(DataServer.buffer, 0, ServerData.size, 0, new AsyncCallback(ReadCallback), receiveData); }
+            catch { }
+
+            return ClientState.Connected;
+        }
+
+        public void Close()
+        {
+            try { ServerSocket?.Shutdown(SocketShutdown.Both); } catch { }
+            ServerSocket?.Close();
+            Connected = false;
+        }
+
+        public void ReadCallback(IAsyncResult ar)
+        {
+            ReceiveData cd = (ReceiveData)ar.AsyncState;
+            if (cd == null) return;
+
+            int bytes = 0;
+            try { bytes = ServerSocket.EndReceive(ar); } catch { }
+
+            if (bytes == 0)
+            {
+                ServerSocket?.Close();
+
+                Connected = false;
+
+                if (ServerSocket != null) ConnectionCallback(new ConnectData { Connect = false, Server = null });
+
+                return;
+            }
+
+            ReceiveCallback(new ReceiveData { Buffer = cd.Buffer, Size = bytes, Server = ServerSocket });
+
+            try { ServerSocket?.BeginReceive(cd.Buffer, 0, ServerData.size, 0, new AsyncCallback(ReadCallback), cd); }
+            catch { }
+        }
     }
-
-    public class ReceiveData
-    {
-      public byte[]? Buffer;
-      public int Size;
-
-      public Socket? Server;
-    }
-
-    private class ServerData
-    {
-      public const int size = 1024;
-      public byte[] buffer = new byte[size];
-    }
-
-    private bool Connected = false;
-    private Socket? ServerSocket = null;
-    private ServerData DataServer = new ServerData();
-
-    public delegate void ClientCallback(object o);
-
-    private ClientCallback? ConnectionCallback;
-    private ClientCallback? ReceiveCallback;
-
-    public enum ClientState { Error, Connected, Stop };
-
-    public ClientState Connect(string Addr, int Port, ClientCallback Connection, ClientCallback Receive)
-    {
-      if (Connected)
-      {
-        try { ServerSocket?.Shutdown(SocketShutdown.Both); } catch { }
-        ServerSocket?.Close();
-        Connected = false;
-        return ClientState.Stop;
-      }
-
-      ConnectionCallback = Connection;
-      ReceiveCallback = Receive;
-
-      IPEndPoint ep = new IPEndPoint(IPAddress.Parse(Addr), Port);
-      ServerSocket = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
-
-      try { ServerSocket.Connect(ep); }
-      catch { ServerSocket.Close(); return ClientState.Error; }
-
-      Connected = true;
-
-      ConnectionCallback(new ConnectData { Connect = true, Server = ServerSocket });
-
-      ReceiveData receiveData = new ReceiveData { Buffer = DataServer.buffer, Size = ServerData.size, Server = ServerSocket };
-
-      try { ServerSocket.BeginReceive(DataServer.buffer, 0, ServerData.size, 0, new AsyncCallback(ReadCallback), receiveData); }
-      catch { }
-
-      return ClientState.Connected;
-    }
-
-    public void Close()
-    {
-      try { ServerSocket?.Shutdown(SocketShutdown.Both); } catch { }
-      ServerSocket?.Close();
-      Connected = false;
-    }
-
-    public void ReadCallback(IAsyncResult ar)
-    {
-      ReceiveData cd = (ReceiveData)ar.AsyncState;
-      if (cd == null) return;
-
-      int bytes = 0;
-      try { bytes = ServerSocket.EndReceive(ar); } catch {  }
-
-      if (bytes == 0)
-      {
-        ServerSocket?.Close();
-
-        Connected = false;
-
-        if (ServerSocket != null) ConnectionCallback(new ConnectData { Connect = false, Server = null });
-
-        return;
-      }
-
-      ReceiveCallback(new ReceiveData { Buffer = cd.Buffer, Size = bytes, Server = ServerSocket });
-
-      try { ServerSocket?.BeginReceive(cd.Buffer, 0, ServerData.size, 0, new AsyncCallback(ReadCallback), cd); }  
-      catch { }
-    }
-  }
 }

DroneSimulator/Drone.cs

@@ -1,262 +1,256 @@
-using System;
+using System.Numerics;
-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
+    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 int ID;
-      public float GyrX, GyrY, GyrZ;
+        public float Mass; // Масса
-      public float PosX, PosY;
+        public bool Active; // Живой?
-      public float LaserRange;
+        public float Length; // Длинна лучей
+        public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
+        public Quaternion Quat; // Основной кватернион
+        public float Power = 0; // Тяга всех двигателей
+        public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw
 
-      public byte[] getBytes()
+        public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
-      {
+        public float LaserRange; // Имитация: Дальномер
-        int size = Marshal.SizeOf(this);
-        byte[] arr = new byte[size];
 
-        IntPtr ptr = IntPtr.Zero;
+        private const float Gravity = 1.0f;
-        try
+
+        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
         {
-          ptr = Marshal.AllocHGlobal(size);
+            public float AccX, AccY, AccZ;
-          Marshal.StructureToPtr(this, ptr, true);
+            public float GyrX, GyrY, GyrZ;
-          Marshal.Copy(ptr, arr, 0, size);
+            public float PosX, PosY;
-        }
+            public float LaserRange;
-        finally
-        {
-          Marshal.FreeHGlobal(ptr);
-        }
-        return arr;
-      }
-    }
 
-    public struct DataIn
+            public byte[] getBytes()
-    {
+            {
-      public float MotorUL, MotorUR, MotorDL, MotorDR;
+                int size = Marshal.SizeOf(this);
+                byte[] arr = new byte[size];
 
-      public void fromBytes(byte[] arr)
+                IntPtr ptr = IntPtr.Zero;
-      {
+                try
-        DataIn mem = new DataIn();
+                {
-
+                    ptr = Marshal.AllocHGlobal(size);
-        int size = Marshal.SizeOf(mem);
+                    Marshal.StructureToPtr(this, ptr, true);
-        IntPtr ptr = IntPtr.Zero;
+                    Marshal.Copy(ptr, arr, 0, size);
-        try
+                }
-        {
+                finally
-          ptr = Marshal.AllocHGlobal(size);
+                {
-
+                    Marshal.FreeHGlobal(ptr);
-          Marshal.Copy(arr, 0, ptr, size);
+                }
-
+                return arr;
-          mem = (DataIn)Marshal.PtrToStructure(ptr, mem.GetType());
+            }
-        }
-        finally
-        {
-          Marshal.FreeHGlobal(ptr);
         }
 
-        this = mem;
+        public struct DataIn
-      }
-    }
-
-    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; // Сильно ударился о землю
+            public float MotorUL, MotorUR, MotorDL, MotorDR;
-        }*/
 
-        /*if (MathF.Abs(ori.X) > 45 || MathF.Abs(ori.Y) > 45)
+            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()
         {
-          Active = false; // Повредил винты при посадке
+            while (DroneThread != null)
-        }*/
+            {
+                float time = Environment.TickCount - Timer;
+                Timer = Environment.TickCount;
+                Action(time / 100);
+                Thread.Sleep(1);
+            }
+        }
 
-        SpdXYZ.Z = 0;
+        public Drone(int id)
-
-        Acc = new Vector3(0, 0, 1);
-        Gyr = Vector3.Zero;
-        LaserRange = 0;
-      }
-      else
-      {
-        /*if (ori.W < 0)
         {
-          Active = false; // Перевернулся вверх ногами
+            ID = id;
-        }*/
+            Active = false;
+            PosXYZ = new Vector3(2000, 1000, 0);
+            SpdXYZ = Vector3.Zero;
+            AccXYZ = Vector3.Zero;
+            Quat = Quaternion.Identity;
 
-        Quaternion grav = new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0);
+            DroneThread = new Thread(new ThreadStart(ThreadFunction));
-        grav = (Quat * grav) * Quaternion.Inverse(Quat);
+            Timer = Environment.TickCount;
-        Acc = new Vector3(grav.X, grav.Y, grav.Z);
+            DroneThread.Start();
+        }
 
-        Gyr = SpdPRY;
+        public int Create(float mass, float len)
+        {
+            Mass = Range(mass);
+            Length = len;
 
-        float tilt = (MathF.Abs(ori.X) + MathF.Abs(ori.Y)) * TO_RADI;
+            Active = true;
 
-        if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt);
+            return ID;
-        else LaserRange = float.MaxValue;
+        }
-      }
+
+        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;
+        }
     }
-
-    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;
-    }
-  }
 }

DroneSimulator/NetServerClients.cs

@@ -1,145 +1,140 @@
-using System;
+using System.Net;
-using System.Collections.Generic;
-using System.Linq;
-using System.Text;
-using System.Threading.Tasks;
 using System.Net.Sockets;
-using System.Net;
 
 namespace DroneSimulator
 {
-  internal class NetServerClients
+    internal class NetServerClients
-  {
-    public class ConnectData
     {
-      public int ID;
+        public class ConnectData
-      public bool Connect;
-      public int Count;
-
-      public Socket? Client;
-    }
-
-    public class ReceiveData
-    {
-      public int ID;
-      public byte[]? Buffer;
-      public int Size;
-
-      public Socket? Client;
-    }
-
-    private class ClientData
-    {
-      public int ID;
-      public Socket? workSocket = null;
-      public const int BufferSize = 1024;
-      public byte[] buffer = new byte[BufferSize];
-    }
-
-    private int SocketID = 0;
-    private int SocketLimit;
-    private Socket? ServerSocket;
-    private List<ClientData> ClientSockets = new List<ClientData>();
-
-    public delegate void ServerCallback(object o);
-
-    private ServerCallback? ConnectionCallback;
-    private ServerCallback? ReceiveCallback;
-
-    private bool Active = false;
-
-    public enum ServerState { Error, Start, Stop };
-
-    public ServerState StartServer(int Port, int Limit, ServerCallback Connection, ServerCallback Receive)
-    {
-      if (Active)
-      {
-        ServerSocket?.Close();
-        foreach (ClientData c in ClientSockets)
         {
-          try { c.workSocket?.Shutdown(SocketShutdown.Both); } catch { }
+            public int ID;
-          c.workSocket?.Close();
+            public bool Connect;
+            public int Count;
+
+            public Socket? Client;
         }
-        ClientSockets.Clear();
-        return ServerState.Stop;
-      }
 
-      ConnectionCallback = Connection;
+        public class ReceiveData
-      ReceiveCallback = Receive;
+        {
+            public int ID;
+            public byte[]? Buffer;
+            public int Size;
 
-      SocketLimit = Limit;
+            public Socket? Client;
+        }
 
-      IPEndPoint ip = new(IPAddress.Any, Port);
+        private class ClientData
-      ServerSocket = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
+        {
+            public int ID;
+            public Socket? workSocket = null;
+            public const int BufferSize = 1024;
+            public byte[] buffer = new byte[BufferSize];
+        }
 
-      try
+        private int SocketID = 0;
-      {
+        private int SocketLimit;
-        ServerSocket.Bind(ip);
+        private Socket? ServerSocket;
-        ServerSocket.Listen(10);
+        private List<ClientData> ClientSockets = new List<ClientData>();
-        ServerSocket.BeginAccept(new AsyncCallback(AcceptCallback), ServerSocket);
-        Active = true;
-      }
-      catch { ServerSocket.Close(); return ServerState.Error; }
 
-      return ServerState.Start;
+        public delegate void ServerCallback(object o);
+
+        private ServerCallback? ConnectionCallback;
+        private ServerCallback? ReceiveCallback;
+
+        private bool Active = false;
+
+        public enum ServerState { Error, Start, Stop };
+
+        public ServerState StartServer(int Port, int Limit, ServerCallback Connection, ServerCallback Receive)
+        {
+            if (Active)
+            {
+                ServerSocket?.Close();
+                foreach (ClientData c in ClientSockets)
+                {
+                    try { c.workSocket?.Shutdown(SocketShutdown.Both); } catch { }
+                    c.workSocket?.Close();
+                }
+                ClientSockets.Clear();
+                return ServerState.Stop;
+            }
+
+            ConnectionCallback = Connection;
+            ReceiveCallback = Receive;
+
+            SocketLimit = Limit;
+
+            IPEndPoint ip = new(IPAddress.Any, Port);
+            ServerSocket = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
+
+            try
+            {
+                ServerSocket.Bind(ip);
+                ServerSocket.Listen(10);
+                ServerSocket.BeginAccept(new AsyncCallback(AcceptCallback), ServerSocket);
+                Active = true;
+            }
+            catch { ServerSocket.Close(); return ServerState.Error; }
+
+            return ServerState.Start;
+        }
+
+        public void AcceptCallback(IAsyncResult ar)
+        {
+            Socket listener = (Socket)ar.AsyncState;
+            if (listener == null) return;
+
+            Socket handler;
+
+            try { handler = listener.EndAccept(ar); }
+            catch { ServerSocket?.Close(); Active = false; return; }
+
+            if (SocketLimit > ClientSockets.Count)
+            {
+                ClientData clientData = new ClientData();
+
+                clientData.ID = ++SocketID;
+
+                clientData.workSocket = handler;
+
+                ClientSockets.Add(clientData);
+
+                ConnectionCallback(new ConnectData { ID = clientData.ID, Connect = true, Count = ClientSockets.Count, Client = handler });
+
+                handler.BeginReceive(clientData.buffer, 0, ClientData.BufferSize, 0, new AsyncCallback(ReadCallback), clientData);
+            }
+            else handler.Close();
+
+            listener.BeginAccept(new AsyncCallback(AcceptCallback), listener);
+        }
+
+        public void ReadCallback(IAsyncResult ar)
+        {
+            ClientData cd = (ClientData)ar.AsyncState;
+            if (cd == null) return;
+
+            int bytes = 0;
+            try { bytes = cd.workSocket.EndReceive(ar); }
+            catch { }
+
+            if (bytes == 0)
+            {
+                cd.workSocket?.Close();
+
+                ClientSockets.Remove(cd);
+
+                ConnectionCallback(new ConnectData { ID = cd.ID, Connect = false, Count = ClientSockets.Count, Client = null });
+
+                return;
+            }
+
+            ReceiveCallback(new ReceiveData { ID = cd.ID, Buffer = cd.buffer, Size = bytes, Client = cd.workSocket });
+
+            try
+            {
+                cd.workSocket?.BeginReceive(cd.buffer, 0, ClientData.BufferSize, 0, new AsyncCallback(ReadCallback), cd);
+            }
+            catch { }
+        }
     }
-
-    public void AcceptCallback(IAsyncResult ar)
-    {
-      Socket listener = (Socket)ar.AsyncState;
-      if (listener == null) return;
-
-      Socket handler;
-
-      try { handler = listener.EndAccept(ar); }
-      catch{ ServerSocket?.Close(); Active = false; return; }
-
-      if (SocketLimit > ClientSockets.Count)
-      {
-        ClientData clientData = new ClientData();
-
-        clientData.ID = ++SocketID;
-
-        clientData.workSocket = handler;
-
-        ClientSockets.Add(clientData);
-
-        ConnectionCallback(new ConnectData { ID = clientData.ID, Connect = true, Count = ClientSockets.Count, Client = handler });
-
-        handler.BeginReceive(clientData.buffer, 0, ClientData.BufferSize, 0, new AsyncCallback(ReadCallback), clientData);
-      }
-      else handler.Close();
-
-      listener.BeginAccept(new AsyncCallback(AcceptCallback), listener);
-    }
-
-    public void ReadCallback(IAsyncResult ar)
-    {
-      ClientData cd = (ClientData)ar.AsyncState;
-      if (cd == null) return;
-
-      int bytes = 0;
-      try { bytes = cd.workSocket.EndReceive(ar); }
-      catch {  }
-
-      if (bytes == 0)
-      {
-        cd.workSocket?.Close();
-
-        ClientSockets.Remove(cd);
-
-        ConnectionCallback(new ConnectData { ID = cd.ID, Connect = false, Count = ClientSockets.Count, Client = null });
-
-        return;
-      }
-
-      ReceiveCallback(new ReceiveData { ID = cd.ID, Buffer = cd.buffer, Size = bytes, Client = cd.workSocket });
-
-      try
-      {
-        cd.workSocket?.BeginReceive(cd.buffer, 0, ClientData.BufferSize, 0, new AsyncCallback(ReadCallback), cd);
-      }
-      catch { }
-    }
-  }
 }

DroneSimulator/Screen2D.cs

@@ -1,186 +1,180 @@
-using System;
+using System.Numerics;
-using System.Collections.Generic;
-using System.Linq;
-using System.Numerics;
-using System.Reflection.Metadata;
-using System.Text;
-using System.Threading.Tasks;
 
 namespace DroneSimulator
 {
-  internal class Screen2D
+    internal class Screen2D
-  {
-
-    public delegate void DrawCallback(Bitmap bmp);
-
-    public Screen2D(DrawCallback callback)
     {
-      drawCallback = callback;
-    }
 
-    private class DroneInfo
+        public delegate void DrawCallback(Bitmap bmp);
-    {
-      public int ID;
-      public Color RGB;
-      public Bitmap? Drone;
-      public Point PosXY;
-      public int Height = 0;
 
-      public PointF TiltXY = new Point(0, 0);
+        public Screen2D(DrawCallback callback)
-      public int Azimuth = 0;
-    }
-
-    private DrawCallback drawCallback;
-    private Bitmap MainArea = new Bitmap(4096, 2560);
-    private List<DroneInfo> DroneList = new List<DroneInfo>();
-
-    public static Bitmap DrawQadro(Color ColorFill)
-    {
-      Bitmap drone = new Bitmap(130, 130);
-
-      using (Graphics g = Graphics.FromImage(drone))
-      {
-        g.PixelOffsetMode = System.Drawing.Drawing2D.PixelOffsetMode.HighSpeed;
-
-        SolidBrush solidBrush = new SolidBrush(ColorFill);
-
-        Point[] mid = { new Point(35, 65), new Point(70, 40), new Point(70, 90) };
-        g.FillPolygon(solidBrush, mid);
-
-        g.FillEllipse(solidBrush, new Rectangle(15, 15, 35, 35));
-        g.FillEllipse(solidBrush, new Rectangle(15, 80, 35, 35));
-        g.FillEllipse(solidBrush, new Rectangle(80, 80, 35, 35));
-        g.FillEllipse(solidBrush, new Rectangle(80, 15, 35, 35));
-
-        g.FillRectangle(solidBrush, new Rectangle(50, 60, 50, 10));
-
-        g.DrawEllipse(new Pen(ColorFill), new Rectangle(0, 0, 129, 129));
-
-        solidBrush.Dispose();
-      }
-
-      return RotateImage(drone, 90);
-    }
-
-    private static Bitmap RotateImage(Bitmap bmp, float angle)
-    {
-      Bitmap rotatedImage = new Bitmap(bmp.Width, bmp.Height);
-
-      using (Graphics g = Graphics.FromImage(rotatedImage))
-      {
-        // Set the rotation point to the center in the matrix
-        g.TranslateTransform(bmp.Width / 2, bmp.Height / 2);
-        // Rotate
-        g.RotateTransform(angle);
-        // Restore rotation point in the matrix
-        g.TranslateTransform(-bmp.Width / 2, -bmp.Height / 2);
-        // Draw the image on the bitmap
-        g.DrawImage(bmp, new Point(0, 0));
-      }
-
-      return rotatedImage;
-    }
-
-    public void CreateDrone(Color ColorFill, int ID)
-    {
-      DroneInfo info = new DroneInfo();
-      info.ID = ID;
-      info.RGB = ColorFill;
-      info.Drone = DrawQadro(ColorFill);
-
-      DroneList.Add(info);
-    }
-
-    public void RemoveDrone(int ID)
-    {
-      foreach (DroneInfo i in DroneList)
-      {
-        if (i.ID != ID) continue;
-        DroneList.Remove(i);
-        break;
-      }      
-    }
-
-    public void DrawScene()
-    {
-      using (Graphics g = Graphics.FromImage(MainArea))
-      {
-        g.Clear(Color.Gainsboro);
-        SolidBrush shadowBrush = new SolidBrush(Color.Black);
-        g.DrawRectangle(new Pen(Color.Black), new Rectangle { Width = MainArea.Width - 1, Height = MainArea.Height - 1 });
-
-        foreach (var d in DroneList)
         {
-          if (d.Azimuth >= 360) d.Azimuth -= 360;
+            drawCallback = callback;
-          var bmp = RotateImage(d.Drone, d.Azimuth);
-
-          g.FillEllipse(new SolidBrush(Color.FromArgb(50, d.RGB)), d.PosXY.X + d.Height, d.PosXY.Y + d.Height, 130, 130);
-
-          g.DrawLine(new Pen(Color.Black), new Point(d.PosXY.X + d.Drone.Width / 2, d.PosXY.Y + d.Drone.Height / 2), new Point(d.PosXY.X + d.Height + d.Drone.Width / 2, d.PosXY.Y + d.Height + d.Drone.Height / 2));
-
-          //g.DrawImage(bmp, new Rectangle(d.PosXY.X+32, d.PosXY.Y, 65, 130));
-
-          float x1 = 0, y1 = 0;
-          float x2 = 130, y2 = 0;
-          float x3 = 0, y3 = 130;
-
-          const float TO_RADI = MathF.PI / 180;
-
-          Quaternion tilt = new Quaternion(d.TiltXY.X, d.TiltXY.Y, 0, 0);
-          Quaternion rotate = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), d.Azimuth * TO_RADI);
-
-          tilt = tilt * rotate * rotate;
-
-          if (tilt.Y > 0)
-          {
-            x1 = (int)(Math.Sin(tilt.Y) * 130);
-            x3 = (int)(Math.Sin(tilt.Y) * 130);
-          }
-          else
-          {
-            x2 = (int)(Math.Cos(tilt.Y) * 130);
-          }
-
-          if (tilt.X > 0)
-          {
-            y1 = (int)(Math.Sin(tilt.X) * 130);
-            y2 = (int)(Math.Sin(tilt.X) * 130);
-          }
-          else
-          {
-            y3 = (int)(Math.Cos(tilt.X) * 130);
-          }
-
-          PointF ul = new PointF(d.PosXY.X + x1, d.PosXY.Y + y1); PointF ur = new PointF(d.PosXY.X + x2, d.PosXY.Y + y2);
-          PointF dl = new PointF(d.PosXY.X + x3, d.PosXY.Y + y3);
-          PointF[] dest = { ul, ur, dl };
-
-          g.DrawImage(bmp, dest);
         }
-      }
 
-      drawCallback(MainArea);
+        private class DroneInfo
+        {
+            public int ID;
+            public Color RGB;
+            public Bitmap? Drone;
+            public Point PosXY;
+            public int Height = 0;
+
+            public PointF TiltXY = new Point(0, 0);
+            public int Azimuth = 0;
+        }
+
+        private DrawCallback drawCallback;
+        private Bitmap MainArea = new Bitmap(4096, 2560);
+        private List<DroneInfo> DroneList = new List<DroneInfo>();
+
+        public static Bitmap DrawQadro(Color ColorFill)
+        {
+            Bitmap drone = new Bitmap(130, 130);
+
+            using (Graphics g = Graphics.FromImage(drone))
+            {
+                g.PixelOffsetMode = System.Drawing.Drawing2D.PixelOffsetMode.HighSpeed;
+
+                SolidBrush solidBrush = new SolidBrush(ColorFill);
+
+                Point[] mid = { new Point(35, 65), new Point(70, 40), new Point(70, 90) };
+                g.FillPolygon(solidBrush, mid);
+
+                g.FillEllipse(solidBrush, new Rectangle(15, 15, 35, 35));
+                g.FillEllipse(solidBrush, new Rectangle(15, 80, 35, 35));
+                g.FillEllipse(solidBrush, new Rectangle(80, 80, 35, 35));
+                g.FillEllipse(solidBrush, new Rectangle(80, 15, 35, 35));
+
+                g.FillRectangle(solidBrush, new Rectangle(50, 60, 50, 10));
+
+                g.DrawEllipse(new Pen(ColorFill), new Rectangle(0, 0, 129, 129));
+
+                solidBrush.Dispose();
+            }
+
+            return RotateImage(drone, 90);
+        }
+
+        private static Bitmap RotateImage(Bitmap bmp, float angle)
+        {
+            Bitmap rotatedImage = new Bitmap(bmp.Width, bmp.Height);
+
+            using (Graphics g = Graphics.FromImage(rotatedImage))
+            {
+                // Set the rotation point to the center in the matrix
+                g.TranslateTransform(bmp.Width / 2, bmp.Height / 2);
+                // Rotate
+                g.RotateTransform(angle);
+                // Restore rotation point in the matrix
+                g.TranslateTransform(-bmp.Width / 2, -bmp.Height / 2);
+                // Draw the image on the bitmap
+                g.DrawImage(bmp, new Point(0, 0));
+            }
+
+            return rotatedImage;
+        }
+
+        public void CreateDrone(Color ColorFill, int ID)
+        {
+            DroneInfo info = new DroneInfo();
+            info.ID = ID;
+            info.RGB = ColorFill;
+            info.Drone = DrawQadro(ColorFill);
+
+            DroneList.Add(info);
+        }
+
+        public void RemoveDrone(int ID)
+        {
+            foreach (DroneInfo i in DroneList)
+            {
+                if (i.ID != ID) continue;
+                DroneList.Remove(i);
+                break;
+            }
+        }
+
+        public void DrawScene()
+        {
+            using (Graphics g = Graphics.FromImage(MainArea))
+            {
+                g.Clear(Color.Gainsboro);
+                SolidBrush shadowBrush = new SolidBrush(Color.Black);
+                g.DrawRectangle(new Pen(Color.Black), new Rectangle { Width = MainArea.Width - 1, Height = MainArea.Height - 1 });
+
+                foreach (var d in DroneList)
+                {
+                    if (d.Azimuth >= 360) d.Azimuth -= 360;
+                    var bmp = RotateImage(d.Drone, d.Azimuth);
+
+                    g.FillEllipse(new SolidBrush(Color.FromArgb(50, d.RGB)), d.PosXY.X + d.Height, d.PosXY.Y + d.Height, 130, 130);
+
+                    g.DrawLine(new Pen(Color.Black), new Point(d.PosXY.X + d.Drone.Width / 2, d.PosXY.Y + d.Drone.Height / 2), new Point(d.PosXY.X + d.Height + d.Drone.Width / 2, d.PosXY.Y + d.Height + d.Drone.Height / 2));
+
+                    //g.DrawImage(bmp, new Rectangle(d.PosXY.X+32, d.PosXY.Y, 65, 130));
+
+                    float x1 = 0, y1 = 0;
+                    float x2 = 130, y2 = 0;
+                    float x3 = 0, y3 = 130;
+
+                    const float TO_RADI = MathF.PI / 180;
+
+                    Quaternion tilt = new Quaternion(d.TiltXY.X, d.TiltXY.Y, 0, 0);
+                    Quaternion rotate = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), d.Azimuth * TO_RADI);
+
+                    tilt = tilt * rotate * rotate;
+
+                    if (tilt.Y > 0)
+                    {
+                        x1 = (int)(Math.Sin(tilt.Y) * 130);
+                        x3 = (int)(Math.Sin(tilt.Y) * 130);
+                    }
+                    else
+                    {
+                        x2 = (int)(Math.Cos(tilt.Y) * 130);
+                    }
+
+                    if (tilt.X > 0)
+                    {
+                        y1 = (int)(Math.Sin(tilt.X) * 130);
+                        y2 = (int)(Math.Sin(tilt.X) * 130);
+                    }
+                    else
+                    {
+                        y3 = (int)(Math.Cos(tilt.X) * 130);
+                    }
+
+                    PointF ul = new PointF(d.PosXY.X + x1, d.PosXY.Y + y1); PointF ur = new PointF(d.PosXY.X + x2, d.PosXY.Y + y2);
+                    PointF dl = new PointF(d.PosXY.X + x3, d.PosXY.Y + y3);
+                    PointF[] dest = { ul, ur, dl };
+
+                    g.DrawImage(bmp, dest);
+                }
+            }
+
+            drawCallback(MainArea);
+        }
+
+        public void Move(int id, Vector3 pos, Vector4 tilt)
+        {
+            const float TO_GRAD = 180 / MathF.PI;
+            const float TO_RADI = MathF.PI / 180;
+
+            foreach (var d in DroneList)
+            {
+                if (d.ID != id) continue;
+
+                d.PosXY.X = (int)pos.X;
+                d.PosXY.Y = MainArea.Height - (int)pos.Y;
+                d.Height = (int)pos.Z;
+
+                d.TiltXY.X = tilt.X * TO_RADI;
+                d.TiltXY.Y = tilt.Y * TO_RADI;
+                d.Azimuth = (int)tilt.Z;
+
+                break;
+            }
+        }
     }
-
-    public void Move(int id, Vector3 pos, Vector4 tilt)
-    {
-      const float TO_GRAD = 180 / MathF.PI;
-      const float TO_RADI = MathF.PI / 180;
-
-      foreach (var d in DroneList)
-      {
-        if (d.ID != id) continue;
-
-        d.PosXY.X = (int)pos.X;
-        d.PosXY.Y = MainArea.Height - (int)pos.Y;
-        d.Height = (int)pos.Z;
-
-        d.TiltXY.X = tilt.X * TO_RADI;
-        d.TiltXY.Y = tilt.Y * TO_RADI;
-        d.Azimuth = (int)tilt.Z;
-
-        break;
-      }
-    }
-  }
 }