CPL/Simulator
6
0
Fork 2
Code Issues 1 Pull Requests Actions Packages Projects Releases Activity

Mag active

Browse Source
This commit is contained in:
Sergey Sklyarov
2025-07-26 02:35:15 +03:00
parent 65011e65ee
commit bb32111f29
4 changed files with 732 additions and 670 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
DroneSimulator/Drone.cs
View File

@ -62,7 +62,7 @@ namespace DroneSimulator
private RealMode.Barometer RealBar = new RealMode.Barometer(); private RealMode.Barometer RealBar = new RealMode.Barometer();
private RealMode.Range RealRange = new RealMode.Range(); private RealMode.Range RealRange = new RealMode.Range();
private RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow(); private RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow();
private RealMode.Magnetometer RealMagnetometer = new RealMode.Magnetometer(); private RealMode.Magnetometer RealMag = new RealMode.Magnetometer();
public static byte[] getBytes(object data) public static byte[] getBytes(object data)
{ {
@ -373,7 +373,7 @@ namespace DroneSimulator
bool of = RealOF.Update(of_xy, LaserRange, tick); bool of = RealOF.Update(of_xy, LaserRange, tick);
RealMagnetometer.Update(Quat, tick); RealMag.Update(Quat, tick);
lock (this) lock (this)
{ {
@ -481,8 +481,8 @@ namespace DroneSimulator
mag.Head.Type = DroneData.DataType.DataMag; mag.Head.Type = DroneData.DataType.DataMag;
mag.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000); mag.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0; mag.Mag.X = RealMag.result.X; mag.Mag.Y = RealMag.result.Y; mag.Mag.Z = RealMag.result.Z;
mag.Time = Timer; mag.Time = RealMag.timer;
return getBytes(mag); return getBytes(mag);
} }

122
DroneSimulator/GPS.cs
View File

@ -8,67 +8,67 @@ using System.Threading.Tasks;
namespace DroneSimulator namespace DroneSimulator
{ {
internal class GPS internal class GPS
{
static double PI = 3.14159265358979323846;
public struct Home
{ {
static double PI = 3.14159265358979323846; public static double Lat, Lon;
public struct Home public static float Alt;
{
public static double Lat, Lon;
public static float Alt;
}
public struct State
{
public static byte Fix; // Тип решения 0-8 (NMEA Fix type)
public static byte SatVisible; // Количество видимых спутников
public static byte SatUsed; // Количество используемых спутников
public static float Hdop, Vdop, Pdop; // Геометрический фактор
public static float Noise; // Шум (db)
}
public struct GlobalCoords
{
public double latitude, longitude;
}
public struct Point
{
public double x, y;
}
// Конвертация градусов в радианы
static double deg2rad(double deg)
{
return deg * PI / 180.0;
}
// Конвертация радиан в градусы
static double rad2deg(double rad)
{
return rad * 180.0 / PI;
}
// Перевод локальных координат в глобальные
public static GlobalCoords localToGlobal(Point local, GlobalCoords origin)
{
const double er = 6371000; // Radius of the earth in m
// Преобразование приращений координат
double dLat = local.x / er; // В радианах
double originLatRad = deg2rad(origin.latitude);
// Вычисление новой широты
double newLatRad = originLatRad + dLat;
double newLat = rad2deg(newLatRad);
// Вычисление новой долготы (с использованием средней широты для точности)
double avgLatRad = (originLatRad + newLatRad) / 2.0;
double dLon = local.y / (er * Math.Cos(avgLatRad)); // В радианах
double newLon = origin.longitude + rad2deg(dLon);
GlobalCoords coord = new GlobalCoords();
coord.latitude = newLat;
coord.longitude = newLon;
return coord;
}
} }
public struct State
{
public static byte Fix; // Тип решения 0-8 (NMEA Fix type)
public static byte SatVisible; // Количество видимых спутников
public static byte SatUsed; // Количество используемых спутников
public static float Hdop, Vdop, Pdop; // Геометрический фактор
public static float Noise; // Шум (db)
}
public struct GlobalCoords
{
public double latitude, longitude;
}
public struct Point
{
public double x, y;
}
// Конвертация градусов в радианы
static double deg2rad(double deg)
{
return deg * PI / 180.0;
}
// Конвертация радиан в градусы
static double rad2deg(double rad)
{
return rad * 180.0 / PI;
}
// Перевод локальных координат в глобальные
public static GlobalCoords localToGlobal(Point local, GlobalCoords origin)
{
const double er = 6371000; // Radius of the earth in m
// Преобразование приращений координат
double dLat = local.x / er; // В радианах
double originLatRad = deg2rad(origin.latitude);
// Вычисление новой широты
double newLatRad = originLatRad + dLat;
double newLat = rad2deg(newLatRad);
// Вычисление новой долготы (с использованием средней широты для точности)
double avgLatRad = (originLatRad + newLatRad) / 2.0;
double dLon = local.y / (er * Math.Cos(avgLatRad)); // В радианах
double newLon = origin.longitude + rad2deg(dLon);
GlobalCoords coord = new GlobalCoords();
coord.latitude = newLat;
coord.longitude = newLon;
return coord;
}
}
} }

960
DroneSimulator/RealMode.cs
View File

@ -1,4 +1,5 @@
using System; using Microsoft.VisualBasic.Devices;
using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.Linq; using System.Linq;
using System.Numerics; using System.Numerics;
@ -9,464 +10,525 @@ using static System.Windows.Forms.VisualStyles.VisualStyleElement.TaskbarClock;
namespace DroneSimulator namespace DroneSimulator
{ {
internal class RealMode internal class RealMode
{
internal class Accelerometer
{ {
public static uint Freq;
public static float Noise;
public static float ScaleLeft;
public static float ScaleRight;
public static float Lateness;
public static bool RealSimulation;
private uint last = 0;
internal class Accelerometer private Random rand = new Random();
private const int count = 1000;
private Vector3[] laten = new Vector3[count];
private int index = 0;
public uint timer = 0;
public Vector3 result;
public void Update(Vector3 value, uint time)
{
if (!RealSimulation)
{ {
public static uint Freq; result = value;
public static float Noise; timer = time;
public static float ScaleLeft; return;
public static float ScaleRight;
public static float Lateness;
public static bool RealSimulation;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private Vector3[] laten = new Vector3[count];
private int index = 0;
public uint timer = 0;
public Vector3 result;
public void Update(Vector3 value, uint time)
{
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
float scale = (ScaleRight - ScaleLeft) / 2;
float shift = scale + ScaleLeft;
value.X = (value.X * scale) + shift;
value.Y = (value.Y * scale) + shift;
value.Z = (value.Z * scale) + shift;
int noise = (int)(Noise * 1000);
value.X += ((float)rand.Next(-noise, noise)) / 1000;
value.Y += ((float)rand.Next(-noise, noise)) / 1000;
value.Z += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = time - last;
while (true)
{
laten[index] = value;
clock--;
if (clock == 0) break;
index++;
if (index >= count) index = 0;
}
last = time;
int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
value = laten[move];
uint freq = 1000 / Freq;
if (timer + freq <= time)
{
result = value;
timer = time;
}
}
} }
internal class Gyroscope float scale = (ScaleRight - ScaleLeft) / 2;
float shift = scale + ScaleLeft;
value.X = (value.X * scale) + shift;
value.Y = (value.Y * scale) + shift;
value.Z = (value.Z * scale) + shift;
int noise = (int)(Noise * 1000);
value.X += ((float)rand.Next(-noise, noise)) / 1000;
value.Y += ((float)rand.Next(-noise, noise)) / 1000;
value.Z += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = time - last;
while (true)
{ {
public static uint Freq; laten[index] = value;
public static float Noise; clock--;
public static Vector3 Shift; if (clock == 0) break;
public static float Lateness; index++;
public static bool RealSimulation; if (index >= count) index = 0;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private Vector3[] laten = new Vector3[count];
private int index = 0;
public uint timer = 0;
public Vector3 result;
public void Update(Vector3 value, uint time)
{
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
value.X += Shift.X;
value.Y += Shift.Y;
value.Z += Shift.Z;
int noise = (int)(Noise * 1000);
value.X += ((float)rand.Next(-noise, noise)) / 1000;
value.Y += ((float)rand.Next(-noise, noise)) / 1000;
value.Z += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = time - last;
while (true)
{
laten[index] = value;
clock--;
if (clock == 0) break;
index++;
if (index >= count) index = 0;
}
last = time;
int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
value = laten[move];
uint freq = 1000 / Freq;
if (timer + freq <= time)
{
result = value;
timer = time;
}
}
} }
last = time;
internal class Magnetometer int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
value = laten[move];
uint freq = 1000 / Freq;
if (timer + freq <= time)
{ {
/** result = value;
* The model is produced by the United States’ National Geospatial-Intelligence Agency (NGA) timer = time;
* and the United Kingdom’s Defence Geographic Centre (DGC)
* NCEI and the British Geological Survey (BGS) jointly developed the WMM.
*/
/* Taganrog
* 47° 12' 32" N
* 38° 56' 10" E
* Declination: 8° 32' 28"
* Inclination: 65° 34' 9"
* Total Field: 51,120.8 nT
*/
public static float fieldStrength = 51.1208F; // uT
public static float fieldDeclination = (8 + 32/60 + 28/3600) * (MathF.PI / 180);
public static float fieldInclination = (65 + 34/60 + 9/3600) * (MathF.PI / 180);
private static Vector3 InitializeMagneticField()
{
float horizontalComponent = fieldStrength * MathF.Cos(fieldInclination);
float northComponent = horizontalComponent * MathF.Cos(fieldDeclination); // X
float eastComponent = horizontalComponent * MathF.Sin(fieldDeclination); // Y
float downComponent = fieldStrength * MathF.Sin(fieldInclination); // Z
return new Vector3(northComponent, eastComponent, downComponent);
}
private static Vector3 magneticField = InitializeMagneticField();
//TODO: noise and delay(?)
public uint timer = 0;
public Vector3 result;
public void Update(Quaternion oreintantion, uint time)
{
result = Vector3.Transform(magneticField, oreintantion);
timer = time;
}
}
internal class Position
{
public static bool Enable;
public static uint Freq;
public static float Noise;
public static float Lateness;
public static bool RealSimulation;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private Vector3[] laten = new Vector3[count];
private int index = 0;
public uint timer = 0;
public Vector3 result;
public void Update(Vector3 value, uint time)
{
Vector3 v = value;
int noise = (int)(Noise * 1000);
v.X += ((float)rand.Next(-noise, noise)) / 1000;
v.Y += ((float)rand.Next(-noise, noise)) / 1000;
v.Z += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = time - last;
while (true)
{
laten[index] = v;
clock--;
if (clock == 0) break;
index++;
if (index >= count) index = 0;
}
last = time;
if (!Enable)
{
result = Vector3.NaN;
timer = time;
return;
}
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
v = laten[move];
uint freq = 1000 / Freq;
if (timer + freq <= time)
{
result = v;
timer = time;
}
}
}
internal class Barometer
{
public static bool Enable;
public static float Pressure;
public static uint Freq;
public static float Noise;
public static float Lateness;
public static bool RealSimulation;
public static float Temperature = 25.0f;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private float[] laten = new float[count];
private int index = 0;
public uint timer = 0;
public float result;
public void Update(float value, uint time)
{
value = Pressure * MathF.Exp(-0.02896f * 9.81f * value / (8.314f * (Temperature + 273.15f)));
float v = value;
int noise = (int)(Noise * 1000);
v += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = time - last;
while (true)
{
laten[index] = v;
clock--;
if (clock == 0) break;
index++;
if (index >= count) index = 0;
}
last = time;
if (!Enable)
{
result = float.NaN;
timer = time;
return;
}
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
v = laten[move];
uint freq = 1000 / Freq;
if (timer + freq <= time)
{
result = v;
timer = time;
}
}
}
internal class OpticalFlow
{
public static bool Enable;
public static float MaxHeight;
public static uint Freq;
public static float Noise;
public static float Lateness;
public static float Lens;
public static bool RealSimulation;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private Vector2[] laten = new Vector2[count];
private int index = 0;
public uint delay = 0;
public uint timer = 0;
public Vector2 result;
public bool Update(Vector2 value, float Range, uint time)
{
value *= Lens;
Vector2 v = value;
if (Range > MaxHeight) v = Vector2.Zero;
else
{
int noise = (int)(Noise * 1000);
v.X += ((float)rand.Next(-noise, noise)) / 1000;
v.Y += ((float)rand.Next(-noise, noise)) / 1000;
}
uint clock = time - last;
while (true)
{
laten[index] = v;
clock--;
if (clock == 0) break;
index++;
if (index >= count) index = 0;
}
last = time;
if (!Enable)
{
result = Vector2.NaN;
timer = time;
return true;
}
if (!RealSimulation)
{
result = value;
timer = time;
return true;
}
int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
result = laten[move];
uint freq = count / Freq;
if (timer + freq <= time)
{
timer = time;
return true;
}
return false;
}
}
internal class Range
{
public static bool Enable;
public static float MaxHeight;
public static uint Freq;
public static float Noise;
public static float Lateness;
public static bool RealSimulation;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private float[] laten = new float[count];
private int index = 0;
public uint timer = 0;
public float result;
public void Update(float value, uint time)
{
float v = value;
if (v > MaxHeight) v = -1;
else
{
int noise = (int)(Noise * 1000);
v += ((float)rand.Next(-noise, noise)) / 1000;
}
uint clock = time - last;
while (true)
{
laten[index] = v;
clock--;
if (clock == 0) break;
index++;
if (index >= count) index = 0;
}
last = time;
if (!Enable)
{
result = float.NaN;
timer = time;
return;
}
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
v = laten[move];
uint freq = 1000 / Freq;
if (timer + freq <= time)
{
result = v;
timer = time;
}
}
} }
}
} }
internal class Gyroscope
{
public static uint Freq;
public static float Noise;
public static Vector3 Shift;
public static float Lateness;
public static bool RealSimulation;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private Vector3[] laten = new Vector3[count];
private int index = 0;
public uint timer = 0;
public Vector3 result;
public void Update(Vector3 value, uint time)
{
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
value.X += Shift.X;
value.Y += Shift.Y;
value.Z += Shift.Z;
int noise = (int)(Noise * 1000);
value.X += ((float)rand.Next(-noise, noise)) / 1000;
value.Y += ((float)rand.Next(-noise, noise)) / 1000;
value.Z += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = time - last;
while (true)
{
laten[index] = value;
clock--;
if (clock == 0) break;
index++;
if (index >= count) index = 0;
}
last = time;
int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
value = laten[move];
uint freq = 1000 / Freq;
if (timer + freq <= time)
{
result = value;
timer = time;
}
}
}
internal class Magnetometer
{
/**
* The model is produced by the United States’ National Geospatial-Intelligence Agency (NGA)
* and the United Kingdom’s Defence Geographic Centre (DGC)
* NCEI and the British Geological Survey (BGS) jointly developed the WMM.
*/
/* Taganrog
* 47° 12' 32" N
* 38° 56' 10" E
* Declination: 8° 32' 28"
* Inclination: 65° 34' 9"
* Total Field: 51,120.8 nT
*/
public static bool Enable;
public static uint Freq;
public static float Noise;
public static Vector3 Shift;
public static float Lateness;
public static bool RealSimulation;
private uint last = 0;
private Random rand = new Random();
public static float fieldStrength = 51.1208F; // uT
public static float fieldDeclination = (8 + 32 / 60 + 28 / 3600) * (MathF.PI / 180);
public static float fieldInclination = (65 + 34 / 60 + 9 / 3600) * (MathF.PI / 180);
private static Vector3 InitializeMagneticField()
{
float horizontalComponent = fieldStrength * MathF.Cos(fieldInclination);
float northComponent = horizontalComponent * MathF.Cos(fieldDeclination); // X
float eastComponent = horizontalComponent * MathF.Sin(fieldDeclination); // Y
float downComponent = fieldStrength * MathF.Sin(fieldInclination); // Z
return new Vector3(northComponent, eastComponent, downComponent);
}
private static Vector3 magneticField = InitializeMagneticField();
private const int count = 1000;
private Vector3[] laten = new Vector3[count];
private int index = 0;
public uint timer = 0;
public Vector3 result;
public void Update(Quaternion oreintantion, uint time)
{
Vector3 value = Vector3.Transform(magneticField, oreintantion);
Vector3 v = value;
v.X += Shift.X;
v.Y += Shift.Y;
v.Z += Shift.Z;
int noise = (int)(Noise * 1000);
v.X += ((float)rand.Next(-noise, noise)) / 1000;
v.Y += ((float)rand.Next(-noise, noise)) / 1000;
v.Z += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = time - last;
while (true)
{
laten[index] = v;
clock--;
if (clock == 0) break;
index++;
if (index >= count) index = 0;
}
last = time;
if (!Enable)
{
result = Vector3.NaN;
timer = time;
return;
}
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
v = laten[move];
uint freq = 1000 / Freq;
if (timer + freq <= time)
{
result = v;
timer = time;
}
}
}
internal class Position
{
public static bool Enable;
public static uint Freq;
public static float Noise;
public static float Lateness;
public static bool RealSimulation;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private Vector3[] laten = new Vector3[count];
private int index = 0;
public uint timer = 0;
public Vector3 result;
public void Update(Vector3 value, uint time)
{
Vector3 v = value;
int noise = (int)(Noise * 1000);
v.X += ((float)rand.Next(-noise, noise)) / 1000;
v.Y += ((float)rand.Next(-noise, noise)) / 1000;
v.Z += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = time - last;
while (true)
{
laten[index] = v;
clock--;
if (clock == 0) break;
index++;
if (index >= count) index = 0;
}
last = time;
if (!Enable)
{
result = Vector3.NaN;
timer = time;
return;
}
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
v = laten[move];
uint freq = 1000 / Freq;
if (timer + freq <= time)
{
result = v;
timer = time;
}
}
}
internal class Barometer
{
public static bool Enable;
public static float Pressure;
public static uint Freq;
public static float Noise;
public static float Lateness;
public static bool RealSimulation;
public static float Temperature = 25.0f;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private float[] laten = new float[count];
private int index = 0;
public uint timer = 0;
public float result;
public void Update(float value, uint time)
{
value = Pressure * MathF.Exp(-0.02896f * 9.81f * value / (8.314f * (Temperature + 273.15f)));
float v = value;
int noise = (int)(Noise * 1000);
v += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = time - last;
while (true)
{
laten[index] = v;
clock--;
if (clock == 0) break;
index++;
if (index >= count) index = 0;
}
last = time;
if (!Enable)
{
result = float.NaN;
timer = time;
return;
}
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
v = laten[move];
uint freq = 1000 / Freq;
if (timer + freq <= time)
{
result = v;
timer = time;
}
}
}
internal class OpticalFlow
{
public static bool Enable;
public static float MaxHeight;
public static uint Freq;
public static float Noise;
public static float Lateness;
public static float Lens;
public static bool RealSimulation;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private Vector2[] laten = new Vector2[count];
private int index = 0;
public uint delay = 0;
public uint timer = 0;
public Vector2 result;
public bool Update(Vector2 value, float Range, uint time)
{
value *= Lens;
Vector2 v = value;
if (Range > MaxHeight) v = Vector2.Zero;
else
{
int noise = (int)(Noise * 1000);
v.X += ((float)rand.Next(-noise, noise)) / 1000;
v.Y += ((float)rand.Next(-noise, noise)) / 1000;
}
uint clock = time - last;
while (true)
{
laten[index] = v;
clock--;
if (clock == 0) break;
index++;
if (index >= count) index = 0;
}
last = time;
if (!Enable)
{
result = Vector2.NaN;
timer = time;
return true;
}
if (!RealSimulation)
{
result = value;
timer = time;
return true;
}
int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
result = laten[move];
uint freq = count / Freq;
if (timer + freq <= time)
{
timer = time;
return true;
}
return false;
}
}
internal class Range
{
public static bool Enable;
public static float MaxHeight;
public static uint Freq;
public static float Noise;
public static float Lateness;
public static bool RealSimulation;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private float[] laten = new float[count];
private int index = 0;
public uint timer = 0;
public float result;
public void Update(float value, uint time)
{
float v = value;
if (v > MaxHeight) v = -1;
else
{
int noise = (int)(Noise * 1000);
v += ((float)rand.Next(-noise, noise)) / 1000;
}
uint clock = time - last;
while (true)
{
laten[index] = v;
clock--;
if (clock == 0) break;
index++;
if (index >= count) index = 0;
}
last = time;
if (!Enable)
{
result = float.NaN;
timer = time;
return;
}
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
v = laten[move];
uint freq = 1000 / Freq;
if (timer + freq <= time)
{
result = v;
timer = time;
}
}
}
}
} }

312
DroneSimulator/Screen2D.cs
View File

@ -3,194 +3,194 @@ using System.Numerics;
namespace DroneSimulator namespace DroneSimulator
{ {
internal class Screen2D internal class Screen2D
{
public delegate void DrawCallback(Bitmap bmp);
public Screen2D(DrawCallback callback)
{ {
drawCallback = callback;
}
public delegate void DrawCallback(Bitmap bmp); private class DroneInfo
{
public int ID;
public Color RGB;
public Bitmap? Drone;
public Point PosXY;
public int Height = 0;
public Screen2D(DrawCallback callback) public PointF TiltXY = new Point(0, 0);
{ public int Azimuth = 0;
drawCallback = callback; public Quaternion Quaternion;
} }
private class DroneInfo private float Scale = 100;
{ private DrawCallback drawCallback;
public int ID; private Bitmap MainArea = new Bitmap(4096, 2560);
public Color RGB; private List<DroneInfo> DroneList = new List<DroneInfo>();
public Bitmap? Drone;
public Point PosXY;
public int Height = 0;
public PointF TiltXY = new Point(0, 0); public static Bitmap DrawQadro(Color ColorFill)
public int Azimuth = 0; {
public Quaternion Quaternion; Bitmap drone = new Bitmap(130, 130);
}
private float Scale = 100; using (Graphics g = Graphics.FromImage(drone))
private DrawCallback drawCallback; {
private Bitmap MainArea = new Bitmap(4096, 2560); g.PixelOffsetMode = System.Drawing.Drawing2D.PixelOffsetMode.HighSpeed;
private List<DroneInfo> DroneList = new List<DroneInfo>();
public static Bitmap DrawQadro(Color ColorFill) SolidBrush solidBrush = new SolidBrush(ColorFill);
{
Bitmap drone = new Bitmap(130, 130);
using (Graphics g = Graphics.FromImage(drone)) Point[] mid = { new Point(35, 65), new Point(70, 40), new Point(70, 90) };
{ g.FillPolygon(solidBrush, mid);
g.PixelOffsetMode = System.Drawing.Drawing2D.PixelOffsetMode.HighSpeed;
SolidBrush solidBrush = new SolidBrush(ColorFill); 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));
Point[] mid = { new Point(35, 65), new Point(70, 40), new Point(70, 90) }; g.FillRectangle(solidBrush, new Rectangle(50, 60, 50, 10));
g.FillPolygon(solidBrush, mid);
g.FillEllipse(solidBrush, new Rectangle(15, 15, 35, 35)); g.DrawEllipse(new Pen(ColorFill), new Rectangle(0, 0, 129, 129));
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)); solidBrush.Dispose();
}
g.DrawEllipse(new Pen(ColorFill), new Rectangle(0, 0, 129, 129)); return RotateImage(drone, 90);
}
solidBrush.Dispose(); private static Bitmap RotateImage(Bitmap bmp, float angle)
} {
Bitmap rotatedImage = new Bitmap(bmp.Width, bmp.Height);
return RotateImage(drone, 90); 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));
}
private static Bitmap RotateImage(Bitmap bmp, float angle) return rotatedImage;
{ }
Bitmap rotatedImage = new Bitmap(bmp.Width, bmp.Height);
using (Graphics g = Graphics.FromImage(rotatedImage)) public void CreateDrone(Color ColorFill, int ID)
{ {
// Set the rotation point to the center in the matrix DroneInfo info = new DroneInfo();
g.TranslateTransform(bmp.Width / 2, bmp.Height / 2); info.ID = ID;
// Rotate info.RGB = ColorFill;
g.RotateTransform(angle); info.Drone = DrawQadro(ColorFill);
// 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; DroneList.Add(info);
} }
public void CreateDrone(Color ColorFill, int ID) public void RemoveDrone(int ID)
{ {
DroneInfo info = new DroneInfo(); foreach (DroneInfo i in DroneList)
info.ID = ID; {
info.RGB = ColorFill; if (i.ID != ID) continue;
info.Drone = DrawQadro(ColorFill); DroneList.Remove(i);
break;
}
}
DroneList.Add(info); private static Bitmap DrawImageByQuaternion(Bitmap bmp, Quaternion orientation)
} {
if (bmp == null) return null;
orientation.X = -orientation.X;
public void RemoveDrone(int ID) int canvasSize = (int)System.Math.Sqrt(bmp.Width * bmp.Width + bmp.Height * bmp.Height);
{ Bitmap result = new Bitmap(canvasSize, canvasSize);
foreach (DroneInfo i in DroneList)
{
if (i.ID != ID) continue;
DroneList.Remove(i);
break;
}
}
private static Bitmap DrawImageByQuaternion(Bitmap bmp, Quaternion orientation) float halfWidth = bmp.Width / 2f;
{ float halfHeight = bmp.Height / 2f;
if (bmp == null) return null;
orientation.X = -orientation.X;
int canvasSize = (int)System.Math.Sqrt(bmp.Width * bmp.Width + bmp.Height * bmp.Height);
Bitmap result = new Bitmap(canvasSize, canvasSize);
float halfWidth = bmp.Width / 2f;
float halfHeight = bmp.Height / 2f;
Vector3[] sourceCorners = new Vector3[] Vector3[] sourceCorners = new Vector3[]
{ {
new Vector3(-halfWidth, -halfHeight, 0), // верхний левый new Vector3(-halfWidth, -halfHeight, 0), // верхний левый
new Vector3( halfWidth, -halfHeight, 0), // верхний правый new Vector3( halfWidth, -halfHeight, 0), // верхний правый
new Vector3(-halfWidth, halfHeight, 0), // нижний левый new Vector3(-halfWidth, halfHeight, 0), // нижний левый
}; };
PointF[] destPoints = new PointF[3]; PointF[] destPoints = new PointF[3];
for (int i = 0; i < 3; i++) for (int i = 0; i < 3; i++)
{ {
Vector3 rotatedPoint = Vector3.Transform(sourceCorners[i], orientation); Vector3 rotatedPoint = Vector3.Transform(sourceCorners[i], orientation);
destPoints[i] = new PointF( destPoints[i] = new PointF(
rotatedPoint.X + canvasSize / 2f, rotatedPoint.X + canvasSize / 2f,
rotatedPoint.Y + canvasSize / 2f rotatedPoint.Y + canvasSize / 2f
); );
} }
using (Graphics g = Graphics.FromImage(result)) using (Graphics g = Graphics.FromImage(result))
{ {
g.InterpolationMode = InterpolationMode.HighQualityBicubic; g.InterpolationMode = InterpolationMode.HighQualityBicubic;
g.SmoothingMode = SmoothingMode.AntiAlias; g.SmoothingMode = SmoothingMode.AntiAlias;
g.DrawImage(bmp, destPoints); g.DrawImage(bmp, destPoints);
} }
return result; return result;
}
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)
{
try
{
if (d.Azimuth >= 360) d.Azimuth -= 360;
var bmp = DrawImageByQuaternion(d.Drone, d.Quaternion);
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, d.PosXY.X - d.Drone.Width / 2, d.PosXY.Y - d.Drone.Height / 2); // Draw the transformed image
}
catch { }
}
}
drawCallback(MainArea);
}
public void Move(int id, Vector3 pos, Vector4 tilt, Quaternion quaternion)
{
const float TO_GRAD = 180 / MathF.PI;
const float TO_RADI = MathF.PI / 180;
pos *= Scale;
foreach (var d in DroneList)
{
if (d.ID != id) continue;
d.PosXY.X = MainArea.Width / 2 + (int)pos.Y;
d.PosXY.Y = MainArea.Height / 2 - (int)pos.X;
d.Height = (int)pos.Z;
d.TiltXY.X = tilt.X * TO_RADI;
d.TiltXY.Y = tilt.Y * TO_RADI;
d.Azimuth = (int)tilt.Z;
d.Quaternion = quaternion;
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)
{
try
{
if (d.Azimuth >= 360) d.Azimuth -= 360;
var bmp = DrawImageByQuaternion(d.Drone, d.Quaternion);
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, d.PosXY.X - d.Drone.Width / 2, d.PosXY.Y - d.Drone.Height / 2); // Draw the transformed image
}
catch { }
}
}
drawCallback(MainArea);
}
public void Move(int id, Vector3 pos, Vector4 tilt, Quaternion quaternion)
{
const float TO_GRAD = 180 / MathF.PI;
const float TO_RADI = MathF.PI / 180;
pos *= Scale;
foreach (var d in DroneList)
{
if (d.ID != id) continue;
d.PosXY.X = MainArea.Width / 2 + (int)pos.Y;
d.PosXY.Y = MainArea.Height / 2 - (int)pos.X;
d.Height = (int)pos.Z;
d.TiltXY.X = tilt.X * TO_RADI;
d.TiltXY.Y = tilt.Y * TO_RADI;
d.Azimuth = (int)tilt.Z;
d.Quaternion = quaternion;
break;
}
}
}
} }