bisultanov/RaDrone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
*Добавлены реализации кватерниона и векторов
This commit is contained in:
Раджаб Бисултанов
2026-04-16 13:19:08 +03:00
parent 52922afeb1
commit da4dfbfae5
10 changed files with 872 additions and 427 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Source/INS/IRS.h
View File

@@ -1,8 +1,8 @@
#ifndef IRS_H #ifndef IRS_H
#define IRS_H #define IRS_H
#include "quaternion.h" #include "Quaternion.h"
#include "vector.h" #include "Vector.h"
#define PI 3.14159265359f #define PI 3.14159265359f
#define DEG2RAD PI / 180.0f #define DEG2RAD PI / 180.0f

167
Source/INS/geometry/quaternion.c
View File

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

31
Source/INS/geometry/quaternion.h
View File

@@ -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

174
Source/INS/geometry/vector.c
View File

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

53
Source/INS/geometry/vector.h
View File

@@ -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
View 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
View 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
View 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
View 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
View File

@@ -0,0 +1,6 @@
#include "stm32g4xx.h"
int main()
{
return 0;
}