Обновлено математическое окружение прошивки

*Добавлены реализации кватерниона и векторов
2026-04-16 13:19:08 +03:00
parent 52922afeb1
commit da4dfbfae5
10 changed files with 872 additions and 427 deletions
--- a/Source/MathEnv/Quaternion.cpp
+++ b/Source/MathEnv/Quaternion.cpp
@@ -0,0 +1,265 @@
+#include "Quaternion.h"
+#include <math.h>
+
+
+void Quaternion::Zero()
+{
+  X = 0.0f; Y = 0.0f; Z = 0.0f; W = 1.0f;
+}
+
+Quaternion Quaternion::Norm(float Gain) const
+{
+  float norm = sqrtf(X * X + Y * Y + Z * Z + W * W);
+
+  if (norm > 1e-6f)
+  {
+    norm = Gain / norm;
+
+    return
+    {
+      X * norm,
+      Y * norm,
+      Z * norm,
+      W * norm,
+    };
+  }
+  
+  return { 0.0f, 0.0f, 0.0f, 0.0f };
+}
+
+Quaternion Quaternion::Conjugate() const
+{
+  return { -X, -Y, -Z, W };
+}
+
+Quaternion Quaternion::Invert() const
+{
+  float nsq = X * X + Y * Y + Z * Z + W * W;
+
+  if (nsq > 1e-6f)
+  {
+    nsq = 1.0f / nsq;
+
+    return
+    {
+      -X * nsq,
+      -Y * nsq,
+      -Z * nsq,
+      W * nsq,
+    };
+  }
+
+  return { 0.0f, 0.0f, 0.0f, 0.0f };
+}
+
+Quaternion Quaternion::Negate() const
+{
+  return { -X, -Y, -Z, -W };
+}
+
+bool Quaternion::IsNAN() const
+{
+  return (X != X) || (Y != Y) || (Z != Z) || (W != W);
+}
+
+Quaternion& Quaternion::operator=(const Quaternion& Q)
+{
+  W = Q.W;
+  X = Q.X;
+  Y = Q.Y;
+  Z = Q.Z;
+
+  return *this;
+}
+
+Quaternion& Quaternion::operator+=(const Quaternion& Q)
+{
+  X += Q.X;
+  Y += Q.Y;
+  Z += Q.Z;
+  W += Q.W;
+
+  return *this;
+}
+
+Quaternion& Quaternion::operator-=(const Quaternion& Q)
+{
+  X -= Q.X;
+  Y -= Q.Y;
+  Z -= Q.Z;
+  W -= Q.W;
+
+  return *this;
+}
+
+Quaternion& Quaternion::operator*=(const float Value)
+{
+  X *= Value;
+  Y *= Value;
+  Z *= Value;
+  W *= Value;
+
+  return *this;
+}
+
+Quaternion& Quaternion::operator*=(const Quaternion& Q)
+{
+  const float x = X;
+  const float y = Y;
+  const float z = Z;
+  const float w = W;
+  
+  X = w * Q.X + x * Q.W + y * Q.Z - z * Q.Y;
+  Y = w * Q.Y - x * Q.Z + y * Q.W + z * Q.X;
+  Z = w * Q.Z + x * Q.Y - y * Q.X + z * Q.W;
+  W = w * Q.W - x * Q.X - y * Q.Y - z * Q.Z;
+
+  return *this;
+}
+
+Quaternion Quaternion::operator*(const float Value) const
+{
+  return
+  {
+    X * Value,
+    Y * Value,
+    Z * Value,
+    W * Value,
+  };
+}
+
+Quaternion Quaternion::operator*(const Quaternion& Q) const
+{
+  return
+  {
+    W * Q.X + X * Q.W + Y * Q.Z - Z * Q.Y,
+    W * Q.Y - X * Q.Z + Y * Q.W + Z * Q.X,
+    W * Q.Z + X * Q.Y - Y * Q.X + Z * Q.W,
+    W * Q.W - X * Q.X - Y * Q.Y - Z * Q.Z,
+  };
+}
+
+Quaternion Quaternion::operator+(const Quaternion& Q) const
+{
+  return
+  {
+    X + Q.X,
+    Y + Q.Y,
+    Z + Q.Z,
+    W + Q.W,
+  };
+}
+
+Quaternion Quaternion::operator-(const Quaternion& Q) const
+{
+  return
+  {
+    X - Q.X,
+    Y - Q.Y,
+    Z - Q.Z,
+    W - Q.W,
+  };
+}
+
+Vector3 Quaternion::Rotate(const Vector3& vec) const
+{
+  Quaternion p = { vec.X, vec.Y, vec.Z, 0.0f };
+  
+  // Вычисляем p' = q * p * q^ (q^ - сопряженный)
+  Quaternion rotated = *this * p * this->Conjugate();
+  
+  // Возвращаем векторную часть результата
+  return { rotated.X, rotated.Y, rotated.Z };
+}
+
+Vector3 Quaternion::RotateAroundZ(const Vector3& vec, bool CCW) const
+{
+  float yaw_sin_term = 2.0f * (W * Z + X * Y);
+  float yaw_cos_term = 1.0f - 2.0f * (Y * Y + Z * Z);
+  
+  float mag_sq = yaw_sin_term * yaw_sin_term + yaw_cos_term * yaw_cos_term;
+  
+  if (mag_sq < 1e-6f) return vec;
+  
+  float inv_mag = 1.0f / sqrtf(mag_sq); 
+  
+  float c = yaw_cos_term * inv_mag;
+  float s = yaw_sin_term * inv_mag;
+
+  if (CCW) s = -s;
+
+  return
+  {
+    vec.X * c - vec.Y * s,
+    vec.X * s + vec.Y * c,
+    vec.Z
+  };
+}
+
+Quaternion Quaternion::CreateYawPitchRoll(const Vector3& PitchRollYawRad) // Глобальный поворот
+{
+  float hp = 0.5f * PitchRollYawRad.X;
+  float hr = 0.5f * PitchRollYawRad.Y;
+  float hy = 0.5f * PitchRollYawRad.Z;
+
+  float cr = cosf(hr), sr = sinf(hr);
+  float cp = cosf(hp), sp = sinf(hp);
+  float cy = cosf(hy), sy = sinf(hy);
+  
+  return // Это эквивалент q_roll(Y) * q_pitch(X) * q_yaw(Z) [Yaw -> Pitch -> Roll]
+  {
+    cr * sp * cy - sr * cp * sy,
+    sr * cp * cy + cr * sp * sy,
+   -cr * cp * sy - sr * sp * cy,
+    cr * cp * cy - sr * sp * sy
+  };
+}
+
+Quaternion Quaternion::CreatePitchRollYaw(const Vector3& PitchRollYawRad) // Локальный поворот
+{
+  float hp = 0.5f * PitchRollYawRad.X;
+  float hr = 0.5f * PitchRollYawRad.Y;
+  float hy = 0.5f * PitchRollYawRad.Z;
+
+  float cr = cosf(hr), sr = sinf(hr);
+  float cp = cosf(hp), sp = sinf(hp);
+  float cy = cosf(hy), sy = sinf(hy);
+  
+  return // Это эквивалент  q_yaw(Z) * q_roll(Y) * q_pitch(X) [ Pitch -> Roll -> Yaw ]
+  {
+    cy * cr * sp + sy * sr * cp,
+    cy * sr * cp - sy * cr * sp,
+   -cr * cp * sy - cy * sr * sp,
+    cy * cr * cp - sy * sr * sp
+  };
+}
+
+Quaternion Quaternion::CreateYaw(const float YawRad)
+{
+  float hy = - 0.5f * YawRad;
+  return { 0.0f, 0.0f, sinf(hy), cosf(hy) };
+}
+
+Quaternion Quaternion::CreateDirection(const Vector2& Course)
+{
+  Vector2 xy = Course.Norm();
+  
+  if(xy.X < -0.999f) return { 0.0, 0.0, 1.0, 0.0 }; // Поворот на 180 градусов
+  float w = sqrtf((1.0f + xy.X) * 0.5f);
+  return { 0.0f, 0.0f, xy.Y / (2.0f * w), w };
+}
+
+Quaternion Quaternion::GetError(const Quaternion& Target, bool FastWay) const
+{
+  Quaternion error // Формула произведения Гамильтона с учетом инверсии current
+  {
+    W * Target.X - X * Target.W - Y * Target.Z + Z * Target.Y,
+    W * Target.Y + X * Target.Z - Y * Target.W - Z * Target.X,
+    W * Target.Z - X * Target.Y + Y * Target.X - Z * Target.W,
+    W * Target.W + X * Target.X + Y * Target.Y + Z * Target.Z
+  };
+  
+  if (FastWay && (error.W < 0.0f)) return error.Negate();
+  
+  return error;
+}
--- a/Source/MathEnv/Quaternion.h
+++ b/Source/MathEnv/Quaternion.h
@@ -0,0 +1,46 @@
+#pragma once
+
+#ifndef QUATERNION_H
+#define QUATERNION_H
+
+#include "Vector.h"
+
+struct Quaternion
+{
+  float X, Y, Z, W;
+  
+  Quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) { }
+  Quaternion(float v) : X(v), Y(v), Z(v), W(v) { }
+  Quaternion(float x, float y, float z, float w) : X(x), Y(y), Z(z), W(w) { }
+  Quaternion(const Vector3& Vec, float w = 0.0f) : X(Vec.X), Y(Vec.Y), Z(Vec.Z), W(w) { }
+
+  void Zero();
+
+  Quaternion Norm(float Gain = 1.0f) const;
+  Quaternion Conjugate() const;
+  Quaternion Invert() const;
+  Quaternion Negate() const;
+  bool IsNAN() const;
+
+  Quaternion& operator=(const Quaternion& Q);
+  Quaternion& operator+=(const Quaternion& Q);
+  Quaternion& operator-=(const Quaternion& Q);
+  Quaternion& operator*=(const float Value);
+  Quaternion& operator*=(const Quaternion& Q);
+  Quaternion operator*(const float Value) const;
+  Quaternion operator*(const Quaternion& Q) const;
+  Quaternion operator+(const Quaternion& Q) const;
+  Quaternion operator-(const Quaternion& Q) const;
+
+  Vector3 Rotate(const Vector3& vec) const;
+  Vector3 RotateAroundZ(const Vector3& vec, bool CCW = false) const;
+  
+  static Quaternion CreateYawPitchRoll(const Vector3& PitchRollYawRad);
+  static Quaternion CreatePitchRollYaw(const Vector3& PitchRollYawRad);
+  static Quaternion CreateYaw(const float YawRad);
+  static Quaternion CreateDirection(const Vector2& Course);
+  
+  Quaternion GetError(const Quaternion& Target, bool FastWay) const;
+};
+
+#endif
--- a/Source/MathEnv/Vector.cpp
+++ b/Source/MathEnv/Vector.cpp
@@ -0,0 +1,458 @@
+#include "Vector.h"
+#include "Quaternion.h"
+#include <math.h>
+
+
+// ========================Vector2========================
+Vector2::Vector2(const Vector3& Vec) : X(Vec.X), Y(Vec.Y) {}
+
+void Vector2::Zero()
+{
+  X = Y = 0;
+}
+
+Vector2 Vector2::Norm(float Gain) const
+{
+  float n = sqrtf(X * X + Y * Y);
+
+  if (n > 1e-12f)
+  {
+    n = Gain / n;
+
+    return
+    {
+      X * n,
+      Y * n
+    };
+  }
+
+  return {0, 0};
+}
+
+Vector2 Vector2::Abs() const
+{
+  return { fabsf(X) , fabsf(Y) };
+}
+
+float Vector2::Length() const
+{
+  return sqrtf(X * X + Y * Y);
+}
+
+float Vector2::LengthSquared() const
+{
+  return X * X + Y * Y;
+}
+
+bool Vector2::IsNAN() const
+{
+  return (X != X) || (Y != Y);
+}
+
+bool Vector2::IsFinite() const 
+{
+  return (X - X == 0) && (Y - Y == 0);
+}
+
+Vector2 Vector2::Limit(float Max, float Min) const
+{
+  Vector2 lim;
+  
+  if (X > Max) lim.X = Max; else if (X < Min) lim.X = Min; else lim.X = X;
+  if (Y > Max) lim.Y = Max; else if (Y < Min) lim.Y = Min; else lim.Y = Y;
+  
+  return lim;
+}
+
+Vector2 Vector2::Power(float Pow) const
+{
+  return
+  {
+    powf(X, Pow),
+    powf(Y, Pow)
+  };
+}
+
+float Vector2::Dot(const Vector2& Vec) const
+{
+  return X * Vec.X + Y * Vec.Y;
+}
+
+Vector2& Vector2::operator+=(const Vector2& Vec)
+{
+  X += Vec.X;
+  Y += Vec.Y;
+
+  return *this;
+}
+
+Vector2& Vector2::operator-=(const Vector2& Vec)
+{
+  X -= Vec.X;
+  Y -= Vec.Y;
+
+  return *this;
+}
+
+Vector2& Vector2::operator*=(float Val)
+{
+  X *= Val;
+  Y *= Val;
+
+  return *this;
+}
+
+Vector2& Vector2::operator*=(const Vector2& Vec)
+{
+  X *= Vec.X;
+  Y *= Vec.Y;
+
+  return *this;
+}
+
+Vector2& Vector2::operator/=(float Val)
+{
+  X /= Val;
+  Y /= Val;
+
+  return *this;
+}
+
+Vector2& Vector2::operator=(float Val)
+{
+  X = Val;
+  Y = Val;
+
+  return *this;
+}
+
+Vector2 Vector2::operator*(float Val) const
+{
+  return
+  {
+    X * Val,
+    Y * Val
+  };
+}
+
+Vector2 Vector2::operator*(const Vector2& Vec) const
+{
+  return
+  {
+    X * Vec.X,
+    Y * Vec.Y
+  };
+}
+
+Vector2 Vector2::operator+(const Vector2& Vec) const
+{
+  return
+  {
+    X + Vec.X,
+    Y + Vec.Y
+  };
+}
+
+Vector2 Vector2::operator-(const Vector2& Vec) const
+{
+  return
+  {
+    X - Vec.X,
+    Y - Vec.Y
+  };
+}
+
+Vector2 Vector2::operator/(float Val) const
+{
+  return
+  {
+    X / Val,
+    Y / Val
+  };
+}
+
+Vector2 operator-(float Val, const Vector2& Vec)
+{
+  return
+  {
+    Val - Vec.X,
+    Val - Vec.Y
+  };
+}
+
+
+// ========================Vector3========================
+Vector3::Vector3(const struct Quaternion& q) : X(q.X),Y(q.Y),Z(q.Z) { }
+
+void Vector3::Zero()
+{
+  X = Y = Z = 0;
+}
+
+Vector3 Vector3::Norm(float Gain) const
+{
+  float n = sqrtf(X * X + Y * Y + Z * Z);
+
+  if (n > 1e-12f)
+  {
+    n = Gain / n;
+
+    return
+    {
+      X * n,
+      Y * n,
+      Z * n
+    };
+  }
+
+  return {0, 0};
+}
+
+Vector3 Vector3::Abs() const
+{
+  return { fabsf(X) , fabsf(Y), fabsf(Z) };
+}
+
+float Vector3::Length() const
+{
+  return sqrtf(X * X + Y * Y + Z * Z);
+}
+
+float Vector3::LengthSquared() const
+{
+  return X * X + Y * Y + Z * Z;
+}
+
+Vector3 Vector3::Copy() const
+{
+  return { X, Y, Z };
+}
+
+bool Vector3::IsNAN() const
+{
+  return (X != X) || (Y != Y) || (Z != Z);
+}
+
+bool Vector3::IsFinite() const 
+{
+  return (X - X == 0) && (Y - Y == 0) && (Z - Z == 0);
+}
+
+Vector3 Vector3::Limit(float Max, float Min) const
+{
+  Vector3 lim;
+  
+  if (X > Max) lim.X = Max; else if (X < Min) lim.X = Min; else lim.X = X;
+  if (Y > Max) lim.Y = Max; else if (Y < Min) lim.Y = Min; else lim.Y = Y;
+  if (Z > Max) lim.Z = Max; else if (Z < Min) lim.Z = Min; else lim.Z = Z;
+  
+  return lim;
+}
+
+Vector3 Vector3::Power(float Pow) const
+{
+  return
+  {
+    powf(X, Pow),
+    powf(Y, Pow),
+    powf(Z, Pow)
+  };
+}
+
+float Vector3::Dot(const Vector3& Vec) const
+{
+  return X * Vec.X + Y * Vec.Y + Z * Vec.Z;
+}
+
+Vector3 Vector3::Cross(const Vector3& Vec) const
+{
+  return
+  {
+    Y * Vec.Z - Z * Vec.Y,
+    Z * Vec.X - X * Vec.Z,
+    X * Vec.Y - Y * Vec.X
+  };
+}
+
+Vector3& Vector3::operator+=(const Vector3& Vec)
+{
+  X += Vec.X;
+  Y += Vec.Y;
+  Z += Vec.Z;
+
+  return *this;
+}
+
+Vector3& Vector3::operator+=(float Val)
+{
+  X += Val;
+  Y += Val;
+  Z += Val;
+
+  return *this;
+}
+
+Vector3& Vector3::operator-=(const Vector3& Vec)
+{
+  X -= Vec.X;
+  Y -= Vec.Y;
+  Z -= Vec.Z;
+
+  return *this;
+}
+
+Vector3& Vector3::operator-=(float Val)
+{
+  X -= Val;
+  Y -= Val;
+  Z -= Val;
+
+  return *this;
+}
+
+Vector3& Vector3::operator*=(float Val)
+{
+  X *= Val;
+  Y *= Val;
+  Z *= Val;
+
+  return *this;
+}
+
+Vector3& Vector3::operator*=(const Vector3& Vec)
+{
+  X *= Vec.X;
+  Y *= Vec.Y;
+  Z *= Vec.Z;
+
+  return *this;
+}
+
+Vector3& Vector3::operator/=(float Val)
+{
+  X /= Val;
+  Y /= Val;
+  Z /= Val;
+
+  return *this;
+}
+
+Vector3& Vector3::operator=(const struct Quaternion& Quat)
+{
+  X = Quat.X;
+  Y = Quat.Y;
+  Z = Quat.Z;
+
+  return *this;
+}
+
+Vector3& Vector3::operator=(float Val)
+{
+  X = Val;
+  Y = Val;
+  Z = Val;
+
+  return *this;
+}
+
+Vector3 Vector3::operator*(float Val) const
+{
+  return
+  {
+    X * Val,
+    Y * Val,
+    Z * Val,
+  };
+}
+
+Vector3 Vector3::operator*(const Vector3& Vec) const
+{
+  return
+  {
+    X * Vec.X,
+    Y * Vec.Y,
+    Z * Vec.Z,
+  };
+}
+
+Vector3 Vector3::operator+(float Val) const
+{
+  return
+  {
+    X + Val,
+    Y + Val,
+    Z + Val,
+  };
+}
+
+Vector3 Vector3::operator+(const Vector3& Vec) const
+{
+  return
+  {
+    X + Vec.X,
+    Y + Vec.Y,
+    Z + Vec.Z,
+  };
+}
+
+Vector3 Vector3::operator-(const Vector3& Vec) const
+{
+  return
+  {
+    X - Vec.X,
+    Y - Vec.Y,
+    Z - Vec.Z,
+  };
+}
+
+Vector3 Vector3::operator/(float Val) const
+{
+  return
+  {
+    X / Val,
+    Y / Val,
+    Z / Val,
+  };
+}
+
+Vector3 operator-(float Val, const Vector3& Vec)
+{
+  return
+  {
+    Val - Vec.X,
+    Val - Vec.Y,
+    Val - Vec.Z,
+  };
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
--- a/Source/MathEnv/Vector.h
+++ b/Source/MathEnv/Vector.h
@@ -0,0 +1,95 @@
+#pragma once
+
+#ifndef VECTOR_H
+#define VECTOR_H
+
+#include <functional>
+
+struct Vector2
+{
+  float X = 0.0f, Y = 0.0f;
+  
+  Vector2(const float v = 0) : X(v), Y(v) {}
+  Vector2(const struct Vector3& Vec);
+  Vector2(const Vector2& Vec) : X(Vec.X), Y(Vec.Y) {}
+  Vector2(float x, float y) : X(x), Y(y) {}
+  
+  void Zero();
+  
+  Vector2 Norm(float Gain = 1.0f) const;
+  Vector2 Abs() const;
+  float Length() const;
+  float LengthSquared() const;
+  bool IsNAN() const;
+  bool IsFinite() const;
+  Vector2 Limit(float Min, float Max) const;
+  Vector2 Power(float Pow) const;
+  float Dot(const Vector2& Vec) const;
+  
+  template<typename T> Vector2 Action(T Act) const { return { Act(X), Act(Y) }; };
+  
+  Vector2& operator+=(const Vector2& Vec);
+  Vector2& operator-=(const Vector2& Vec);
+  Vector2& operator*=(float Val);
+  Vector2& operator*=(const Vector2& Vec);
+  Vector2& operator/=(float Val);
+
+  Vector2& operator=(float Val);
+
+  Vector2 operator*(float Val) const;
+  Vector2 operator*(const Vector2& Vec) const;
+  Vector2 operator+(const Vector2& Vec) const;
+  Vector2 operator-(const Vector2& Vec) const;
+  Vector2 operator/(float Val) const;
+  
+  friend Vector2 operator-(float Val, const Vector2& Vec);
+};
+
+struct Vector3
+{
+  float X = 0.0f, Y = 0.0f, Z = 0.0f;
+  
+  Vector3(const float v = 0) : X(v), Y(v), Z(v) {}
+  Vector3(const Vector2& Vec, const float z = 0.0f) : X(Vec.X), Y(Vec.Y), Z(z) {}
+  Vector3(const Vector3& Vec) : X(Vec.X), Y(Vec.Y), Z(Vec.Z) {}
+  Vector3(float x, float y, float z): X(x), Y(y), Z(z) {}
+  Vector3(const struct Quaternion& q);
+  
+  void Zero();
+
+  Vector3 Norm(float Gain = 1.0f) const;
+  Vector3 Abs() const;
+  float Length() const;
+  float LengthSquared() const;
+  Vector3 Copy() const;
+  bool IsNAN() const;
+  bool IsFinite() const;
+  Vector3 Limit(float Max, float Min) const;
+  Vector3 Power(float Pow) const;
+  float Dot(const Vector3& Vec) const;
+  Vector3 Cross(const Vector3& Vec) const;
+  
+  template<typename T> Vector3 Action(T Act) const { return { Act(X), Act(Y), Act(Z) }; };
+
+  Vector3& operator+=(const Vector3& Vec);
+  Vector3& operator+=(float Val);
+  Vector3& operator-=(const Vector3& Vec);
+  Vector3& operator-=(float Val);
+  Vector3& operator*=(float Val);
+  Vector3& operator*=(const Vector3& Vec);
+  Vector3& operator/=(float Val);
+
+  Vector3& operator=(const struct Quaternion& Quat);
+  Vector3& operator=(float Val);
+
+  Vector3 operator*(float Val) const;
+  Vector3 operator*(const Vector3& Vec) const;
+  Vector3 operator+(float Val) const;
+  Vector3 operator+(const Vector3& Vec) const;
+  Vector3 operator-(const Vector3& Vec) const;
+  Vector3 operator/(float Val) const;
+
+  friend Vector3 operator-(float Val, const Vector3& Vec);
+};
+
+#endif