|
@@ -4,29 +4,27 @@
|
|
#include "core/ToString.h"
|
|
#include "core/ToString.h"
|
|
|
|
|
|
Quaternion* axisAngleQ(Quaternion* q, const Vector3* axis, float angle) {
|
|
Quaternion* axisAngleQ(Quaternion* q, const Vector3* axis, float angle) {
|
|
- q->xyz = *axis;
|
|
|
|
- normalize(&q->xyz);
|
|
|
|
|
|
+ q->v.xyz = *axis;
|
|
|
|
+ normalize(&q->v.xyz);
|
|
angle *= 0.5f;
|
|
angle *= 0.5f;
|
|
- q->w = cosf(angle);
|
|
|
|
- mulSet(&q->xyz, sinf(angle));
|
|
|
|
|
|
+ q->v.w = cosf(angle);
|
|
|
|
+ mulSet(&q->v.xyz, sinf(angle));
|
|
return q;
|
|
return q;
|
|
}
|
|
}
|
|
|
|
|
|
Quaternion* lerpQ(Quaternion* q, const Quaternion* a, float f,
|
|
Quaternion* lerpQ(Quaternion* q, const Quaternion* a, float f,
|
|
const Quaternion* b) {
|
|
const Quaternion* b) {
|
|
- add(&q->xyz, mul(&a->xyz, 1.0f - f), mul(&b->xyz, f));
|
|
|
|
- q->w = a->w * (1.0f - f) + b->w * f;
|
|
|
|
- float iLength = 1.0f / sqrtf(squareLength(&q->xyz) + q->w * q->w);
|
|
|
|
- mulSet(&q->xyz, iLength);
|
|
|
|
- q->w *= iLength;
|
|
|
|
|
|
+ add(&q->v, mul(&a->v, 1.0f - f), mul(&b->v, f));
|
|
|
|
+ float iLength = 1.0f / length(&q->v);
|
|
|
|
+ mulSet(&q->v, iLength);
|
|
return q;
|
|
return q;
|
|
}
|
|
}
|
|
|
|
|
|
Quaternion* mulSetQ(Quaternion* q, const Quaternion* o) {
|
|
Quaternion* mulSetQ(Quaternion* q, const Quaternion* o) {
|
|
- float dot = dot(&q->xyz, &o->xyz);
|
|
|
|
- add(&q->xyz, mul(&o->xyz, q->w),
|
|
|
|
- add(mul(&q->xyz, o->w), cross(&q->xyz, &o->xyz)));
|
|
|
|
- q->w = q->w * o->w - dot;
|
|
|
|
|
|
+ float dot = dot(&q->v.xyz, &o->v.xyz);
|
|
|
|
+ add(&q->v.xyz, mul(&o->v.xyz, q->v.w),
|
|
|
|
+ add(mul(&q->v.xyz, o->v.w), cross(&q->v.xyz, &o->v.xyz)));
|
|
|
|
+ q->v.w = q->v.w * o->v.w - dot;
|
|
return q;
|
|
return q;
|
|
}
|
|
}
|
|
|
|
|
|
@@ -38,14 +36,14 @@ Quaternion* mulQ(Quaternion* q, const Quaternion* a, const Quaternion* b) {
|
|
|
|
|
|
Vector3* mulQV3(Vector3* r, const Quaternion* q, const Vector3* v) {
|
|
Vector3* mulQV3(Vector3* r, const Quaternion* q, const Vector3* v) {
|
|
Vector3 qv;
|
|
Vector3 qv;
|
|
- add(&qv, mul(v, q->w), cross(&q->xyz, v));
|
|
|
|
- add(r, mul(&q->xyz, dot(&q->xyz, v)),
|
|
|
|
- sub(mul(&qv, q->w), cross(&qv, &q->xyz)));
|
|
|
|
|
|
+ add(&qv, mul(v, q->v.w), cross(&q->v.xyz, v));
|
|
|
|
+ add(r, mul(&q->v.xyz, dot(&q->v.xyz, v)),
|
|
|
|
+ sub(mul(&qv, q->v.w), cross(&qv, &q->v.xyz)));
|
|
return r;
|
|
return r;
|
|
}
|
|
}
|
|
|
|
|
|
size_t toStringQ(const Quaternion* q, char* buffer, size_t n) {
|
|
size_t toStringQ(const Quaternion* q, char* buffer, size_t n) {
|
|
return toString(buffer, n, "(%.3f i + %.3f j + %.3f k + %.3f)",
|
|
return toString(buffer, n, "(%.3f i + %.3f j + %.3f k + %.3f)",
|
|
- (double)q->xyz.x, (double)q->xyz.y, (double)q->xyz.z,
|
|
|
|
- (double)q->w);
|
|
|
|
|
|
+ (double)q->v.x, (double)q->v.y, (double)q->v.z,
|
|
|
|
+ (double)q->v.w);
|
|
}
|
|
}
|