From acf6379279caad5e6de0ba810c4134f69c34f378 Mon Sep 17 00:00:00 2001 From: Dizel Date: Tue, 1 Apr 2025 14:27:21 +0300 Subject: [PATCH] =?UTF-8?q?=D0=A7=D0=B8=D1=81=D1=82=D0=BA=D0=B0=20=D0=BA?= =?UTF-8?q?=D0=BE=D0=B4=D0=B0=20=D0=BE=D1=82=20=D0=BC=D1=83=D1=81=D0=BE?= =?UTF-8?q?=D1=80=D0=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- DroneClient/Drone.cs | 117 ++++--- DroneClient/NetClient.cs | 195 ++++++------ DroneSimulator/Drone.cs | 472 ++++++++++++++--------------- DroneSimulator/NetServerClients.cs | 257 ++++++++-------- DroneSimulator/Screen2D.cs | 344 +++++++++++---------- 5 files changed, 679 insertions(+), 706 deletions(-) diff --git a/DroneClient/Drone.cs b/DroneClient/Drone.cs index a7619d3..33e2b4d 100644 --- a/DroneClient/Drone.cs +++ b/DroneClient/Drone.cs @@ -1,67 +1,62 @@ -using System; -using System.Collections.Generic; -using System.Linq; -using System.Runtime.InteropServices; -using System.Text; -using System.Threading.Tasks; +using System.Runtime.InteropServices; namespace DroneClient { - internal class Drone - { - public struct DataOut + internal class Drone { - public float AccX, AccY, AccZ; - public float GyrX, GyrY, GyrZ; - public float PosX, PosY; - public float LaserRange; + public struct DataOut + { + public float AccX, AccY, AccZ; + 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; - } - - } } diff --git a/DroneClient/NetClient.cs b/DroneClient/NetClient.cs index 27f8f61..ea1a28d 100644 --- a/DroneClient/NetClient.cs +++ b/DroneClient/NetClient.cs @@ -1,108 +1,103 @@ -using System; -using System.Collections.Generic; -using System.Linq; -using System.Text; -using System.Threading.Tasks; +using System.Net; using System.Net.Sockets; -using System.Net; namespace DroneSimulator { - internal class NetClient - { - public class ConnectData + internal class NetClient { - 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 { } - } - } } diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index df65b3a..3f056f8 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -1,262 +1,256 @@ -using System; -using System.Collections.Generic; -using System.Linq; -using System.Numerics; +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 + internal class Drone { - public float AccX, AccY, AccZ; - public float GyrX, GyrY, GyrZ; - public float PosX, PosY; - public float LaserRange; + 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 byte[] getBytes() - { - int size = Marshal.SizeOf(this); - byte[] arr = new byte[size]; + public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп + public float LaserRange; // Имитация: Дальномер - IntPtr ptr = IntPtr.Zero; - try + 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 { - ptr = Marshal.AllocHGlobal(size); - Marshal.StructureToPtr(this, ptr, true); - Marshal.Copy(ptr, arr, 0, size); - } - finally - { - Marshal.FreeHGlobal(ptr); - } - return arr; - } - } + public float AccX, AccY, AccZ; + public float GyrX, GyrY, GyrZ; + public float PosX, PosY; + public float LaserRange; - public struct DataIn - { - public float MotorUL, MotorUR, MotorDL, MotorDR; + public byte[] getBytes() + { + int size = Marshal.SizeOf(this); + byte[] arr = new byte[size]; - 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); + 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; + } } - 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) + public struct DataIn { - 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; - - Acc = new Vector3(0, 0, 1); - Gyr = Vector3.Zero; - LaserRange = 0; - } - else - { - /*if (ori.W < 0) + public Drone(int id) { - 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); - grav = (Quat * grav) * Quaternion.Inverse(Quat); - Acc = new Vector3(grav.X, grav.Y, grav.Z); + DroneThread = new Thread(new ThreadStart(ThreadFunction)); + Timer = Environment.TickCount; + 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); - else LaserRange = float.MaxValue; - } + 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; + } } - - 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; - } - } } diff --git a/DroneSimulator/NetServerClients.cs b/DroneSimulator/NetServerClients.cs index 0a6384d..f92a566 100644 --- a/DroneSimulator/NetServerClients.cs +++ b/DroneSimulator/NetServerClients.cs @@ -1,145 +1,140 @@ -using System; -using System.Collections.Generic; -using System.Linq; -using System.Text; -using System.Threading.Tasks; +using System.Net; using System.Net.Sockets; -using System.Net; namespace DroneSimulator { - internal class NetServerClients - { - public class ConnectData + internal class NetServerClients { - public int ID; - 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 ClientSockets = new List(); - - 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) + public class ConnectData { - try { c.workSocket?.Shutdown(SocketShutdown.Both); } catch { } - c.workSocket?.Close(); + public int ID; + public bool Connect; + public int Count; + + public Socket? Client; } - ClientSockets.Clear(); - return ServerState.Stop; - } - ConnectionCallback = Connection; - ReceiveCallback = Receive; + public class ReceiveData + { + public int ID; + public byte[]? Buffer; + public int Size; - SocketLimit = Limit; + public Socket? Client; + } - IPEndPoint ip = new(IPAddress.Any, Port); - ServerSocket = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp); + private class ClientData + { + public int ID; + public Socket? workSocket = null; + public const int BufferSize = 1024; + public byte[] buffer = new byte[BufferSize]; + } - try - { - ServerSocket.Bind(ip); - ServerSocket.Listen(10); - ServerSocket.BeginAccept(new AsyncCallback(AcceptCallback), ServerSocket); - Active = true; - } - catch { ServerSocket.Close(); return ServerState.Error; } + private int SocketID = 0; + private int SocketLimit; + private Socket? ServerSocket; + private List ClientSockets = new List(); - 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 { } - } - } } diff --git a/DroneSimulator/Screen2D.cs b/DroneSimulator/Screen2D.cs index f8f3854..d9dd5a0 100644 --- a/DroneSimulator/Screen2D.cs +++ b/DroneSimulator/Screen2D.cs @@ -1,186 +1,180 @@ -using System; -using System.Collections.Generic; -using System.Linq; -using System.Numerics; -using System.Reflection.Metadata; -using System.Text; -using System.Threading.Tasks; +using System.Numerics; namespace DroneSimulator { - internal class Screen2D - { - - public delegate void DrawCallback(Bitmap bmp); - - public Screen2D(DrawCallback callback) + internal class Screen2D { - drawCallback = callback; - } - private class DroneInfo - { - public int ID; - public Color RGB; - public Bitmap? Drone; - public Point PosXY; - public int Height = 0; + public delegate void DrawCallback(Bitmap bmp); - public PointF TiltXY = new Point(0, 0); - public int Azimuth = 0; - } - - private DrawCallback drawCallback; - private Bitmap MainArea = new Bitmap(4096, 2560); - private List DroneList = new List(); - - 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) + public Screen2D(DrawCallback callback) { - 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 = callback; } - } - 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 DroneList = new List(); + + 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; - } - } - } }