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