Simulator/DroneSimulator/Screen2D.cs

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

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

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

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

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