const PIdivUU = 0.0000479369; // pi/65536
const UUdivPI = 10430.37835; // 32768/pi
// Quaternion, a rotation in 4D space.
struct Quaternion
{
var() float W, X, Y, Z;
};
// Add a rotator relative to the first one. Note: R1 & R2 != R2 & R1.
final operator(12) rotator & ( rotator R1, rotator R2 )
{
return QuatToRotation( RotationToQuat( R1 ) * RotationToQuat( R2 ) );
}
// Multiply 2 quaternions. Note: Q1 * Q2 != Q2 * Q1.
static final operator(16) Quaternion * ( Quaternion Q1, Quaternion Q2 )
{
local Quaternion Qp;
Qp.W = Q1.W * Q2.W - Q1.X * Q2.X - Q1.Y * Q2.Y - Q1.Z * Q2.Z;
Qp.X = Q1.W * Q2.X + Q1.X * Q2.W + Q1.Y * Q2.Z - Q1.Z * Q2.Y;
Qp.Y = Q1.W * Q2.Y + Q1.Y * Q2.W + Q1.Z * Q2.X - Q1.X * Q2.Z;
Qp.Z = Q1.W * Q2.Z + Q1.Z * Q2.W + Q1.X * Q2.Y - Q1.Y * Q2.X;
return Qp;
}
// Returns the arctangent of a given sine and cosine in a range of [0,2Pi].
static final function float Atan2( float Y, float X )
{
local float theta;
if( X==0 ) // Divide by 0 check.
{
if( Y<0 )
return 1.5 * pi;
else if( Y>0 )
return 0.5 * pi;
else
return 0; // Technically impossible (nothing exists).
}
theta = Atan( Y/X );
if( X<0 )
theta += pi; // 1st/3th quadrant.
// Normalize (from 0 to 2Pi).
if( theta<0 )
theta += 2.0 * pi;
return theta;
}
// Converts an Euler rotation to a quaternion.
static final function Quaternion RotationToQuat( rotator R )
{
local Quaternion QQ;
local float cp, cy, cr, sp, sy, sr;
// local float L;
cp = cos(R.Pitch * PIdivUU);
cy = cos(R.Yaw * PIdivUU);
cr = cos(R.Roll * PIdivUU);
sp = sin(R.Pitch * PIdivUU);
sy = sin(R.Yaw * PIdivUU);
sr = sin(R.Roll * PIdivUU);
QQ.W = cp * cy * cr + sp * sy * sr;
QQ.X = sp * cy * cr + cp * sy * sr;
QQ.Y = cp * sy * cr - sp * cy * sr;
QQ.Z = cp * cy * sr - sp * sy * cr;
// Normalize for higher precision, but not necessecary in my case.
// L = Sqrt( QQ.W**2 + QQ.X**2 + QQ.Y**2 + QQ.Z**2 );
// QQ.W /= L;
// QQ.X /= L;
// QQ.Y /= L;
// QQ.Z /= L;
return QQ;
}
// Converts a quaternion to an Euler rotation.
// Note that QuatToRotation(RotationToQuat(SomeRotator)) doesn't always
// return the SAME rotator, but it does return an EQUIVALENT rotator.
static final function rotator QuatToRotation( Quaternion Q )
{
local float sinPitchR, cosPitchR, sinYawR, cosYawR, sinRollR, cosRollR;
local rotator Ru;
sinPitchR = 2.0 * ( Q.X * Q.W - Q.Y * Q.Z );
cosPitchR = Sqrt( 1 - sinPitchR * sinPitchR );
Ru.Pitch = Atan2( sinPitchR, cosPitchR ) * UUdivPI;
if( cosPitchR == 0 )
{ // Argh no! Gimbal Lock!
sinYawR = 2.0 * ( Q.X * Q.Y - Q.Z * Q.W );
cosYawR = 2.0 * ( Q.Y * Q.Z + Q.X * Q.W );
Ru.Yaw = Atan2( sinYawR, cosYawR ) * UUdivPI;
Ru.Roll = 0;
// Yaw seems to be 32768 off if Pitch is 49152. This is a quick
// fix as I'm too lazy to search for the error.
if( sinPitchR < 0 )
Ru.Yaw += 32768;
}
else
{
sinYawR = 2.0 * ( Q.X * Q.Z + Q.Y * Q.W ) / cosPitchR;
cosYawR = ( 1.0 - 2.0 * Q.X * Q.X - 2.0 * Q.Y * Q.Y ) / cosPitchR;
Ru.Yaw = Atan2( sinYawR, cosYawR ) * UUdivPI;
sinRollR = 2.0 * ( Q.X * Q.Y + Q.Z * Q.W ) / cosPitchR;
cosRollR = ( 1.0 - 2.0 * Q.X * Q.X - 2.0 * Q.Z * Q.Z ) / cosPitchR;
Ru.Roll = Atan2( sinRollR, cosRollR ) * UUdivPI;
}
return Ru;
}