diff --git a/Source/INS/IRS.h b/Source/INS/IRS.h index 3ea1c9b..e853bcb 100644 --- a/Source/INS/IRS.h +++ b/Source/INS/IRS.h @@ -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 diff --git a/Source/INS/geometry/quaternion.c b/Source/INS/geometry/quaternion.c deleted file mode 100644 index 3bb8a46..0000000 --- a/Source/INS/geometry/quaternion.c +++ /dev/null @@ -1,167 +0,0 @@ -#include "quaternion.h" -#include - -#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; -} - - - - - - - - - - - - - diff --git a/Source/INS/geometry/quaternion.h b/Source/INS/geometry/quaternion.h deleted file mode 100644 index d3513b2..0000000 --- a/Source/INS/geometry/quaternion.h +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once - -#ifndef QUATERNION_H -#define QUATERNION_H - -#include "vector.h" -#include - -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 - diff --git a/Source/INS/geometry/vector.c b/Source/INS/geometry/vector.c deleted file mode 100644 index 092946d..0000000 --- a/Source/INS/geometry/vector.c +++ /dev/null @@ -1,174 +0,0 @@ -#include "vector.h" -#include - -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; -} - - - - - - - - - - - diff --git a/Source/INS/geometry/vector.h b/Source/INS/geometry/vector.h deleted file mode 100644 index eebf4ad..0000000 --- a/Source/INS/geometry/vector.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/Source/MathEnv/Quaternion.cpp b/Source/MathEnv/Quaternion.cpp new file mode 100644 index 0000000..5b7e0b5 --- /dev/null +++ b/Source/MathEnv/Quaternion.cpp @@ -0,0 +1,265 @@ +#include "Quaternion.h" +#include + + +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; +} \ No newline at end of file diff --git a/Source/MathEnv/Quaternion.h b/Source/MathEnv/Quaternion.h new file mode 100644 index 0000000..052be43 --- /dev/null +++ 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 \ No newline at end of file diff --git a/Source/MathEnv/Vector.cpp b/Source/MathEnv/Vector.cpp new file mode 100644 index 0000000..32c12e6 --- /dev/null +++ b/Source/MathEnv/Vector.cpp @@ -0,0 +1,458 @@ +#include "Vector.h" +#include "Quaternion.h" +#include + + +// ========================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, + }; +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Source/MathEnv/Vector.h b/Source/MathEnv/Vector.h new file mode 100644 index 0000000..ed50dcc --- /dev/null +++ b/Source/MathEnv/Vector.h @@ -0,0 +1,95 @@ +#pragma once + +#ifndef VECTOR_H +#define VECTOR_H + +#include + +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 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 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 \ No newline at end of file diff --git a/Source/main.cpp b/Source/main.cpp new file mode 100644 index 0000000..4a41551 --- /dev/null +++ b/Source/main.cpp @@ -0,0 +1,6 @@ +#include "stm32g4xx.h" + +int main() +{ + return 0; +} \ No newline at end of file