126 lines
2.7 KiB
C++
126 lines
2.7 KiB
C++
#include "math/lin/quat.h"
|
|
#include "math/lin/matrix4x4.h"
|
|
|
|
void Quaternion::toMatrix(Matrix4x4 *out) const {
|
|
Matrix4x4 temp;
|
|
temp.setIdentity();
|
|
float ww, xx, yy, zz, wx, wy, wz, xy, xz, yz;
|
|
ww = w*w; xx = x*x; yy = y*y; zz = z*z;
|
|
wx = w*x*2; wy = w*y*2; wz = w*z*2;
|
|
xy = x*y*2; xz = x*z*2; yz = y*z*2;
|
|
|
|
temp.xx = ww + xx - yy - zz;
|
|
temp.xy = xy + wz;
|
|
temp.xz = xz - wy;
|
|
|
|
temp.yx = xy - wz;
|
|
temp.yy = ww - xx + yy - zz;
|
|
temp.yz = yz + wx;
|
|
|
|
temp.zx = xz + wy;
|
|
temp.zy = yz - wx;
|
|
temp.zz = ww - xx - yy + zz;
|
|
|
|
*out = temp;
|
|
}
|
|
|
|
Quaternion Quaternion::fromMatrix(Matrix4x4 &m)
|
|
{
|
|
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
|
|
// article "Quaternion Calculus and Fast Animation".
|
|
Quaternion q(0,0,0,1);
|
|
/*
|
|
float fTrace = m[0][0] + m[1][1] + m[2][2];
|
|
float fRoot;
|
|
|
|
if( fTrace > 0.0 )
|
|
{
|
|
fRoot = sqrtf( fTrace + 1.0f );
|
|
|
|
q.w = 0.5f * fRoot;
|
|
|
|
fRoot = 0.5f / fRoot;
|
|
|
|
q.x = ( m[2][1] - m[1][2] ) * fRoot;
|
|
q.y = ( m[0][2] - m[2][0] ) * fRoot;
|
|
q.z = ( m[1][0] - m[0][1] ) * fRoot;
|
|
}
|
|
else
|
|
{
|
|
int iNext[3] = { 1, 2, 0 };
|
|
|
|
int i = 0;
|
|
if( m[1][1] > m[0][0] )
|
|
i = 1;
|
|
|
|
if( m[2][2] > m[i][i] )
|
|
i = 2;
|
|
|
|
int j = iNext[i];
|
|
int k = iNext[j];
|
|
|
|
fRoot = sqrtf( m[i][i] - m[j][j] - m[k][k] + 1.0f );
|
|
|
|
float *apfQuat = &q.x;
|
|
|
|
apfQuat[i] = 0.5f * fRoot;
|
|
|
|
fRoot = 0.5f / fRoot;
|
|
|
|
q.w = ( m[k][j] - m[j][k] ) * fRoot;
|
|
|
|
apfQuat[j] = ( m[j][i] + m[i][j] ) * fRoot;
|
|
apfQuat[k] = ( m[k][i] + m[i][k] ) * fRoot;
|
|
}
|
|
q.normalize(); */
|
|
return q;
|
|
};
|
|
|
|
// TODO: Allegedly, lerp + normalize can achieve almost as good results.
|
|
Quaternion Quaternion::slerp(const Quaternion &to, const float a) const {
|
|
Quaternion to2;
|
|
float angle, cos_angle, scale_from, scale_to, sin_angle;
|
|
|
|
cos_angle = (x * to.x) + (y * to.y) + (z * to.z) + (w * to.w); //4D dot product
|
|
|
|
if (cos_angle < 0.0f)
|
|
{
|
|
cos_angle = -cos_angle;
|
|
to2.w = -to.w; to2.x = -to.x; to2.y = -to.y; to2.z = -to.z;
|
|
}
|
|
else
|
|
{
|
|
to2 = to;
|
|
}
|
|
|
|
if ((1.0f - fabsf(cos_angle)) > 0.00001f)
|
|
{
|
|
/* spherical linear interpolation (SLERP) */
|
|
angle = acosf(cos_angle);
|
|
sin_angle = sinf(angle);
|
|
scale_from = sinf((1.0f - a) * angle) / sin_angle;
|
|
scale_to = sinf(a * angle) / sin_angle;
|
|
}
|
|
else
|
|
{
|
|
/* to prevent divide-by-zero, resort to linear interpolation */
|
|
// This is okay in 99% of cases anyway, maybe should be the default?
|
|
scale_from = 1.0f - a;
|
|
scale_to = a;
|
|
}
|
|
|
|
return Quaternion(
|
|
scale_from*x + scale_to*to2.x,
|
|
scale_from*y + scale_to*to2.y,
|
|
scale_from*z + scale_to*to2.z,
|
|
scale_from*w + scale_to*to2.w
|
|
);
|
|
}
|
|
|
|
Quaternion Quaternion::multiply(const Quaternion &q) const {
|
|
return Quaternion((w * q.x) + (x * q.w) + (y * q.z) - (z * q.y),
|
|
(w * q.y) + (y * q.w) + (z * q.x) - (x * q.z),
|
|
(w * q.z) + (z * q.w) + (x * q.y) - (y * q.x),
|
|
(w * q.w) - (x * q.x) - (y * q.y) - (z * q.z));
|
|
}
|