From 565209d6b9d8734e2c066951697d6a307114db8e Mon Sep 17 00:00:00 2001
From: Sergey Sklyarov <sasklyarov@sfedu.ru>
Date: Mon, 21 Jul 2025 23:28:10 +0300
Subject: [PATCH] Update Drone.cs
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Димончик нашёл ошибочку, которую поправили. Хвала всемогущему Димончику!!!
---
 DroneSimulator/Drone.cs | 23 +++++++++++++++++++----
 1 file changed, 19 insertions(+), 4 deletions(-)

diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs
index e7d09f7..a6af941 100644
--- a/DroneSimulator/Drone.cs
+++ b/DroneSimulator/Drone.cs
@@ -1,7 +1,8 @@
-using System.Net.Sockets;
-using System.Diagnostics;
+using System.Diagnostics;
+using System.Net.Sockets;
 using System.Numerics;
 using System.Runtime.InteropServices;
+using static VisualData.VisualDrone;
 
 namespace DroneSimulator
 {
@@ -221,6 +222,17 @@ namespace DroneSimulator
       Quat = Quaternion.Normalize(map);
     }
 
+    Vector2 RotateToZ(Vector2 vec, bool Rev = false)
+    {
+      Quaternion v = new Quaternion(vec.X, vec.Y, 0, 0);
+      Quaternion q = new Quaternion(0, 0, Rev ? -Quat.Z : Quat.Z, Quat.W);
+      q = Quaternion.Normalize(q);
+
+      q = (v * q) * q;
+
+      return new Vector2(q.X, q.Y);
+    }
+
     public Vector4 GetOrientation()
     {
       Quaternion grav = new Quaternion(0, 0, 1, 0);
@@ -336,7 +348,7 @@ namespace DroneSimulator
       }*/
       else
       {
-        if (ori.W < 0)
+        if (ori.W < 0) // это обман
         {
           //Active = false; // Перевернулся вверх ногами
         }
@@ -360,8 +372,11 @@ namespace DroneSimulator
       RealBar.Update(PosXYZ.Z, tick);
       RealPos.Update(PosXYZ, tick);
 
+      Vector2 xy = new Vector2(SpdXYZ.X * TO_GRAD / range, SpdXYZ.Y * TO_GRAD / range);
+      xy = RotateToZ(xy, true);
+
       Vector2 of_xy;
-      if (range > 0.1) of_xy = new Vector2(SpdXYZ.X * TO_GRAD / range - Gyr.Y, SpdXYZ.Y * TO_GRAD / range + Gyr.X);
+      if (range > 0.1) of_xy = new Vector2(xy.X - Gyr.Y, xy.Y + Gyr.X);
       else of_xy = Vector2.Zero;
 
       bool of = RealOF.Update(of_xy, LaserRange, tick);