diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs index 169a58c..58dc940 100644 --- a/DroneSimulator/Drone.cs +++ b/DroneSimulator/Drone.cs @@ -62,7 +62,7 @@ namespace DroneSimulator private RealMode.Barometer RealBar = new RealMode.Barometer(); private RealMode.Range RealRange = new RealMode.Range(); 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) { @@ -373,7 +373,7 @@ namespace DroneSimulator bool of = RealOF.Update(of_xy, LaserRange, tick); - RealMagnetometer.Update(Quat, tick); + RealMag.Update(Quat, tick); lock (this) { @@ -481,8 +481,8 @@ namespace DroneSimulator mag.Head.Type = DroneData.DataType.DataMag; mag.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000); - mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0; - mag.Time = Timer; + mag.Mag.X = RealMag.result.X; mag.Mag.Y = RealMag.result.Y; mag.Mag.Z = RealMag.result.Z; + mag.Time = RealMag.timer; return getBytes(mag); } diff --git a/DroneSimulator/GPS.cs b/DroneSimulator/GPS.cs index 00c97e6..5d431b1 100644 --- a/DroneSimulator/GPS.cs +++ b/DroneSimulator/GPS.cs @@ -8,67 +8,67 @@ using System.Threading.Tasks; namespace DroneSimulator { - internal class GPS + internal class GPS + { + static double PI = 3.14159265358979323846; + public struct Home { - static double PI = 3.14159265358979323846; - public struct Home - { - 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 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; + } + } } diff --git a/DroneSimulator/RealMode.cs b/DroneSimulator/RealMode.cs index 8bc788d..4eb31fa 100644 --- a/DroneSimulator/RealMode.cs +++ b/DroneSimulator/RealMode.cs @@ -1,4 +1,5 @@ -using System; +using Microsoft.VisualBasic.Devices; +using System; using System.Collections.Generic; using System.Linq; using System.Numerics; @@ -9,464 +10,525 @@ using static System.Windows.Forms.VisualStyles.VisualStyleElement.TaskbarClock; 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; - public static float Noise; - public static float ScaleLeft; - 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; - } - } + result = value; + timer = time; + return; } - 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; - 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; - } - } + laten[index] = value; + clock--; + if (clock == 0) break; + index++; + if (index >= count) index = 0; } + 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) { - /** - * 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 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; - } - } + result = value; + 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; + } + } + } + } } diff --git a/DroneSimulator/Screen2D.cs b/DroneSimulator/Screen2D.cs index 455fc4c..bfb2fe0 100644 --- a/DroneSimulator/Screen2D.cs +++ b/DroneSimulator/Screen2D.cs @@ -3,194 +3,194 @@ using System.Numerics; 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) - { - drawCallback = callback; - } + public PointF TiltXY = new Point(0, 0); + public int Azimuth = 0; + public Quaternion Quaternion; + } - private class DroneInfo - { - public int ID; - public Color RGB; - public Bitmap? Drone; - public Point PosXY; - public int Height = 0; + private float Scale = 100; + private DrawCallback drawCallback; + private Bitmap MainArea = new Bitmap(4096, 2560); + private List DroneList = new List(); - public PointF TiltXY = new Point(0, 0); - public int Azimuth = 0; - public Quaternion Quaternion; - } + public static Bitmap DrawQadro(Color ColorFill) + { + Bitmap drone = new Bitmap(130, 130); - private float Scale = 100; - private DrawCallback drawCallback; - private Bitmap MainArea = new Bitmap(4096, 2560); - private List DroneList = new List(); + using (Graphics g = Graphics.FromImage(drone)) + { + g.PixelOffsetMode = System.Drawing.Drawing2D.PixelOffsetMode.HighSpeed; - public static Bitmap DrawQadro(Color ColorFill) - { - Bitmap drone = new Bitmap(130, 130); + SolidBrush solidBrush = new SolidBrush(ColorFill); - using (Graphics g = Graphics.FromImage(drone)) - { - g.PixelOffsetMode = System.Drawing.Drawing2D.PixelOffsetMode.HighSpeed; + Point[] mid = { new Point(35, 65), new Point(70, 40), new Point(70, 90) }; + g.FillPolygon(solidBrush, mid); - 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.FillPolygon(solidBrush, mid); + g.FillRectangle(solidBrush, new Rectangle(50, 60, 50, 10)); - 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.DrawEllipse(new Pen(ColorFill), new Rectangle(0, 0, 129, 129)); - 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) - { - Bitmap rotatedImage = new Bitmap(bmp.Width, bmp.Height); + return rotatedImage; + } - 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)); - } + public void CreateDrone(Color ColorFill, int ID) + { + DroneInfo info = new DroneInfo(); + info.ID = ID; + info.RGB = ColorFill; + info.Drone = DrawQadro(ColorFill); - return rotatedImage; - } + DroneList.Add(info); + } - public void CreateDrone(Color ColorFill, int ID) - { - DroneInfo info = new DroneInfo(); - info.ID = ID; - info.RGB = ColorFill; - info.Drone = DrawQadro(ColorFill); + public void RemoveDrone(int ID) + { + foreach (DroneInfo i in DroneList) + { + if (i.ID != ID) continue; + 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) - { - foreach (DroneInfo i in DroneList) - { - if (i.ID != ID) continue; - DroneList.Remove(i); - break; - } - } + int canvasSize = (int)System.Math.Sqrt(bmp.Width * bmp.Width + bmp.Height * bmp.Height); + Bitmap result = new Bitmap(canvasSize, canvasSize); - private static Bitmap DrawImageByQuaternion(Bitmap bmp, Quaternion orientation) - { - 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; + 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), // нижний левый - }; + }; - PointF[] destPoints = new PointF[3]; + PointF[] destPoints = new PointF[3]; - for (int i = 0; i < 3; i++) - { - Vector3 rotatedPoint = Vector3.Transform(sourceCorners[i], orientation); + for (int i = 0; i < 3; i++) + { + Vector3 rotatedPoint = Vector3.Transform(sourceCorners[i], orientation); - destPoints[i] = new PointF( - rotatedPoint.X + canvasSize / 2f, - rotatedPoint.Y + canvasSize / 2f - ); - } + destPoints[i] = new PointF( + rotatedPoint.X + canvasSize / 2f, + rotatedPoint.Y + canvasSize / 2f + ); + } - using (Graphics g = Graphics.FromImage(result)) - { - g.InterpolationMode = InterpolationMode.HighQualityBicubic; - g.SmoothingMode = SmoothingMode.AntiAlias; - g.DrawImage(bmp, destPoints); - } + using (Graphics g = Graphics.FromImage(result)) + { + g.InterpolationMode = InterpolationMode.HighQualityBicubic; + g.SmoothingMode = SmoothingMode.AntiAlias; + g.DrawImage(bmp, destPoints); + } - 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; - } - } + 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; + } + } + } }