diff --git a/DroneSimulator/FormMain.cs b/DroneSimulator/FormMain.cs index cfa8f03..f6ea18b 100644 --- a/DroneSimulator/FormMain.cs +++ b/DroneSimulator/FormMain.cs @@ -171,7 +171,7 @@ namespace DroneSimulator { foreach (Drone d in Drone.AllDrones) { - screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation()); + screen2D.Move(d.ID, d.PosXYZ, d.GetOrientation(), d.Quat); string line = "ID:" + d.ID.ToString() + " Pitch:" + ((int)d.Orientation.X).ToString() + " Roll:" + ((int)d.Orientation.Y).ToString() + " Yaw:" + ((int)d.Orientation.Z).ToString(); diff --git a/DroneSimulator/Screen2D.cs b/DroneSimulator/Screen2D.cs index d7e7295..383408f 100644 --- a/DroneSimulator/Screen2D.cs +++ b/DroneSimulator/Screen2D.cs @@ -1,190 +1,198 @@ -using System.Numerics; +using System.Drawing.Drawing2D; +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 float Scale = 100; - private DrawCallback drawCallback; - private Bitmap MainArea = new Bitmap(4096, 2560); - private List DroneList = new List(); - - 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) { - try - { - if (d.Azimuth >= 360) d.Azimuth -= 360; - var bmp = RotateImage(d.Drone, d.Azimuth); + drawCallback = callback; + } - g.FillEllipse(new SolidBrush(Color.FromArgb(50, d.RGB)), d.PosXY.X + d.Height, d.PosXY.Y + d.Height, 130, 130); + private class DroneInfo + { + public int ID; + public Color RGB; + public Bitmap? Drone; + public Point PosXY; + public int Height = 0; - 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)); + public PointF TiltXY = new Point(0, 0); + public int Azimuth = 0; + public Quaternion Quaternion; + } - //g.DrawImage(bmp, new Rectangle(d.PosXY.X+32, d.PosXY.Y, 65, 130)); + private float Scale = 100; + private DrawCallback drawCallback; + private Bitmap MainArea = new Bitmap(4096, 2560); + private List DroneList = new List(); - float x1 = 0, y1 = 0; - float x2 = 130, y2 = 0; - float x3 = 0, y3 = 130; + 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; + } + } + + private static Bitmap DrawImageByQuaternion(Bitmap bmp, Quaternion orientation) + { + if (bmp == null) return null; + + 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[] + { + new Vector3(-halfWidth, -halfHeight, 0), // верхний левый + new Vector3( halfWidth, -halfHeight, 0), // верхний правый + new Vector3(-halfWidth, halfHeight, 0), // нижний левый + }; + + PointF[] destPoints = new PointF[3]; + + 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 + ); + } + + 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; - 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); + pos *= Scale; - tilt = tilt * rotate * rotate; + pos.X += MainArea.Width / 2; + pos.Y += MainArea.Height / 2; - if (tilt.Y > 0) + foreach (var d in DroneList) { - x1 = (int)(Math.Sin(tilt.Y) * 130); - x3 = (int)(Math.Sin(tilt.Y) * 130); - } - else - { - x2 = (int)(Math.Cos(tilt.Y) * 130); - } + if (d.ID != id) continue; - 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); - } + d.PosXY.X = (int)pos.X; + d.PosXY.Y = MainArea.Height - (int)pos.Y; + d.Height = (int)pos.Z; - 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 }; + d.TiltXY.X = tilt.X * TO_RADI; + d.TiltXY.Y = tilt.Y * TO_RADI; + d.Azimuth = (int)tilt.Z; + d.Quaternion = quaternion; - g.DrawImage(bmp, dest); - } - catch { } + break; + } } - } - - drawCallback(MainArea); } - - public void Move(int id, Vector3 pos, Vector4 tilt) - { - const float TO_GRAD = 180 / MathF.PI; - const float TO_RADI = MathF.PI / 180; - - pos *= Scale; - - pos.X += MainArea.Width / 2; - pos.Y += MainArea.Height / 2; - - foreach (var d in DroneList) - { - if (d.ID != id) continue; - - d.PosXY.X = (int)pos.X; - d.PosXY.Y = MainArea.Height - (int)pos.Y; - d.Height = (int)pos.Z; - - d.TiltXY.X = tilt.X * TO_RADI; - d.TiltXY.Y = tilt.Y * TO_RADI; - d.Azimuth = (int)tilt.Z; - - break; - } - } - } }