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,4 +1,5 @@
-using System.Numerics;
+using System.Drawing.Drawing2D;
+using System.Numerics;
 
 namespace DroneSimulator
 {
@@ -22,6 +23,7 @@ namespace DroneSimulator
 
             public PointF TiltXY = new Point(0, 0);
             public int Azimuth = 0;
+            public Quaternion Quaternion;
         }
 
         private float Scale = 100;
@@ -96,6 +98,46 @@ namespace DroneSimulator
             }
         }
 
+        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))
@@ -109,50 +151,15 @@ namespace DroneSimulator
                     try
                     {
                         if (d.Azimuth >= 360) d.Azimuth -= 360;
-            var bmp = RotateImage(d.Drone, d.Azimuth);
+
+                        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, new Rectangle(d.PosXY.X+32, d.PosXY.Y, 65, 130));
+                        g.DrawImage(bmp, d.PosXY.X - d.Drone.Width / 2, d.PosXY.Y - d.Drone.Height / 2); // Draw the transformed image
 
-            float x1 = 0, y1 = 0;
-            float x2 = 130, y2 = 0;
-            float x3 = 0, y3 = 130;
-
-            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);
-
-            tilt = tilt * rotate * rotate;
-
-            if (tilt.Y > 0)
-            {
-              x1 = (int)(Math.Sin(tilt.Y) * 130);
-              x3 = (int)(Math.Sin(tilt.Y) * 130);
-            }
-            else
-            {
-              x2 = (int)(Math.Cos(tilt.Y) * 130);
-            }
-
-            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);
-            }
-
-            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 };
-
-            g.DrawImage(bmp, dest);
                     }
                     catch { }
                 }
@@ -161,7 +168,7 @@ namespace DroneSimulator
             drawCallback(MainArea);
         }
 
-    public void Move(int id, Vector3 pos, Vector4 tilt)
+        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;
@@ -182,6 +189,7 @@ namespace DroneSimulator
                 d.TiltXY.X = tilt.X * TO_RADI;
                 d.TiltXY.Y = tilt.Y * TO_RADI;
                 d.Azimuth = (int)tilt.Z;
+                d.Quaternion = quaternion;
 
                 break;
             }