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

+

Browse Source
This commit is contained in:
Sergey Sklyarov
2025-05-22 23:52:47 +03:00
parent 90cec037eb
commit 3a462be82d
5 changed files with 336 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
DroneSimulator/Drone.cs
View File

@ -1,10 +1,5 @@
using System.CodeDom;
using System.Numerics;
using System.Numerics;
using System.Runtime.InteropServices;
using System.Security.Cryptography;
using static DroneSimulator.Drone;
using static System.Windows.Forms.VisualStyles.VisualStyleElement;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
namespace DroneSimulator
{
@ -26,8 +21,6 @@ namespace DroneSimulator
public Vector4 Orientation;
public static bool[] Freeze = new bool[3];
private uint DataTimer;
private const float Gravity = 9.8f;
@ -196,17 +189,33 @@ namespace DroneSimulator
SpdPRY += AccPRY * (Dynamic * time / (Mass * Length));
Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat;
AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) * (Gravity / Mass);
float wind_x = 0, wind_y = 0;
if (Area.Wind.Enable)
{
Quaternion dir = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), Area.Wind.Direction * TO_RADI * 2);
//Quaternion win = new Quaternion(0, 1, 0, 0) * dir;
Quaternion spd = new Quaternion(0, Area.Wind.Speed.From, 0, 0) * dir;
float spd_x = spd.X - SpdXYZ.X;
float spd_y = spd.Y - SpdXYZ.Y;
wind_x = 0.5f * Area.Wind.Density * Area.Wind.Resist * spd_x;
wind_y = 0.5f * Area.Wind.Density * Area.Wind.Resist * spd_y;
}
Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat;
AccXYZ = new Vector3(pow.X + wind_x, pow.Y + wind_y, pow.Z) * (Gravity / Mass);
SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time;
PosXYZ += SpdXYZ * time;
AccXYZ /= Gravity; // Вернуть измерения в G
if (Freeze[0]) { SpdXYZ.X = 0; PosXYZ.X = 0; }
if (Freeze[1]) { SpdXYZ.Y = 0; PosXYZ.Y = 0; }
if (Freeze[2]) { SpdXYZ.Z = 0; PosXYZ.Z = 5; }
if (Area.Poisition.Freeze.X) { SpdXYZ.X = 0; PosXYZ.X = 0; }
if (Area.Poisition.Freeze.Y) { SpdXYZ.Y = 0; PosXYZ.Y = 0; }
if (Area.Poisition.Freeze.Z) { SpdXYZ.Z = 0; PosXYZ.Z = 5; }
if (PosXYZ.Z < 0)
{
@ -248,7 +257,7 @@ namespace DroneSimulator
//Active = false; // Перевернулся вверх ногами
}
Quaternion grav = Quat * new Quaternion(0, 0, flow/Mass, 0) * Quaternion.Inverse(Quat);
Quaternion grav = Quat * new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0) * Quaternion.Inverse(Quat);
Acc = new Vector3(grav.X, grav.Y, grav.Z);
Gyr = SpdPRY;