diff --git a/.gitignore b/.gitignore index b71a09d..98d63b2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,58 +1,3 @@ -# Файлы и папки Visual Studio -*.obj -*.exe -*.pdb -*.user -*.aps -*.pch -*.vspscc -*.vssscc -*_i.c -*_p.c -*.ncb -*.suo -*.tlb -*.tlh -*.bak -*.cache -*.ilk -*.log -*.sbr -*.scc -*.idb -*.tlog -*.ipch -*.db - -# Папки сборки -bin/ -obj/ -[Bb]in/ -[Oo]bj/ -Debug/ -Release/ -x64/ -x86/ -[Any CPU]/ - -# Файлы кэша и настройки VS -.vscode/ -*.code-workspace .vs/ -*.sln.docstates - -# NuGet -packages/ -*.nupkg -*.snupkg - -# Дополнительные временные файлы -Thumbs.db -*.DS_Store - -# Игнорировать файлы резервирования -~$* - -# Опционально: игнорировать файлы конфигурации пользователя -appsettings.json -*.config.user \ No newline at end of file +obj/ +bin/ \ No newline at end of file diff --git a/DroneClient/FormMain.cs b/DroneClient/FormMain.cs index d479e1a..dee166e 100644 --- a/DroneClient/FormMain.cs +++ b/DroneClient/FormMain.cs @@ -112,37 +112,39 @@ namespace DroneSimulator private void button_UU_MouseDown(object sender, MouseEventArgs e) { + const float pow = 0.1f; + if (sender == button_UU) { - sendDrone.MotorUL -= 0.1f; sendDrone.MotorUR -= 0.1f; - sendDrone.MotorDL += 0.1f; sendDrone.MotorDR += 0.1f; + sendDrone.MotorUL -= pow; sendDrone.MotorUR -= pow; + sendDrone.MotorDL += pow; sendDrone.MotorDR += pow; } if (sender == button_DD) { - sendDrone.MotorUL += 0.1f; sendDrone.MotorUR += 0.1f; - sendDrone.MotorDL -= 0.1f; sendDrone.MotorDR -= 0.1f; + sendDrone.MotorUL += pow; sendDrone.MotorUR += pow; + sendDrone.MotorDL -= pow; sendDrone.MotorDR -= pow; } if (sender == button_LL) { - sendDrone.MotorUL -= 0.1f; sendDrone.MotorUR += 0.1f; - sendDrone.MotorDL -= 0.1f; sendDrone.MotorDR += 0.1f; + sendDrone.MotorUL -= pow; sendDrone.MotorUR += pow; + sendDrone.MotorDL -= pow; sendDrone.MotorDR += pow; } if (sender == button_RR) { - sendDrone.MotorUL += 0.1f; sendDrone.MotorUR -= 0.1f; - sendDrone.MotorDL += 0.1f; sendDrone.MotorDR -= 0.1f; + sendDrone.MotorUL += pow; sendDrone.MotorUR -= pow; + sendDrone.MotorDL += pow; sendDrone.MotorDR -= pow; } if (sender == button_ML) { - sendDrone.MotorUL -= 0.1f; sendDrone.MotorUR += 0.1f; - sendDrone.MotorDL += 0.1f; sendDrone.MotorDR -= 0.1f; + sendDrone.MotorUL -= pow; sendDrone.MotorUR += pow; + sendDrone.MotorDL += pow; sendDrone.MotorDR -= pow; } if (sender == button_MR) { - sendDrone.MotorUL += 0.1f; sendDrone.MotorUR -= 0.1f; - sendDrone.MotorDL -= 0.1f; sendDrone.MotorDR += 0.1f; + sendDrone.MotorUL += pow; sendDrone.MotorUR -= pow; + sendDrone.MotorDL -= pow; sendDrone.MotorDR += pow; } } diff --git a/DroneClient/NetClient.cs b/DroneClient/NetClient.cs index ea1a28d..f343708 100644 --- a/DroneClient/NetClient.cs +++ b/DroneClient/NetClient.cs @@ -1,103 +1,103 @@ -using System.Net; -using System.Net.Sockets; +using System.Net.Sockets; +using System.Net; namespace DroneSimulator { - internal class NetClient + internal class NetClient + { + public class ConnectData { - public class ConnectData - { - public bool Connect; + public bool Connect; - 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 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 { } + } + } } diff --git a/DroneClient/README.md b/DroneClient/README.md new file mode 100644 index 0000000..facae82 --- /dev/null +++ b/DroneClient/README.md @@ -0,0 +1,3 @@ +# Client + +Образец клиента для подключения к симулятору. diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index 3f056f8..45d0e39 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -3,254 +3,283 @@ using System.Runtime.InteropServices; namespace DroneSimulator { - internal class Drone + internal class Drone + { + public int ID; + public float Mass; // Масса + public bool Active; // Живой? + public float Length; // Длинна лучей + public const float Dynamic = 10; // Динамика вращения + public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение + public Quaternion Quat; // Основной кватернион + public float Power = 0; // Тяга всех двигателей + public float MaxPower; // Тяга всех двигателей + public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw + + public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп + public float LaserRange; // Имитация: Дальномер + + private const float Gravity = 9.8f; + + 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 static byte[] getBytes(object data) { - 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 + int size = Marshal.SizeOf(data); + byte[] arr = new byte[size]; - 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; - } + 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 DataOut + { + public float AccX, AccY, AccZ; + public float GyrX, GyrY, GyrZ; + public float PosX, PosY; + 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; + } + + public struct DataVisual + { + public int ID; // Идентификатор + public float W, X, Y, Z; // Кватернион вращения + public float PosX, PosY, PosZ; // Координаты в пространстве + } + + public DataVisual GetVisual() + { + return new DataVisual() { ID = this.ID, W = this.Quat.W, X = this.Quat.X, Y = this.Quat.Y, Z = this.Quat.Z, PosX = this.PosXYZ.X, PosY = this.PosXYZ.Y, PosZ = this.PosXYZ.Z }; + } + + private void ThreadFunction() + { + while (DroneThread != null) + { + float time = Environment.TickCount - Timer; + Timer = Environment.TickCount; + Action(time / 1000); + Thread.Sleep(1); + } + } + + public Drone(int id) + { + ID = id; + Active = false; + PosXYZ = Vector3.Zero; + SpdXYZ = Vector3.Zero; + AccXYZ = Vector3.Zero; + Quat = Quaternion.Identity; + + DroneThread = new Thread(new ThreadStart(ThreadFunction)); + Timer = Environment.TickCount; + DroneThread.Start(); + } + + public int Create(float power, float mass, float len) + { + Mass = mass; + Length = len; + MaxPower = power; + + 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 < 0.3f) + { + flow += flow * 0.1f; // Воздушная подушка + } + + SpdPRY += AccPRY * (Dynamic * time / (Mass * Length)); + + Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat; + AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) * (Gravity / Mass); + + SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time; + PosXYZ += SpdXYZ * time; + + AccXYZ /= Gravity; // Вернуть измерения в G + + if (PosXYZ.Z < 0) + { + SpdPRY = Vector3.Zero; + SpdXYZ.X = 0; + SpdXYZ.Y = 0; + Quat = Quaternion.Identity; + } + else Rotate(SpdPRY.X * time, SpdPRY.Y * time, SpdPRY.Z * time); + + Vector4 ori = GetOrientation(); + + if (PosXYZ.Z < 0) + { + PosXYZ.Z = 0; + + /*if (SpdXYZ.Z < -5) + { + Active = false; // Сильно ударился о землю + }*/ + + /*if (MathF.Abs(ori.X) > 20 || MathF.Abs(ori.Y) > 20) + { + 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 * MaxPower; + } + + 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/FormMain.Designer.cs b/DroneSimulator/FormMain.Designer.cs index 2997b8d..bab9e3b 100644 --- a/DroneSimulator/FormMain.Designer.cs +++ b/DroneSimulator/FormMain.Designer.cs @@ -38,6 +38,10 @@ tabControl_Menu = new TabControl(); tabPage_Main = new TabPage(); groupBox_Visual = new GroupBox(); + numericUpDown_Visual_Limit = new NumericUpDown(); + label1 = new Label(); + label_Visual_Num = new Label(); + label3 = new Label(); button_Visual_Start = new Button(); numericUpDown_Visual_Port = new NumericUpDown(); label_Visual_Port = new Label(); @@ -63,6 +67,7 @@ tabControl_Menu.SuspendLayout(); tabPage_Main.SuspendLayout(); groupBox_Visual.SuspendLayout(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Visual_Limit).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Visual_Port).BeginInit(); groupBox_Clients.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Clients_Limit).BeginInit(); @@ -144,18 +149,61 @@ // // groupBox_Visual // + groupBox_Visual.Controls.Add(numericUpDown_Visual_Limit); + groupBox_Visual.Controls.Add(label1); + groupBox_Visual.Controls.Add(label_Visual_Num); + groupBox_Visual.Controls.Add(label3); groupBox_Visual.Controls.Add(button_Visual_Start); groupBox_Visual.Controls.Add(numericUpDown_Visual_Port); groupBox_Visual.Controls.Add(label_Visual_Port); groupBox_Visual.Dock = DockStyle.Top; groupBox_Visual.Location = new Point(3, 83); groupBox_Visual.Name = "groupBox_Visual"; - groupBox_Visual.Size = new Size(204, 62); + groupBox_Visual.Size = new Size(204, 91); groupBox_Visual.TabIndex = 2; groupBox_Visual.TabStop = false; groupBox_Visual.Tag = "#visual"; groupBox_Visual.Text = "Visual"; // + // numericUpDown_Visual_Limit + // + numericUpDown_Visual_Limit.Location = new Point(44, 57); + numericUpDown_Visual_Limit.Maximum = new decimal(new int[] { 10, 0, 0, 0 }); + numericUpDown_Visual_Limit.Minimum = new decimal(new int[] { 1, 0, 0, 0 }); + numericUpDown_Visual_Limit.Name = "numericUpDown_Visual_Limit"; + numericUpDown_Visual_Limit.Size = new Size(42, 23); + numericUpDown_Visual_Limit.TabIndex = 13; + numericUpDown_Visual_Limit.Value = new decimal(new int[] { 1, 0, 0, 0 }); + // + // label1 + // + label1.AutoSize = true; + label1.Location = new Point(6, 59); + label1.Name = "label1"; + label1.Size = new Size(37, 15); + label1.TabIndex = 12; + label1.Tag = "#clients_limit"; + label1.Text = "Limit:"; + // + // label_Visual_Num + // + label_Visual_Num.AutoSize = true; + label_Visual_Num.Location = new Point(161, 59); + label_Visual_Num.Name = "label_Visual_Num"; + label_Visual_Num.Size = new Size(13, 15); + label_Visual_Num.TabIndex = 11; + label_Visual_Num.Text = "0"; + // + // label3 + // + label3.AutoSize = true; + label3.Location = new Point(112, 59); + label3.Name = "label3"; + label3.Size = new Size(43, 15); + label3.TabIndex = 10; + label3.Tag = "#clients_count"; + label3.Text = "Count:"; + // // button_Visual_Start // button_Visual_Start.Location = new Point(112, 22); @@ -165,6 +213,7 @@ button_Visual_Start.Tag = "#visual_start"; button_Visual_Start.Text = "Start"; button_Visual_Start.UseVisualStyleBackColor = true; + button_Visual_Start.Click += button_Visual_Start_Click; // // numericUpDown_Visual_Port // @@ -376,6 +425,7 @@ tabPage_Main.ResumeLayout(false); groupBox_Visual.ResumeLayout(false); groupBox_Visual.PerformLayout(); + ((System.ComponentModel.ISupportInitialize)numericUpDown_Visual_Limit).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Visual_Port).EndInit(); groupBox_Clients.ResumeLayout(false); groupBox_Clients.PerformLayout(); @@ -416,5 +466,9 @@ private Panel panel1; private ToolStripMenuItem exitToolStripMenuItem; private System.Windows.Forms.Timer timer_Test; + private NumericUpDown numericUpDown_Visual_Limit; + private Label label1; + private Label label_Visual_Num; + private Label label3; } } diff --git a/DroneSimulator/FormMain.cs b/DroneSimulator/FormMain.cs index 9e6489a..862c70c 100644 --- a/DroneSimulator/FormMain.cs +++ b/DroneSimulator/FormMain.cs @@ -12,6 +12,7 @@ namespace DroneSimulator Screen2D screen2D = null; NetServerClients netServerClient = new NetServerClients(); + NetServerVisual netServerVisual = new NetServerVisual(); List AllDrones = new List(); @@ -20,7 +21,7 @@ namespace DroneSimulator InitializeComponent(); } - private void ConnectionCallback(object o) + private void ClientConnectionCallback(object o) { NetServerClients.ConnectData data = (NetServerClients.ConnectData)o; @@ -32,7 +33,7 @@ namespace DroneSimulator if (data.Connect) { Drone drone = new Drone(data.ID); - drone.Create(0.5f, 10.0f); + drone.Create(1.0f, 0.5f, 1.0f); screen2D.CreateDrone(Color.Red, data.ID); @@ -53,7 +54,7 @@ namespace DroneSimulator } } - private void ReceiveCallback(object o) + private void ClientReceiveCallback(object o) { NetServerClients.ReceiveData data = (NetServerClients.ReceiveData)o; @@ -68,31 +69,24 @@ namespace DroneSimulator if (drone == null) return; - Drone.DataIn id = new Drone.DataIn(); - - id.fromBytes(data.Buffer); + Drone.DataIn id = (Drone.DataIn)Drone.fromBytes(data.Buffer, typeof(Drone.DataIn)); drone.SetQadroPow(id.MotorUL, id.MotorUR, id.MotorDL, id.MotorDR); - Drone.DataOut od = new Drone.DataOut(); + Drone.DataOut od = drone.GetDataOut(); - od.AccX= drone.Acc.X; od.AccY= drone.Acc.Y; od.AccZ= drone.Acc.Z; - od.GyrX = drone.Gyr.X; od.GyrY = drone.Gyr.Y; od.GyrZ = drone.Gyr.Z; - od.PosX= drone.PosXYZ.X; od.PosY = drone.PosXYZ.Y; - od.LaserRange = drone.LaserRange; - - try { data.Client.Send(od.getBytes()); } + try { data.Client.Send(Drone.getBytes(od)); } catch { } } private void button_Client_Start_Click(object sender, EventArgs e) { - var done = netServerClient.StartServer((int)numericUpDown_Clients_Port.Value, (int)numericUpDown_Clients_Limit.Value, ConnectionCallback, ReceiveCallback); + var done = netServerClient.StartServer((int)numericUpDown_Clients_Port.Value, (int)numericUpDown_Clients_Limit.Value, ClientConnectionCallback, ClientReceiveCallback); switch (done) { case NetServerClients.ServerState.Error: { - MessageBox.Show("Error to start server"); + MessageBox.Show("Error to start clients server", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error); break; } case NetServerClients.ServerState.Start: @@ -139,12 +133,70 @@ namespace DroneSimulator { screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation()); } - + screen2D.DrawScene(); } private void Form_Main_FormClosing(object sender, FormClosingEventArgs e) { foreach (Drone d in AllDrones) d.Close(); } + + private void VisualConnectionCallback(object o) + { + NetServerVisual.ConnectData data = (NetServerVisual.ConnectData)o; + + Invoke((MethodInvoker)delegate + { + label_Clients_Num.Text = data.Count.ToString(); + }); + + if (data.Connect) + { + //--- + } + else + { + //--- + } + } + + private void VisualReceiveCallback(object o) + { + NetServerVisual.ReceiveData data = (NetServerVisual.ReceiveData)o; + + foreach (Drone d in AllDrones) + { + Drone.DataVisual v = d.GetVisual(); + + try { data.Client.Send(Drone.getBytes(v)); } + catch { } + } + } + + private void button_Visual_Start_Click(object sender, EventArgs e) + { + var done = netServerVisual.StartServer((int)numericUpDown_Visual_Port.Value, (int)numericUpDown_Visual_Limit.Value, VisualConnectionCallback, VisualReceiveCallback); + switch (done) + { + case NetServerVisual.ServerState.Error: + { + MessageBox.Show("Error to start visual server", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error); + break; + } + case NetServerVisual.ServerState.Start: + { + button_Visual_Start.Text = "Stop"; + button_Visual_Start.BackColor = Color.LimeGreen; + break; + } + case NetServerVisual.ServerState.Stop: + { + label_Visual_Num.Text = "0"; + button_Visual_Start.Text = "Start"; + button_Visual_Start.BackColor = Color.Transparent; + break; + } + } + } } } diff --git a/DroneSimulator/NetServerClients.cs b/DroneSimulator/NetServerClients.cs index f92a566..8c28f73 100644 --- a/DroneSimulator/NetServerClients.cs +++ b/DroneSimulator/NetServerClients.cs @@ -1,140 +1,140 @@ -using System.Net; -using System.Net.Sockets; +using System.Net.Sockets; +using System.Net; namespace DroneSimulator { - internal class NetServerClients + internal class NetServerClients + { + public class ConnectData { - public class ConnectData - { - public int ID; - public bool Connect; - public int Count; + 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) - { - 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 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) + { + 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 { } + } + } } diff --git a/DroneSimulator/NetServerVisual.cs b/DroneSimulator/NetServerVisual.cs new file mode 100644 index 0000000..41dd579 --- /dev/null +++ b/DroneSimulator/NetServerVisual.cs @@ -0,0 +1,134 @@ +using System.Net.Sockets; +using System.Net; + +namespace DroneSimulator +{ + internal class NetServerVisual + { + public class ConnectData + { + public bool Connect; + public int Count; + + public Socket? Client; + } + + public class ReceiveData + { + public byte[]? Buffer; + public int Size; + + public Socket? Client; + } + + private class ClientData + { + public Socket? workSocket = null; + public const int BufferSize = 1024; + public byte[] buffer = new byte[BufferSize]; + } + + 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) + { + 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.workSocket = handler; + + ClientSockets.Add(clientData); + + ConnectionCallback(new ConnectData { 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 { Connect = false, Count = ClientSockets.Count, Client = null }); + + return; + } + + ReceiveCallback(new ReceiveData { 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 d9dd5a0..c967b1c 100644 --- a/DroneSimulator/Screen2D.cs +++ b/DroneSimulator/Screen2D.cs @@ -2,179 +2,185 @@ namespace DroneSimulator { - internal class Screen2D + internal class Screen2D + { + + public delegate void DrawCallback(Bitmap bmp); + + public Screen2D(DrawCallback callback) { - - public delegate void DrawCallback(Bitmap bmp); - - public Screen2D(DrawCallback callback) - { - drawCallback = callback; - } - - 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; - } - } + drawCallback = callback; } + + 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 float Scale = 100; + 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; + + pos *= Scale; + + pos.X += MainArea.Width / 2; + pos.Y += MainArea.Height / 2; + + 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; + } + } + } } diff --git a/www.txt b/www.txt new file mode 100644 index 0000000..685dda7 --- /dev/null +++ b/www.txt @@ -0,0 +1 @@ +Hi!!! \ No newline at end of file