Чистка кода от мусора
This commit is contained in:
parent
eeb7c5764f
commit
acf6379279
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -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 { }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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<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)
|
||||
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<ClientData> ClientSockets = new List<ClientData>();
|
||||
|
||||
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 { }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -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<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)
|
||||
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<DroneInfo> DroneList = new List<DroneInfo>();
|
||||
|
||||
public static Bitmap DrawQadro(Color ColorFill)
|
||||
{
|
||||
Bitmap drone = new Bitmap(130, 130);
|
||||
|
||||
using (Graphics g = Graphics.FromImage(drone))
|
||||
{
|
||||
g.PixelOffsetMode = System.Drawing.Drawing2D.PixelOffsetMode.HighSpeed;
|
||||
|
||||
SolidBrush solidBrush = new SolidBrush(ColorFill);
|
||||
|
||||
Point[] mid = { new Point(35, 65), new Point(70, 40), new Point(70, 90) };
|
||||
g.FillPolygon(solidBrush, mid);
|
||||
|
||||
g.FillEllipse(solidBrush, new Rectangle(15, 15, 35, 35));
|
||||
g.FillEllipse(solidBrush, new Rectangle(15, 80, 35, 35));
|
||||
g.FillEllipse(solidBrush, new Rectangle(80, 80, 35, 35));
|
||||
g.FillEllipse(solidBrush, new Rectangle(80, 15, 35, 35));
|
||||
|
||||
g.FillRectangle(solidBrush, new Rectangle(50, 60, 50, 10));
|
||||
|
||||
g.DrawEllipse(new Pen(ColorFill), new Rectangle(0, 0, 129, 129));
|
||||
|
||||
solidBrush.Dispose();
|
||||
}
|
||||
|
||||
return RotateImage(drone, 90);
|
||||
}
|
||||
|
||||
private static Bitmap RotateImage(Bitmap bmp, float angle)
|
||||
{
|
||||
Bitmap rotatedImage = new Bitmap(bmp.Width, bmp.Height);
|
||||
|
||||
using (Graphics g = Graphics.FromImage(rotatedImage))
|
||||
{
|
||||
// Set the rotation point to the center in the matrix
|
||||
g.TranslateTransform(bmp.Width / 2, bmp.Height / 2);
|
||||
// Rotate
|
||||
g.RotateTransform(angle);
|
||||
// Restore rotation point in the matrix
|
||||
g.TranslateTransform(-bmp.Width / 2, -bmp.Height / 2);
|
||||
// Draw the image on the bitmap
|
||||
g.DrawImage(bmp, new Point(0, 0));
|
||||
}
|
||||
|
||||
return rotatedImage;
|
||||
}
|
||||
|
||||
public void CreateDrone(Color ColorFill, int ID)
|
||||
{
|
||||
DroneInfo info = new DroneInfo();
|
||||
info.ID = ID;
|
||||
info.RGB = ColorFill;
|
||||
info.Drone = DrawQadro(ColorFill);
|
||||
|
||||
DroneList.Add(info);
|
||||
}
|
||||
|
||||
public void RemoveDrone(int ID)
|
||||
{
|
||||
foreach (DroneInfo i in DroneList)
|
||||
{
|
||||
if (i.ID != ID) continue;
|
||||
DroneList.Remove(i);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
public void DrawScene()
|
||||
{
|
||||
using (Graphics g = Graphics.FromImage(MainArea))
|
||||
{
|
||||
g.Clear(Color.Gainsboro);
|
||||
SolidBrush shadowBrush = new SolidBrush(Color.Black);
|
||||
g.DrawRectangle(new Pen(Color.Black), new Rectangle { Width = MainArea.Width - 1, Height = MainArea.Height - 1 });
|
||||
|
||||
foreach (var d in DroneList)
|
||||
{
|
||||
if (d.Azimuth >= 360) d.Azimuth -= 360;
|
||||
var bmp = RotateImage(d.Drone, d.Azimuth);
|
||||
|
||||
g.FillEllipse(new SolidBrush(Color.FromArgb(50, d.RGB)), d.PosXY.X + d.Height, d.PosXY.Y + d.Height, 130, 130);
|
||||
|
||||
g.DrawLine(new Pen(Color.Black), new Point(d.PosXY.X + d.Drone.Width / 2, d.PosXY.Y + d.Drone.Height / 2), new Point(d.PosXY.X + d.Height + d.Drone.Width / 2, d.PosXY.Y + d.Height + d.Drone.Height / 2));
|
||||
|
||||
//g.DrawImage(bmp, new Rectangle(d.PosXY.X+32, d.PosXY.Y, 65, 130));
|
||||
|
||||
float x1 = 0, y1 = 0;
|
||||
float x2 = 130, y2 = 0;
|
||||
float x3 = 0, y3 = 130;
|
||||
|
||||
const float TO_RADI = MathF.PI / 180;
|
||||
|
||||
Quaternion tilt = new Quaternion(d.TiltXY.X, d.TiltXY.Y, 0, 0);
|
||||
Quaternion rotate = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), d.Azimuth * TO_RADI);
|
||||
|
||||
tilt = tilt * rotate * rotate;
|
||||
|
||||
if (tilt.Y > 0)
|
||||
{
|
||||
x1 = (int)(Math.Sin(tilt.Y) * 130);
|
||||
x3 = (int)(Math.Sin(tilt.Y) * 130);
|
||||
}
|
||||
else
|
||||
{
|
||||
x2 = (int)(Math.Cos(tilt.Y) * 130);
|
||||
}
|
||||
|
||||
if (tilt.X > 0)
|
||||
{
|
||||
y1 = (int)(Math.Sin(tilt.X) * 130);
|
||||
y2 = (int)(Math.Sin(tilt.X) * 130);
|
||||
}
|
||||
else
|
||||
{
|
||||
y3 = (int)(Math.Cos(tilt.X) * 130);
|
||||
}
|
||||
|
||||
PointF ul = new PointF(d.PosXY.X + x1, d.PosXY.Y + y1); PointF ur = new PointF(d.PosXY.X + x2, d.PosXY.Y + y2);
|
||||
PointF dl = new PointF(d.PosXY.X + x3, d.PosXY.Y + y3);
|
||||
PointF[] dest = { ul, ur, dl };
|
||||
|
||||
g.DrawImage(bmp, dest);
|
||||
}
|
||||
}
|
||||
|
||||
drawCallback(MainArea);
|
||||
}
|
||||
|
||||
public void Move(int id, Vector3 pos, Vector4 tilt)
|
||||
{
|
||||
const float TO_GRAD = 180 / MathF.PI;
|
||||
const float TO_RADI = MathF.PI / 180;
|
||||
|
||||
foreach (var d in DroneList)
|
||||
{
|
||||
if (d.ID != id) continue;
|
||||
|
||||
d.PosXY.X = (int)pos.X;
|
||||
d.PosXY.Y = MainArea.Height - (int)pos.Y;
|
||||
d.Height = (int)pos.Z;
|
||||
|
||||
d.TiltXY.X = tilt.X * TO_RADI;
|
||||
d.TiltXY.Y = tilt.Y * TO_RADI;
|
||||
d.Azimuth = (int)tilt.Z;
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void Move(int id, Vector3 pos, Vector4 tilt)
|
||||
{
|
||||
const float TO_GRAD = 180 / MathF.PI;
|
||||
const float TO_RADI = MathF.PI / 180;
|
||||
|
||||
foreach (var d in DroneList)
|
||||
{
|
||||
if (d.ID != id) continue;
|
||||
|
||||
d.PosXY.X = (int)pos.X;
|
||||
d.PosXY.Y = MainArea.Height - (int)pos.Y;
|
||||
d.Height = (int)pos.Z;
|
||||
|
||||
d.TiltXY.X = tilt.X * TO_RADI;
|
||||
d.TiltXY.Y = tilt.Y * TO_RADI;
|
||||
d.Azimuth = (int)tilt.Z;
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user