CPL/Simulator
5
0
Fork 3
Code Issues 1 Pull Requests Actions Packages Projects Releases Activity

Merge branch 'main' into devVlad

Browse Source
This commit is contained in:
Dizel 2025-04-04 13:13:16 +03:00
commit 099ec402cf
11 changed files with 961 additions and 735 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
.gitignore vendored
View File

@ -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
obj/
bin/

26
DroneClient/FormMain.cs
View File

@ -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;
}
}

192
DroneClient/NetClient.cs
View File

@ -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 { }
}
}
}

3
DroneClient/README.md Normal file
View File

@ -0,0 +1,3 @@
# Client
Образец клиента для подключения к симулятору.

523
DroneSimulator/Drone.cs
View File

@ -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;
}
}
}

56
DroneSimulator/FormMain.Designer.cs generated
View File

@ -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;
}
}

82
DroneSimulator/FormMain.cs
View File

@ -12,6 +12,7 @@ namespace DroneSimulator
Screen2D screen2D = null;
NetServerClients netServerClient = new NetServerClients();
NetServerVisual netServerVisual = new NetServerVisual();
List<Drone> AllDrones = new List<Drone>();
@ -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:
@ -146,5 +140,63 @@ namespace DroneSimulator
{
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;
}
}
}
}
}

266
DroneSimulator/NetServerClients.cs
View File

@ -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<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 { }
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<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 { }
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 { }
}
}
}

134
DroneSimulator/NetServerVisual.cs Normal file
View File

@ -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<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 { }
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 { }
}
}
}

352
DroneSimulator/Screen2D.cs
View File

@ -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<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;
}
}
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<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;
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;
}
}
}
}

1
www.txt Normal file
View File

@ -0,0 +1 @@
Hi!!!