150 lines
3.4 KiB
C
150 lines
3.4 KiB
C
#include "quaternion.h"
|
|
#include <math.h>
|
|
|
|
Quaternion QuatNormalize(const Quaternion* q, const float gain)
|
|
{
|
|
Quaternion res;
|
|
|
|
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 = {.x = q1->x + q2->x, .y = q1->y + q2->y, .z = q1->z + q2->z, .w = 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 = {.x = q->x * value, .y = q->y * value, .z = q->z * value, .w = q->w * value};
|
|
return res;
|
|
}
|
|
|
|
Quaternion QuatProd(const Quaternion* q1, const Quaternion* q2)
|
|
{
|
|
Quaternion res = {
|
|
.x = q1->w * q2->x + q1->x * q2->w + q1->y * q2->z - q1->z * q2->y,
|
|
.y = q1->w * q2->y + q1->x * q2->z + q1->y * q2->w - q1->z * q2->x,
|
|
.z = q1->w * q2->z + q1->x * q2->y + q1->y * q2->x - q1->z * q2->w,
|
|
.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 QuatCreateRollPitchYaw(const Vector3* RollPitchYawRad)
|
|
{
|
|
Quaternion res;
|
|
|
|
float h_r = 0.5f * RollPitchYawRad->y;
|
|
float h_p = 0.5f * RollPitchYawRad->x;
|
|
float h_y = 0.5f * RollPitchYawRad->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;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|