forked from CPL/Simulator
Перенос в общую репу
This commit is contained in:
266
DroneSimulator/Drone.cs
Normal file
266
DroneSimulator/Drone.cs
Normal file
@ -0,0 +1,266 @@
|
||||
using System.Numerics;
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
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 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 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 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 / 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user