new drone render method

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();
 

DroneSimulator/Screen2D.cs

@@ -1,190 +1,198 @@
-using System.Numerics;
+using System.Drawing.Drawing2D;
+using System.Numerics;
 
 namespace DroneSimulator
 {
-  internal class Screen2D
+    internal class Screen2D
-  {
-
-    public delegate void DrawCallback(Bitmap bmp);
-
-    public Screen2D(DrawCallback callback)
     {
-      drawCallback = callback;
-    }
 
-    private class DroneInfo
+        public delegate void DrawCallback(Bitmap bmp);
-    {
-      public int ID;
-      public Color RGB;
-      public Bitmap? Drone;
-      public Point PosXY;
-      public int Height = 0;
 
-      public PointF TiltXY = new Point(0, 0);
+        public Screen2D(DrawCallback callback)
-      public int Azimuth = 0;
-    }
-
-    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;
-      }      
-    }
-
-    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
+            drawCallback = callback;
-          {
+        }
-            if (d.Azimuth >= 360) d.Azimuth -= 360;
-            var bmp = RotateImage(d.Drone, d.Azimuth);
 
-            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<DroneInfo> DroneList = new List<DroneInfo>();
 
-            float x1 = 0, y1 = 0;
+        public static Bitmap DrawQadro(Color ColorFill)
-            float x2 = 130, y2 = 0;
+        {
-            float x3 = 0, y3 = 130;
+            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);
+            pos *= Scale;
-            Quaternion rotate = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), d.Azimuth * TO_RADI);
 
-            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);
+                if (d.ID != id) continue;
-              x3 = (int)(Math.Sin(tilt.Y) * 130);
-            }
-            else
-            {
-              x2 = (int)(Math.Cos(tilt.Y) * 130);
-            }
 
-            if (tilt.X > 0)
+                d.PosXY.X = (int)pos.X;
-            {
+                d.PosXY.Y = MainArea.Height - (int)pos.Y;
-              y1 = (int)(Math.Sin(tilt.X) * 130);
+                d.Height = (int)pos.Z;
-              y2 = (int)(Math.Sin(tilt.X) * 130);
-            }
-            else
-            {
-              y3 = (int)(Math.Cos(tilt.X) * 130);
-            }
 
-            PointF ul = new PointF(d.PosXY.X + x1, d.PosXY.Y + y1); PointF ur = new PointF(d.PosXY.X + x2, d.PosXY.Y + y2);
+                d.TiltXY.X = tilt.X * TO_RADI;
-            PointF dl = new PointF(d.PosXY.X + x3, d.PosXY.Y + y3);
+                d.TiltXY.Y = tilt.Y * TO_RADI;
-            PointF[] dest = { ul, ur, dl };
+                d.Azimuth = (int)tilt.Z;
+                d.Quaternion = quaternion;
 
-            g.DrawImage(bmp, dest);
+                break;
-          }
+            }
-          catch { }
         }
-      }
-
-      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;
-      }
-    }
-  }
 }