65 lines
1.5 KiB
C
65 lines
1.5 KiB
C
#include "quaternion.h"
|
|
|
|
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;
|
|
}
|