FQuat ToFQuat() const
{
static const float s = 0.70710678118f / 32767; // short -> range(sqrt(2))
float A = short(X & 0xFFFE) * s;
float B = short(Y & 0xFFFE) * s;
float C = short(Z & 0xFFFE) * s;
float D = sqrt(1.0f - (A*A + B*B + C*C));
if (Z & 1) D = -D;
FQuat r;
if (Y & 1)
{
if (X & 1) r.Set(D, A, B, C);
else r.Set(C, D, A, B);
}
else
{
if (X & 1) r.Set(B, C, D, A);
else r.Set(A, B, C, D);
}
return r;
}
FQuat ToFQuatOld() const // for version older than 151
{
static const float s = 0.000095876726845745f; // pi/32767
float X2 = X * s;
float Y2 = Y * s;
float Z2 = Z * s;
float tmp = sqrt(X2*X2 + Y2*Y2 + Z2*Z2);
if (tmp > 0)
{
float scale = sin(tmp / 2) / tmp; // strange code ...
X2 *= scale;
Y2 *= scale;
Z2 *= scale;
}
float W2 = 1.0f - (X2*X2 + Y2*Y2 + Z2*Z2);
if (W2 < 0) W2 = 0;
else W2 = sqrt(W2);
FQuat r;
r.Set(X2, Y2, Z2, W2);
return r;
}
static FQuat EulerToQuat(const FRotator &Rot)
{
// signs was taken from experiments
float yaw = Rot.Yaw * (M_PI / 32768.0f);
float pitch = -Rot.Pitch * (M_PI / 32768.0f);
float roll = -Rot.Roll * (M_PI / 32768.0f);
// derived from Doom3 idAngles::ToQuat()
float sz = sin(yaw / 2);
float cz = cos(yaw / 2);
float sy = sin(pitch / 2);
float cy = cos(pitch / 2);
float sx = sin(roll / 2);
float cx = cos(roll / 2);
float sxcy = sx * cy;
float cxcy = cx * cy;
float sxsy = sx * sy;
float cxsy = cx * sy;
FQuat Q;
Q.Set(cxsy*sz - sxcy*cz, -cxsy*cz - sxcy*sz, sxsy*cz - cxcy*sz, cxcy*cz + sxsy*sz);
return Q;
}