Обновлено математическое окружение прошивки
*Добавлены реализации кватерниона и векторов
This commit is contained in:
@@ -1,8 +1,8 @@
|
||||
#ifndef IRS_H
|
||||
#define IRS_H
|
||||
|
||||
#include "quaternion.h"
|
||||
#include "vector.h"
|
||||
#include "Quaternion.h"
|
||||
#include "Vector.h"
|
||||
|
||||
#define PI 3.14159265359f
|
||||
#define DEG2RAD PI / 180.0f
|
||||
|
||||
@@ -1,167 +0,0 @@
|
||||
#include "quaternion.h"
|
||||
#include <math.h>
|
||||
|
||||
#define PI 3.14159265359f
|
||||
|
||||
Quaternion QuatNormalize(const Quaternion* q, const float gain)
|
||||
{
|
||||
Quaternion res = {0.0f, 0.0f, 0.0f, 0.0f};
|
||||
|
||||
float norm = sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
|
||||
|
||||
if (norm > 1e-6f)
|
||||
{
|
||||
norm = gain / norm;
|
||||
|
||||
res.x = q->x * norm;
|
||||
res.y = q->y * norm;
|
||||
res.z = q->z * norm;
|
||||
res.w = q->w * norm;
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Quaternion QuatConjugate(const Quaternion* q)
|
||||
{
|
||||
Quaternion res = {.x = -q->x, .y = -q->y, .z = -q->z, .w = q->w};
|
||||
return res;
|
||||
}
|
||||
|
||||
Quaternion QuatInvert(const Quaternion* q)
|
||||
{
|
||||
Quaternion res;
|
||||
float nsq = q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w;
|
||||
|
||||
if (nsq > 1e-6f)
|
||||
{
|
||||
nsq = 1.0f / nsq;
|
||||
|
||||
res.x = q->x * nsq;
|
||||
res.y = q->y * nsq;
|
||||
res.z = q->z * nsq;
|
||||
res.w = q->w * nsq;
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
return *q;
|
||||
}
|
||||
|
||||
Quaternion QuatNegate(const Quaternion* q)
|
||||
{
|
||||
Quaternion res = {.x = -q->x, .y = -q->y, .z = -q->z, .w = -q->w};
|
||||
return res;
|
||||
}
|
||||
|
||||
Quaternion QuatSum(const Quaternion* q1, const Quaternion* q2)
|
||||
{
|
||||
Quaternion res = {q1->x + q2->x, q1->y + q2->y, q1->z + q2->z, q1->w + q2->w};
|
||||
return res;
|
||||
}
|
||||
|
||||
Quaternion QuatDiff(const Quaternion* q1, const Quaternion* q2)
|
||||
{
|
||||
Quaternion res = {.x = q1->x - q2->x, .y = q1->y - q2->y, .z = q1->z - q2->z, .w = q1->w - q2->w};
|
||||
return res;
|
||||
}
|
||||
|
||||
Quaternion QuatConstProd(const Quaternion* q, const float value)
|
||||
{
|
||||
Quaternion res = {q->x * value, q->y * value, q->z * value, q->w * value};
|
||||
return res;
|
||||
}
|
||||
|
||||
Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2)
|
||||
{
|
||||
Quaternion res =
|
||||
{
|
||||
q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y,
|
||||
q1->w * q2->y + q1->x * q2->z + q1->y * q2->w - q1->z * q2->x,
|
||||
q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w,
|
||||
q1->w * q2->w + q1->x * q2->x + q1->y * q2->y - q1->z * q2->z
|
||||
};
|
||||
return res;
|
||||
}
|
||||
|
||||
Vector3 QuatRotateAroundZ(const Quaternion* q, const Vector3* vec, bool CCW)
|
||||
{
|
||||
Quaternion v = {vec->x, vec->y, 0, 0};
|
||||
Quaternion h = {0, 0, CCW ? q->z : -q->z, q->w};
|
||||
h = QuatNormalize(&h, 1.0f);
|
||||
|
||||
Quaternion vhprod = QuatProd(&v, &h);
|
||||
h = QuatProd(&vhprod, &h);
|
||||
|
||||
Vector3 res = {h.x, h.y, vec->z};
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Quaternion QuatCreateFromEuler(const Vector3* eulerAngels)
|
||||
{
|
||||
Quaternion res;
|
||||
|
||||
float h_r = 0.5f * eulerAngels->y;
|
||||
float h_p = 0.5f * eulerAngels->x;
|
||||
float h_y = 0.5f * eulerAngels->z;
|
||||
|
||||
float c_r = cosf(h_r), s_r = sinf(h_r);
|
||||
float c_p = cosf(h_p), s_p = sinf(h_p);
|
||||
float c_y = cosf(h_y), s_y = sinf(h_y);
|
||||
|
||||
res.x = c_r * s_p * c_y - s_r * c_p * s_y; // Был +, стал -
|
||||
res.y = s_r * c_p * c_y + c_r * s_p * s_y; // Был -, стал +
|
||||
res.z = -c_r * c_p * s_y - s_r * s_p * c_y; // Первое слагаемое стало отрицательным
|
||||
res.w = c_r * c_p * c_y - s_r * s_p * s_y; // Был +, стал -
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, bool fastWay)
|
||||
{
|
||||
Quaternion error = {
|
||||
current->w * target->x + current->x * target->w + current->y * target->z - current->z * target->y,
|
||||
current->w * target->y + current->x * target->z + current->y * target->w - current->z * target->x,
|
||||
current->w * target->z + current->x * target->y + current->y * target->x - current->z * target->w,
|
||||
current->w * target->w + current->x * target->x + current->y * target->y - current->z * target->z
|
||||
};
|
||||
|
||||
if (fastWay && error.w < 0.0f) return QuatNegate(&error);
|
||||
|
||||
return error;
|
||||
}
|
||||
|
||||
Vector3 QuatToEuler(const Quaternion* q)
|
||||
{
|
||||
Vector3 e;
|
||||
|
||||
e.x = atan2f(2*(q->w*q->x + q->y*q->z),
|
||||
1 - 2*(q->x*q->x + q->y*q->y));
|
||||
|
||||
e.y = asinf(2*(q->w*q->y - q->z*q->x));
|
||||
|
||||
e.z = atan2f(2*(q->w*q->z + q->x*q->y),
|
||||
1 - 2*(q->y*q->y + q->z*q->z));
|
||||
|
||||
e.x *= 180.0f / PI;
|
||||
e.y *= 180.0f / PI;
|
||||
e.z *= 180.0f / PI;
|
||||
|
||||
return e;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,31 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#ifndef QUATERNION_H
|
||||
#define QUATERNION_H
|
||||
|
||||
#include "vector.h"
|
||||
#include <stdbool.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float x, y, z, w;
|
||||
} Quaternion;
|
||||
|
||||
Quaternion QuatNormalize(const Quaternion* q, const float gain);
|
||||
Quaternion QuatConjugate(const Quaternion* q);
|
||||
Quaternion QuatInvert(const Quaternion* q);
|
||||
Quaternion QuatNegate(const Quaternion* q);
|
||||
|
||||
Quaternion QuatSum(const Quaternion* q1, const Quaternion* q2);
|
||||
Quaternion QuatDiff(const Quaternion* q1, const Quaternion* q2);
|
||||
Quaternion QuatConstProd(const Quaternion* q, const float value);
|
||||
Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2);
|
||||
|
||||
Vector3 QuatRotateAroundZ(const Quaternion* q, const Vector3* vec, bool CCW);
|
||||
Quaternion QuatCreateFromEuler(const Vector3* eulerAngels);
|
||||
Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, bool fastWay);
|
||||
|
||||
Vector3 QuatToEuler(const Quaternion* q);
|
||||
|
||||
#endif
|
||||
|
||||
@@ -1,174 +0,0 @@
|
||||
#include "vector.h"
|
||||
#include <math.h>
|
||||
|
||||
Vector2 normalizeV2(const Vector2* v, float gain)
|
||||
{
|
||||
float len = lengthV2(v);
|
||||
Vector2 res = {.x = v->x / len, .y = v->y / len};
|
||||
return res;
|
||||
}
|
||||
|
||||
Vector3 normalizeV3(const Vector3* v, float gain)
|
||||
{
|
||||
Vector3 res = {0.0f, 0.0f, 0.0f};
|
||||
float n = lengthV3(v);
|
||||
|
||||
if (n > 1e-12f)
|
||||
{
|
||||
n = gain / n;
|
||||
res.x = v->x * n;
|
||||
res.y = v->y * n;
|
||||
res.z = v->z * n;
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
float DotV2(const Vector2* v1, const Vector2* v2)
|
||||
{
|
||||
float res = v1->x * v2->x + v1->y * v2->y;
|
||||
return res;
|
||||
}
|
||||
|
||||
float DotV3(const Vector3* v1, const Vector3* v2)
|
||||
{
|
||||
float res = v1->x * v2->x + v1->y * v2->y + v1->z * v2->z;
|
||||
return res;
|
||||
}
|
||||
|
||||
Vector2 absV2(const Vector2* v)
|
||||
{
|
||||
Vector2 res = {.x = fabsf(v->x), .y = fabsf(v->y)};
|
||||
return res;
|
||||
}
|
||||
|
||||
Vector3 absV3(const Vector3* v)
|
||||
{
|
||||
Vector3 res = {.x = fabsf(v->x), .y = fabsf(v->y), .z = fabsf(v->z)};
|
||||
return res;
|
||||
}
|
||||
|
||||
float lengthV2(const Vector2* v)
|
||||
{
|
||||
return sqrtf(v->x * v->x + v->y * v->y);
|
||||
}
|
||||
|
||||
float lengthV3(const Vector3* v)
|
||||
{
|
||||
return sqrtf(v->x * v->x + v->y * v->y + v->z * v->z);
|
||||
}
|
||||
|
||||
float lengthSquaredV2(const Vector2* v)
|
||||
{
|
||||
return v->x * v->x + v->y * v->y;
|
||||
}
|
||||
|
||||
float lengthSquaredV3(const Vector3* v)
|
||||
{
|
||||
return v->x * v->x + v->y * v->y + v->z * v->z;
|
||||
}
|
||||
|
||||
Vector2 limitV2(const Vector2* v, float min, float max)
|
||||
{
|
||||
Vector2 lim;
|
||||
|
||||
if (v->x < min) lim.x = min; else if (v->x > max) lim.x = max; else lim.x = v->x;
|
||||
if (v->y < min) lim.y = min; else if (v->y > max) lim.y = max; else lim.y = v->y;
|
||||
|
||||
return lim;
|
||||
}
|
||||
|
||||
Vector3 limitV3(const Vector3* v, float min, float max)
|
||||
{
|
||||
Vector3 lim;
|
||||
|
||||
if (v->x < min) lim.x = min; else if (v->x > max) lim.x = max; else lim.x = v->x;
|
||||
if (v->y < min) lim.y = min; else if (v->y > max) lim.y = max; else lim.y = v->y;
|
||||
if (v->z < min) lim.z = min; else if (v->z > max) lim.z = max; else lim.z = v->z;
|
||||
|
||||
return lim;
|
||||
}
|
||||
|
||||
Vector2 powerV2(const Vector2* v, float pow)
|
||||
{
|
||||
Vector2 res = {.x = powf(v->x, pow), .y = powf(v->y, pow)};
|
||||
return res;
|
||||
}
|
||||
|
||||
Vector3 powerV3(const Vector3* v, float pow)
|
||||
{
|
||||
Vector3 res = {.x = powf(v->x, pow), .y = powf(v->y, pow), .z = powf(v->z, pow)};
|
||||
return res;
|
||||
}
|
||||
|
||||
Vector2 sumV2(const Vector2* v1, const Vector2* v2)
|
||||
{
|
||||
Vector2 res = {.x = v1->x + v2->x, .y = v1->y + v2->y};
|
||||
return res;
|
||||
}
|
||||
|
||||
Vector3 sumV3(const Vector3* v1, const Vector3* v2)
|
||||
{
|
||||
Vector3 res = {.x = v1->x + v2->x, .y = v1->y + v2->y, .z = v1->z + v2->z};
|
||||
return res;
|
||||
}
|
||||
|
||||
Vector2 diffV2(const Vector2* v1, const Vector2* v2)
|
||||
{
|
||||
Vector2 res = {.x = v1->x - v2->x, .y = v1->y - v2->y};
|
||||
return res;
|
||||
}
|
||||
|
||||
Vector3 diffV3(const Vector3* v1, const Vector3* v2)
|
||||
{
|
||||
Vector3 res = {.x = v1->x - v2->x, .y = v1->y - v2->y, .z = v1->z - v2->z};
|
||||
return res;
|
||||
}
|
||||
|
||||
Vector2 constProdV2(const Vector2* v, float value)
|
||||
{
|
||||
Vector2 res = {.x = v->x * value, .y = v->y * value};
|
||||
return res;
|
||||
}
|
||||
|
||||
Vector3 constProdV3(const Vector3* v, float value)
|
||||
{
|
||||
Vector3 res = {v->x * value, v->y * value, v->z * value};
|
||||
return res;
|
||||
}
|
||||
|
||||
float scalarProdV2(const Vector2* v1, const Vector2* v2)
|
||||
{
|
||||
float res = v1->x * v2->x + v1->y * v2->y;
|
||||
return res;
|
||||
}
|
||||
|
||||
float scalarProdV3(const Vector3* v1, const Vector3* v2)
|
||||
{
|
||||
float res = v1->x * v2->x + v1->y * v2->y + v1->z * v2->z;
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
Vector3 Cross(const Vector3* v1, const Vector3* v2)
|
||||
{
|
||||
Vector3 res =
|
||||
{
|
||||
v1->y * v2->z - v1->z * v2->y,
|
||||
v1->z * v2->x - v1->x * v2->z,
|
||||
v1->x * v2->y - v1->y * v2->x
|
||||
};
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,53 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#ifndef VECTOR_H
|
||||
#define VECTOR_H
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float x, y;
|
||||
} Vector2;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float x, y, z;
|
||||
} Vector3;
|
||||
|
||||
Vector2 normalizeV2(const Vector2* v, float gain);
|
||||
Vector3 normalizeV3(const Vector3* v, float gain);
|
||||
|
||||
float DotV2(const Vector2* v1, const Vector2* v2);
|
||||
float DotV3(const Vector3* v1, const Vector3* v2);
|
||||
|
||||
Vector2 absV2(const Vector2* v);
|
||||
Vector3 absV3(const Vector3* v);
|
||||
|
||||
float lengthV2(const Vector2* v);
|
||||
float lengthV3(const Vector3* v);
|
||||
|
||||
float lengthSquaredV2(const Vector2* v);
|
||||
float lengthSquaredV3(const Vector3* v);
|
||||
|
||||
Vector2 limitV2(const Vector2* v, float min, float max);
|
||||
Vector3 limitV3(const Vector3* v, float min, float max);
|
||||
|
||||
Vector2 powerV2(const Vector2* v, float pow);
|
||||
Vector3 powerV3(const Vector3* v, float pow);
|
||||
|
||||
Vector2 sumV2(const Vector2* v1, const Vector2* v2);
|
||||
Vector3 sumV3(const Vector3* v1, const Vector3* v2);
|
||||
|
||||
Vector2 diffV2(const Vector2* v1, const Vector2* v2);
|
||||
Vector3 diffV3(const Vector3* v1, const Vector3* v2);
|
||||
|
||||
Vector2 constProdV2(const Vector2* v, float value);
|
||||
Vector3 constProdV3(const Vector3* v, float value);
|
||||
|
||||
float scalarProdV2(const Vector2* v1, const Vector2* v2);
|
||||
float scalarProdV3(const Vector3* v1, const Vector3* v2);
|
||||
|
||||
Vector2 vectorProdV2(const Vector2* v1, const Vector2* v2);
|
||||
Vector3 Cross(const Vector3* v1, const Vector3* v2);
|
||||
|
||||
#endif
|
||||
265
Source/MathEnv/Quaternion.cpp
Normal file
265
Source/MathEnv/Quaternion.cpp
Normal file
@@ -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;
|
||||
}
|
||||
46
Source/MathEnv/Quaternion.h
Normal file
46
Source/MathEnv/Quaternion.h
Normal file
@@ -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
|
||||
458
Source/MathEnv/Vector.cpp
Normal file
458
Source/MathEnv/Vector.cpp
Normal file
@@ -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,
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
95
Source/MathEnv/Vector.h
Normal file
95
Source/MathEnv/Vector.h
Normal file
@@ -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
|
||||
6
Source/main.cpp
Normal file
6
Source/main.cpp
Normal file
@@ -0,0 +1,6 @@
|
||||
#include "stm32g4xx.h"
|
||||
|
||||
int main()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user