Vector, Quaternion - математика для ориентации. AdaptivePID - ПИД-регулятор. DroneOrientation.CalcAttitude() - цикл вычисления положения.
This commit is contained in:
88
DroneClient/utils/AdaptivePID.cs
Normal file
88
DroneClient/utils/AdaptivePID.cs
Normal file
@@ -0,0 +1,88 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace DroneClient.utils
|
||||
{
|
||||
internal class AdaptivePID
|
||||
{
|
||||
//Initial
|
||||
private float P = 1;
|
||||
private float I = 0.2f;
|
||||
private float D = 0.005f;
|
||||
|
||||
//Adaptive
|
||||
private float aP = 1;
|
||||
private float aI = 0.2f;
|
||||
private float aD = 0.005f;
|
||||
|
||||
private float P_min = 0; private float P_max = 10;
|
||||
private float I_min = 0; private float I_max = 1;
|
||||
private float D_min = 0; private float D_max = 0.1f;
|
||||
|
||||
private float alpha = 0.001f;
|
||||
private float beta = 0.001f;
|
||||
private float gamma = 0.0005f;
|
||||
|
||||
private float prev_error = float.NaN;
|
||||
private float integral = 0;
|
||||
|
||||
public AdaptivePID()
|
||||
{
|
||||
alpha = 0; beta = 0; gamma = 0;
|
||||
}
|
||||
|
||||
public AdaptivePID(float p, float i, float d, float alpha = 0.001f, float beta = 0.0005f, float gamma = 0.00005f)
|
||||
{
|
||||
P = p; aP = p;
|
||||
I = i; aI = i;
|
||||
D = d; aD = d;
|
||||
this.alpha = alpha;
|
||||
this.beta = beta;
|
||||
this.gamma = gamma;
|
||||
}
|
||||
|
||||
public float process(float error, float T)
|
||||
{
|
||||
if (T <= 0) return 0;
|
||||
integral += error * T;
|
||||
float derivative = 0;
|
||||
if (float.IsFinite(prev_error))
|
||||
{
|
||||
derivative = (error - prev_error) / T;
|
||||
}
|
||||
prev_error = error;
|
||||
|
||||
float dP = -alpha * error * error;
|
||||
float dI = -beta * error * integral;
|
||||
float dD = -gamma * error * derivative;
|
||||
|
||||
aP += dP; aI += dI; aD += dD;
|
||||
|
||||
aP = constrain(aP, P_min, P_max);
|
||||
aI = constrain(aI, I_min, I_max);
|
||||
aD = constrain(aD, D_min, D_max);
|
||||
integral = constrain(integral, -10, 10);
|
||||
float output = aP * error + aI * integral + aD * derivative;
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
public void reset()
|
||||
{
|
||||
integral = 0;
|
||||
prev_error = 0;
|
||||
aP = P; aI = I; aD = D;
|
||||
}
|
||||
|
||||
static float constrain(float val, float min, float max)
|
||||
{
|
||||
if (min >= max) return val;
|
||||
if(val < min) return min;
|
||||
if(val > max) return max;
|
||||
return val;
|
||||
}
|
||||
}
|
||||
}
|
||||
198
DroneClient/utils/DroneOrientation.cs
Normal file
198
DroneClient/utils/DroneOrientation.cs
Normal file
@@ -0,0 +1,198 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
using TelemetryIO.Models;
|
||||
|
||||
namespace DroneClient.utils
|
||||
{
|
||||
internal static class DroneOrientation
|
||||
{
|
||||
public static float AccX, AccY, AccZ;
|
||||
public static float GyrX, GyrY, GyrZ;
|
||||
public static uint TimeAcc, TimeGyr;
|
||||
private static bool isFirstCycle = true;
|
||||
public static uint prevTimeAcc, prevTimeGyr;
|
||||
private static Telemetry telemetry;
|
||||
|
||||
public static float PosX, PosY;
|
||||
public static float LaserRange;
|
||||
public static uint TimeRange;
|
||||
public static uint prevTimeRange;
|
||||
|
||||
public static Vector2 OF = Vector2.Zero;
|
||||
|
||||
public static float MotorUL, MotorUR, MotorDL, MotorDR;
|
||||
|
||||
public const float K_ACC = 0.1f;
|
||||
|
||||
public static Quaternion attitude = new();
|
||||
|
||||
/***************************************************************/
|
||||
private static Quaternion attitudeTarget = new();
|
||||
private static Vector ratesTarget = new();
|
||||
private static Vector torqueTarget = new();
|
||||
private static float thrustTarget = 0;
|
||||
/***************************************************************/
|
||||
private static AdaptivePID RollPid = new();
|
||||
private static AdaptivePID PitchPid = new();
|
||||
|
||||
//private static AdaptivePID YawPid = new();
|
||||
|
||||
private static AdaptivePID RatesRollPid = new(telemetry.pid0[0], telemetry.pid0[1], telemetry.pid0[2], 0.0f, 0, 0);
|
||||
private static AdaptivePID RatesPitchPid = new(telemetry.pid0[3], telemetry.pid0[4], telemetry.pid0[5], 0.01f, 0.05f, 0.001f);
|
||||
private static AdaptivePID RatesYawPid = new(telemetry.pid0[6], telemetry.pid0[7], telemetry.pid0[8], 0.01f, 0.05f, 0.001f);
|
||||
/***************************************************************/
|
||||
private static float tim1s = 0;
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// Вычисление текущей ориентации дрона
|
||||
/// </summary>
|
||||
public static void CalcAttitude(Drone droneData, RC rc)
|
||||
{
|
||||
readData(droneData);
|
||||
|
||||
prevTimeAcc = TimeAcc;
|
||||
prevTimeGyr = TimeGyr;
|
||||
|
||||
|
||||
prevTimeRange = TimeRange;
|
||||
while (true)
|
||||
{
|
||||
readData(droneData);
|
||||
if(prevTimeGyr == 0)
|
||||
{
|
||||
prevTimeGyr = TimeGyr;
|
||||
continue;
|
||||
}
|
||||
float gyro_dt = TimeGyr - prevTimeGyr;
|
||||
tim1s += gyro_dt;
|
||||
if ((prevTimeGyr > 0) && (gyro_dt > 0))
|
||||
{
|
||||
gyro_dt /= 1000;
|
||||
thrustTarget = rc.GetThrottle();
|
||||
attitudeTarget = Quaternion.FromEulerAngles(rc.GetControl());
|
||||
Vector rates = new Vector(-GyrY, -GyrX, GyrZ);
|
||||
//Vector rates = new Vector(0, 0, 0);
|
||||
rates *= Vector.TO_RAD;
|
||||
//Quaternion q_rates = Quaternion.FromRotationVector(rates * (gyro_dt));
|
||||
//attitude = Quaternion.Mul(attitude, q_rates);
|
||||
attitude.UpdateByRates(rates, gyro_dt);
|
||||
attitude.Normalize();
|
||||
Vector errorPos = CalculateErrorAxisAngle();
|
||||
|
||||
if (tim1s > 1000)
|
||||
{
|
||||
//Debug.WriteLine($"Flix = {CalculateErrorFlix().ToString()}");
|
||||
Debug.WriteLine($"Axis = {CalculateErrorAxisAngle().ToString()}");
|
||||
Debug.WriteLine($"Current = {attitude.ToString()}");
|
||||
Debug.WriteLine($"Target = {attitudeTarget.ToString()}");
|
||||
tim1s = 0;
|
||||
}
|
||||
//ratesTarget.X = RollPid.process(errorPos.X, gyro_dt);
|
||||
//ratesTarget.Y = PitchPid.process(errorPos.Y, gyro_dt);
|
||||
|
||||
//Vector errorRates = ratesTarget - rates;
|
||||
|
||||
torqueTarget.X = RatesRollPid.process(errorPos.X, gyro_dt);
|
||||
torqueTarget.Y = RatesPitchPid.process(errorPos.Y, gyro_dt);
|
||||
torqueTarget.Z = RatesYawPid.process(errorPos.Z, gyro_dt);
|
||||
//torqueTarget.X = RatesRollPid.process(errorRates.X, gyro_dt);
|
||||
//torqueTarget.Y = RatesPitchPid.process(errorRates.Y, gyro_dt);
|
||||
//torqueTarget.Z = RatesYawPid.process(errorRates.Z, gyro_dt);
|
||||
|
||||
/*MotorUL = thrustTarget + torqueTarget.X - torqueTarget.Y + torqueTarget.Z;
|
||||
MotorUR = thrustTarget - torqueTarget.X - torqueTarget.Y - torqueTarget.Z;
|
||||
MotorDL = thrustTarget + torqueTarget.X + torqueTarget.Y + torqueTarget.Z;
|
||||
MotorDR = thrustTarget - torqueTarget.X + torqueTarget.Y + torqueTarget.Z;*/
|
||||
MotorUL = thrustTarget - torqueTarget.X - torqueTarget.Y + torqueTarget.Z;
|
||||
MotorUR = thrustTarget + torqueTarget.X - torqueTarget.Y - torqueTarget.Z;
|
||||
MotorDL = thrustTarget - torqueTarget.X + torqueTarget.Y - torqueTarget.Z;
|
||||
MotorDR = thrustTarget + torqueTarget.X + torqueTarget.Y + torqueTarget.Z;
|
||||
|
||||
MotorUL = (float)Vector.constraint(MotorUL, 0, 1);
|
||||
MotorUR = (float)Vector.constraint(MotorUR, 0, 1);
|
||||
MotorDL = (float)Vector.constraint(MotorDL, 0, 1);
|
||||
MotorDR = (float)Vector.constraint(MotorDR, 0, 1);
|
||||
|
||||
droneData.writeDataMotor4(MotorUL, MotorUR, MotorDL, MotorDR);
|
||||
|
||||
prevTimeGyr = TimeGyr;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
if (TimeAcc - prevTimeAcc > 0)
|
||||
{
|
||||
float AccNorm = (float)Math.Sqrt(AccX * AccX + AccY * AccY + AccZ * AccZ);
|
||||
if (Math.Abs(AccNorm - 1) < 0.05)
|
||||
{
|
||||
//Получение кватерниона, описывающего вращение вектора Z(0, 0, 1) в положение вектора ускорений
|
||||
Quaternion accRotation = Quaternion.QuaternionBetweenVectors(new Vector(AccX, AccY, AccZ), new Vector(0, 0, 1));
|
||||
|
||||
//Коррекция по акселерометру
|
||||
//accRotation *= K_ACC;
|
||||
attitude = accRotation;
|
||||
attitude.Normalize();
|
||||
}
|
||||
prevTimeAcc = TimeAcc;
|
||||
}*/
|
||||
}
|
||||
}
|
||||
|
||||
public static Quaternion GetAttitude()
|
||||
{
|
||||
return Quaternion.Copy(attitude);
|
||||
}
|
||||
|
||||
private static void CalculateControls(float throttle, float phi, float theta, float psi)
|
||||
{
|
||||
thrustTarget = throttle;
|
||||
attitudeTarget = Quaternion.FromEulerAngles(new Vector(phi, theta, psi));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Вычисление ошибки как в flix
|
||||
/// </summary>
|
||||
private static Vector CalculateErrorFlix()
|
||||
{
|
||||
Vector up = new Vector(0, 0, 1);
|
||||
Vector current = Quaternion.RotateVector(up, attitude);
|
||||
Vector target = Quaternion.RotateVector(up, attitudeTarget);
|
||||
return Vector.getRotationVector(target, current);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Вычисление ошибки через форму ось-угол
|
||||
/// </summary>
|
||||
private static Vector CalculateErrorAxisAngle()
|
||||
{
|
||||
Quaternion error = Quaternion.Mul(attitudeTarget, attitude.getInverse());
|
||||
error.Normalize();
|
||||
float angle = 2 * (float)Math.Acos(error.W);
|
||||
Vector n = new Vector(error.X, error.Y, error.Z);
|
||||
n.Normalize();
|
||||
return n*angle;
|
||||
}
|
||||
|
||||
private static void readData(Drone droneData)
|
||||
{
|
||||
float[]acc = droneData.readDataAcc();
|
||||
AccX = acc[0]; AccY = acc[1]; AccZ = acc[2]; TimeAcc = (uint)acc[3];
|
||||
|
||||
float[] gyr = droneData.readDataGyr();
|
||||
GyrX = gyr[0]; GyrY = gyr[1]; GyrZ = gyr[2]; TimeGyr = (uint)gyr[3];
|
||||
|
||||
float[] pos = droneData.readDataLocal();
|
||||
PosX = pos[0]; PosY = pos[1];
|
||||
|
||||
float[] range = droneData.readDataRange();
|
||||
LaserRange = range[0]; TimeRange = (uint)range[1];
|
||||
}
|
||||
}
|
||||
}
|
||||
178
DroneClient/utils/Quaternion.cs
Normal file
178
DroneClient/utils/Quaternion.cs
Normal file
@@ -0,0 +1,178 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace DroneClient.utils
|
||||
{
|
||||
internal class Quaternion
|
||||
{
|
||||
private float w;
|
||||
private float x;
|
||||
private float y;
|
||||
private float z;
|
||||
|
||||
public float W
|
||||
{
|
||||
get => w;
|
||||
private set => w = value;
|
||||
}
|
||||
public float X
|
||||
{
|
||||
get => x;
|
||||
private set => x = value;
|
||||
}
|
||||
public float Y
|
||||
{
|
||||
get => y;
|
||||
private set => y = value;
|
||||
}
|
||||
public float Z
|
||||
{
|
||||
get => z;
|
||||
private set => z = value;
|
||||
}
|
||||
|
||||
public Quaternion()
|
||||
{
|
||||
w = 1;
|
||||
x = y = z = 0;
|
||||
}
|
||||
|
||||
public Quaternion(float w, float x, float y, float z)
|
||||
{
|
||||
this.w = w;
|
||||
this.x = x;
|
||||
this.y = y;
|
||||
this.z = z;
|
||||
}
|
||||
|
||||
public bool IsZero()
|
||||
{
|
||||
return w == 0 && x == 0 && y == 0 && z == 0;
|
||||
}
|
||||
|
||||
public float getNorm()
|
||||
{
|
||||
if (IsZero()) return 0;
|
||||
return (float)Math.Sqrt(w * w + x * x + y * y + z * z);
|
||||
}
|
||||
|
||||
public void Normalize()
|
||||
{
|
||||
float norm = getNorm();
|
||||
if (norm != 0)
|
||||
{
|
||||
w /= norm;
|
||||
x /= norm;
|
||||
y /= norm;
|
||||
z /= norm;
|
||||
}
|
||||
}
|
||||
|
||||
static public Quaternion FromRotationVector(Vector Va)
|
||||
{
|
||||
float a = Va.getNorm();
|
||||
if (a == 0)
|
||||
{
|
||||
return new Quaternion();
|
||||
}
|
||||
|
||||
Quaternion w = new Quaternion();
|
||||
float cos = (float)Math.Cos(a / 2);
|
||||
float sin = (float)Math.Sin(a / 2);
|
||||
|
||||
w.w = cos;
|
||||
w.x = Va.X / a * sin;
|
||||
w.y = Va.Y / a * sin;
|
||||
w.z = Va.Z / a * sin;
|
||||
return w;
|
||||
}
|
||||
|
||||
static public Quaternion FromEulerAngles(Vector euler)
|
||||
{
|
||||
float halfphi = euler.X / 2;
|
||||
float halftheta = euler.Y / 2;
|
||||
float halfpsi= euler.Z / 2;
|
||||
float cosx = (float)Math.Cos(halfphi);
|
||||
float sinx = (float)Math.Sin(halfphi);
|
||||
float cosy = (float)Math.Cos(halftheta);
|
||||
float siny = (float)Math.Sin(halftheta);
|
||||
float cosz = (float)Math.Cos(halfpsi);
|
||||
float sinz = (float)Math.Sin(halfpsi);
|
||||
|
||||
return new Quaternion(
|
||||
cosx * cosy * cosz + sinx * siny * sinz,
|
||||
sinx * cosy * cosz - cosx * siny * sinz,
|
||||
cosx * siny * cosz + sinx * cosy * sinz,
|
||||
cosx * cosy * sinz - sinx * siny * cosz);
|
||||
}
|
||||
|
||||
static public Quaternion Copy(Quaternion q)
|
||||
{
|
||||
return new Quaternion(q.w, q.x, q.y, q.z);
|
||||
}
|
||||
|
||||
public Quaternion getInverse()
|
||||
{
|
||||
float n = getNorm();
|
||||
return new Quaternion(w / n, -x / n, -y / n, -z / n);
|
||||
}
|
||||
|
||||
static public Quaternion Mul(Quaternion q1, Quaternion q2)
|
||||
{
|
||||
Quaternion res = new Quaternion();
|
||||
res.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
|
||||
res.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
|
||||
res.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
|
||||
res.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;
|
||||
return res;
|
||||
}
|
||||
|
||||
public void UpdateByRates(Vector w, float dt)
|
||||
{
|
||||
Quaternion omega = new Quaternion(0, w.X, w.Y, w.Z);
|
||||
Quaternion res = Mul(this, omega);
|
||||
this.w += res.w * 0.5f * dt;
|
||||
this.x += res.x * 0.5f * dt;
|
||||
this.y += res.y * 0.5f * dt;
|
||||
this.z += res.z * 0.5f * dt;
|
||||
}
|
||||
|
||||
static public Vector RotateVector(Vector a, Quaternion q)
|
||||
{
|
||||
Quaternion fromVector = new Quaternion(0, a.X, a.Y, a.Z);
|
||||
Quaternion inverse = q.getInverse();
|
||||
Quaternion r = Mul(q, fromVector);
|
||||
r = Mul(r, inverse);
|
||||
return new Vector(r.x, r.y, r.z);
|
||||
}
|
||||
static public Quaternion QuaternionBetweenVectors(Vector a, Vector b)
|
||||
{
|
||||
Quaternion res = new Quaternion();
|
||||
Vector mul_v = Vector.Mul_Vector(a, b);
|
||||
//Если векторы коллинеарны, то нет поворота
|
||||
if (mul_v.IsZero()) return res;
|
||||
|
||||
float mul_s = Vector.Mul_Scalar(a, b);
|
||||
//Хотя бы один из векторов нулевой => кватернион неопределен, но мы возвращаем (1, 0, 0, 0)
|
||||
if (mul_s == 0) return res;
|
||||
|
||||
float norm_scalar = a.getNorm() * b.getNorm() + mul_s;
|
||||
float normQ = (float)Math.Sqrt(norm_scalar * norm_scalar + mul_v.getNorm() * mul_v.getNorm());
|
||||
|
||||
res.w = norm_scalar / normQ;
|
||||
res.x = mul_v.X / normQ;
|
||||
res.y = mul_v.Y / normQ;
|
||||
res.z = mul_v.Z / normQ;
|
||||
return res;
|
||||
}
|
||||
|
||||
override public string ToString()
|
||||
{
|
||||
return $"({w}, {x}, {y}, {z})";
|
||||
}
|
||||
}
|
||||
}
|
||||
45
DroneClient/utils/RC.cs
Normal file
45
DroneClient/utils/RC.cs
Normal file
@@ -0,0 +1,45 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace DroneClient.utils
|
||||
{
|
||||
internal class RC
|
||||
{
|
||||
private Vector control = new();
|
||||
private float throttle = 0;
|
||||
|
||||
public void SetControl(float roll, float pitch, float yaw)
|
||||
{
|
||||
control.X = roll;
|
||||
control.Y = pitch;
|
||||
control.Z = yaw;
|
||||
}
|
||||
|
||||
public void SetRoll(float roll)
|
||||
{
|
||||
control.X = roll;
|
||||
}
|
||||
|
||||
public void SetPitch(float pitch)
|
||||
{
|
||||
control.Y = pitch;
|
||||
}
|
||||
|
||||
public void SetYaw(float yaw)
|
||||
{
|
||||
control.Z = yaw;
|
||||
}
|
||||
|
||||
public void SetThrottle(float throttle)
|
||||
{
|
||||
this.throttle = throttle;
|
||||
}
|
||||
|
||||
public Vector GetControl() { return control; }
|
||||
|
||||
public float GetThrottle() { return throttle; }
|
||||
}
|
||||
}
|
||||
74
DroneClient/utils/UtilityUI.cs
Normal file
74
DroneClient/utils/UtilityUI.cs
Normal file
@@ -0,0 +1,74 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace DroneClient.utils
|
||||
{
|
||||
static class UtilityUI
|
||||
{
|
||||
public static Color ColorFromHsb(float hue, float saturation, float brightness)
|
||||
{
|
||||
// Ограничиваем значения параметров
|
||||
hue = Math.Clamp(hue, 0, 360);
|
||||
saturation = Math.Clamp(saturation, 0, 1);
|
||||
brightness = Math.Clamp(brightness, 0, 1);
|
||||
|
||||
float c = brightness * saturation;
|
||||
float x = c * (1 - Math.Abs((hue / 60) % 2 - 1));
|
||||
float m = brightness - c;
|
||||
|
||||
float r, g, b;
|
||||
|
||||
if (hue < 60) { r = c; g = x; b = 0; }
|
||||
else if (hue < 120) { r = x; g = c; b = 0; }
|
||||
else if (hue < 180) { r = 0; g = c; b = x; }
|
||||
else if (hue < 240) { r = 0; g = x; b = c; }
|
||||
else if (hue < 300) { r = x; g = 0; b = c; }
|
||||
else { r = c; g = 0; b = x; }
|
||||
|
||||
return Color.FromArgb(
|
||||
(int)((r + m) * 255),
|
||||
(int)((g + m) * 255),
|
||||
(int)((b + m) * 255));
|
||||
}
|
||||
|
||||
public static void ColorToHsb(Color color, out float hue, out float saturation, out float brightness)
|
||||
{
|
||||
float r = color.R / 255f;
|
||||
float g = color.G / 255f;
|
||||
float b = color.B / 255f;
|
||||
|
||||
float max = Math.Max(r, Math.Max(g, b));
|
||||
float min = Math.Min(r, Math.Min(g, b));
|
||||
float delta = max - min;
|
||||
|
||||
// Hue calculation
|
||||
if (delta == 0)
|
||||
{
|
||||
hue = 0;
|
||||
}
|
||||
else if (max == r)
|
||||
{
|
||||
hue = 60 * (((g - b) / delta) % 6);
|
||||
}
|
||||
else if (max == g)
|
||||
{
|
||||
hue = 60 * (((b - r) / delta) + 2);
|
||||
}
|
||||
else
|
||||
{
|
||||
hue = 60 * (((r - g) / delta) + 4);
|
||||
}
|
||||
|
||||
if (hue < 0) hue += 360;
|
||||
|
||||
// Saturation calculation
|
||||
saturation = max == 0 ? 0 : delta / max;
|
||||
|
||||
// Brightness/Value
|
||||
brightness = max;
|
||||
}
|
||||
}
|
||||
}
|
||||
148
DroneClient/utils/Vector.cs
Normal file
148
DroneClient/utils/Vector.cs
Normal file
@@ -0,0 +1,148 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Data;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
using System.Windows.Forms;
|
||||
|
||||
namespace DroneClient.utils
|
||||
{
|
||||
internal class Vector
|
||||
{
|
||||
public const float TO_GRAD = (float)(180 / Math.PI);
|
||||
public const float TO_RAD = (float)(Math.PI / 180);
|
||||
private float x, y, z;
|
||||
|
||||
public float X
|
||||
{
|
||||
get => x;
|
||||
set => x = value;
|
||||
}
|
||||
|
||||
public float Y
|
||||
{
|
||||
get => y;
|
||||
set => y = value;
|
||||
}
|
||||
|
||||
public float Z
|
||||
{
|
||||
get => z;
|
||||
set => z = value;
|
||||
}
|
||||
|
||||
public Vector() { }
|
||||
|
||||
public Vector(float x, float y, float z)
|
||||
{
|
||||
this.x = x;
|
||||
this.y = y;
|
||||
this.z = z;
|
||||
}
|
||||
|
||||
public bool IsZero()
|
||||
{
|
||||
return x == 0 && y == 0 && z == 0;
|
||||
}
|
||||
|
||||
public float getNorm()
|
||||
{
|
||||
if (IsZero()) return 0;
|
||||
return (float)Math.Sqrt(x * x + y * y + z * z);
|
||||
}
|
||||
|
||||
public void Normalize()
|
||||
{
|
||||
if (IsZero()) return;
|
||||
float norm = getNorm();
|
||||
x /= norm;
|
||||
y /= norm;
|
||||
z /= norm;
|
||||
}
|
||||
|
||||
static public Vector operator *(float a, Vector v)
|
||||
{
|
||||
return new Vector(a * v.x, a * v.y, a * v.z);
|
||||
}
|
||||
|
||||
static public Vector operator *(Vector v, float a)
|
||||
{
|
||||
return new Vector(a * v.x, a * v.y, a * v.z);
|
||||
}
|
||||
|
||||
static public Vector operator -(Vector a, Vector b)
|
||||
{
|
||||
return new Vector(a.x - b.x, a.y - b.y, a.z - b.z);
|
||||
}
|
||||
|
||||
static public float Mul_Scalar(Vector a, Vector b)
|
||||
{
|
||||
return a.x * b.x + a.y * b.y + a.z * b.z;
|
||||
}
|
||||
|
||||
static public Vector Mul_Vector(Vector a, Vector b)
|
||||
{
|
||||
return new Vector(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
|
||||
}
|
||||
|
||||
static public float AngleBetweenVectors(Vector a, Vector b)
|
||||
{
|
||||
float mul_scalar = Mul_Scalar(a, b);
|
||||
float normA = a.getNorm();
|
||||
float normB = b.getNorm();
|
||||
|
||||
if ((normA == 0) || (normB == 0))
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return (float)Math.Acos(Vector.constraint(mul_scalar / normA / normB));
|
||||
}
|
||||
}
|
||||
|
||||
static public float AngleBetweenVectors2(Vector a, Vector b)
|
||||
{
|
||||
float normAB = Mul_Vector(a, b).getNorm();
|
||||
float normA = a.getNorm();
|
||||
float normB = b.getNorm();
|
||||
|
||||
if ((normA == 0) || (normB == 0))
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return (float)Math.Asin(Vector.constraint(normAB / normA / normB));
|
||||
}
|
||||
}
|
||||
|
||||
static public Vector getRotationVector(Vector a, Vector b)
|
||||
{
|
||||
Vector n = Vector.Mul_Vector(a, b);
|
||||
if (n.IsZero())
|
||||
{
|
||||
return Vector.Mul_Vector(a, new Vector(1, 0, 0));
|
||||
}
|
||||
n.Normalize();
|
||||
return Vector.AngleBetweenVectors(a, b) * n;
|
||||
}
|
||||
|
||||
///<summary>
|
||||
///Ограничивает value в пределах между low и high. Если low >= high будет возвращено value.
|
||||
///</summary>
|
||||
static public double constraint(double value, double low = -1, double high = 1)
|
||||
{
|
||||
if (low >= high) return value;
|
||||
if (value < low) return low;
|
||||
if (value > high) return high;
|
||||
return value;
|
||||
}
|
||||
|
||||
override public string ToString()
|
||||
{
|
||||
return $"({x}, {y}, {z})";
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user