Simulator/DroneSimulator/Screen2D.cs

using System.Drawing.Drawing2D;
using System.Numerics;

namespace DroneSimulator
{
    internal class Screen2D
    {

        public delegate void DrawCallback(Bitmap bmp);

        public Screen2D(DrawCallback callback)
        {
            drawCallback = callback;
        }

        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;
            public Quaternion Quaternion;
        }

        private float Scale = 100;
        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;
            }
        }

        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;


            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;

            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;
            }
        }
    }
}