CPL/Simulator
6
0
Fork 2
Code Issues 1 Pull Requests Actions Packages Projects Releases Activity

Update Drone.cs

Browse Source 
Димончик нашёл ошибочку, которую поправили. Хвала всемогущему Димончику!!!
This commit is contained in:
Sergey Sklyarov
2025-07-21 23:28:10 +03:00
parent 446e45d9df
commit 565209d6b9
1 changed files with 19 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
DroneSimulator/Drone.cs
View File

@ -1,7 +1,8 @@
using System.Net.Sockets; using System.Diagnostics;
using System.Diagnostics; using System.Net.Sockets;
using System.Numerics; using System.Numerics;
using System.Runtime.InteropServices; using System.Runtime.InteropServices;
using static VisualData.VisualDrone;
namespace DroneSimulator namespace DroneSimulator
{ {
@ -221,6 +222,17 @@ namespace DroneSimulator
Quat = Quaternion.Normalize(map); 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() public Vector4 GetOrientation()
{ {
Quaternion grav = new Quaternion(0, 0, 1, 0); Quaternion grav = new Quaternion(0, 0, 1, 0);
@ -336,7 +348,7 @@ namespace DroneSimulator
}*/ }*/
else else
{ {
if (ori.W < 0) if (ori.W < 0) // это обман
{ {
//Active = false; // Перевернулся вверх ногами //Active = false; // Перевернулся вверх ногами
} }
@ -360,8 +372,11 @@ namespace DroneSimulator
RealBar.Update(PosXYZ.Z, tick); RealBar.Update(PosXYZ.Z, tick);
RealPos.Update(PosXYZ, 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; 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; else of_xy = Vector2.Zero;
bool of = RealOF.Update(of_xy, LaserRange, tick); bool of = RealOF.Update(of_xy, LaserRange, tick);