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

*Добавлены реализации кватерниона и векторов

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;
+}