Обновлено математическое окружение прошивки
*Добавлены реализации кватерниона и векторов
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
|
||||
Reference in New Issue
Block a user