119 lines
2.4 KiB
C++
119 lines
2.4 KiB
C++
#include "uquat.h"
|
|
#include "umatrix.h"
|
|
#include "ueuler.h"
|
|
|
|
static const float s_slerpDelta = float ( 1e-6 );
|
|
|
|
UQuat::UQuat
|
|
(
|
|
const UEuler &angles
|
|
)
|
|
|
|
{
|
|
UQuat working;
|
|
|
|
vec3 ().setXYZ ( 0.f, 1.f, 0.f );
|
|
SetupRotation ( angles.yaw );
|
|
|
|
working.vec3 ().setXYZ ( 1.f, 0.f, 0.f );
|
|
working.SetupRotation ( angles.pitch );
|
|
*this *= working;
|
|
|
|
working.vec3 ().setXYZ ( 0.f, 0.f, 1.f );
|
|
working.SetupRotation ( angles.roll );
|
|
*this *= working;
|
|
}
|
|
|
|
void UQuat::operator =
|
|
(
|
|
const UMat3 &mat
|
|
)
|
|
|
|
{
|
|
float
|
|
trace,
|
|
s;
|
|
|
|
trace = mat.vecs[0].x + mat.vecs[1].y + mat.vecs[2].z;
|
|
|
|
if ( trace > 0.f )
|
|
{
|
|
s = Sqrt ( trace + 1.f );
|
|
UQuat::w = s * 0.5f;
|
|
s = 0.5f / s;
|
|
|
|
UQuat::x = ( mat.vecs[2].y - mat.vecs[1].z ) * s;
|
|
UQuat::y = ( mat.vecs[0].z - mat.vecs[2].x ) * s;
|
|
UQuat::z = ( mat.vecs[1].x - mat.vecs[0].y ) * s;
|
|
}
|
|
else
|
|
{
|
|
int
|
|
i,
|
|
j,
|
|
k;
|
|
static int
|
|
s_next[3] = { 1, 2, 0 };
|
|
|
|
i = 0;
|
|
if ( mat.vecs[1].y > mat.vecs[0].x )
|
|
i = 1;
|
|
if ( mat.vecs[2].z > mat.vecs[i][i] )
|
|
i = 2;
|
|
|
|
j = s_next[i];
|
|
k = s_next[j];
|
|
|
|
s = Sqrt ( ( mat.vecs[i][i] - ( mat.vecs[j][j] + mat.vecs[k][k] ) ) + 1.f );
|
|
UQuat::operator[](i) = s * 0.5f;
|
|
|
|
s = 0.5f / s;
|
|
|
|
UQuat::w = ( mat.vecs[k][j] - mat.vecs[j][k] ) * s;
|
|
UQuat::operator[](j) = ( mat.vecs[j][i] + mat.vecs[i][j] ) * s;
|
|
UQuat::operator[](k) = ( mat.vecs[k][i] + mat.vecs[i][k] ) * s;
|
|
}
|
|
}
|
|
|
|
void UQuat::Slerp
|
|
(
|
|
const UQuat &from,
|
|
const UQuat &to,
|
|
float t
|
|
)
|
|
|
|
{
|
|
float omega, cosom, sinom, scale0, scale1;
|
|
float scaleto;
|
|
|
|
if ( from == to )
|
|
{
|
|
*this = from;
|
|
return;
|
|
}
|
|
|
|
cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;
|
|
scaleto = 1.f;
|
|
|
|
if ( cosom < 0.f )
|
|
{
|
|
cosom = -cosom;
|
|
scaleto = -1.f;
|
|
}
|
|
|
|
if ( 1.f - cosom > s_slerpDelta )
|
|
{
|
|
omega = Acos ( cosom );
|
|
sinom = Sin ( omega );
|
|
scale0 = Sin ( ( 1.f - t ) * omega ) / sinom;
|
|
scale1 = Sin ( t * omega ) / sinom;
|
|
}
|
|
else
|
|
{
|
|
scale0 = 1.f - t;
|
|
scale1 = t;
|
|
}
|
|
|
|
*this = ( scale0 * from ) + ( scaleto * scale1 * to );
|
|
}
|
|
|