294 lines
5.6 KiB
C++
294 lines
5.6 KiB
C++
// Copyright (C) 2007 Id Software, Inc.
|
|
//
|
|
|
|
#include "../precompiled.h"
|
|
#pragma hdrstop
|
|
|
|
/*
|
|
=====================
|
|
idQuat::ToAngles
|
|
=====================
|
|
*/
|
|
idAngles idQuat::ToAngles( void ) const {
|
|
return ToMat3().ToAngles();
|
|
}
|
|
|
|
/*
|
|
=====================
|
|
idQuat::ToRotation
|
|
=====================
|
|
*/
|
|
idRotation idQuat::ToRotation( void ) const {
|
|
idVec3 vec;
|
|
float angle;
|
|
|
|
vec.x = x;
|
|
vec.y = y;
|
|
vec.z = z;
|
|
angle = idMath::ACos( w );
|
|
if ( angle == 0.0f ) {
|
|
vec.Set( 0.0f, 0.0f, 1.0f );
|
|
} else {
|
|
//vec *= (1.0f / sin( angle ));
|
|
vec.Normalize();
|
|
vec.FixDegenerateNormal();
|
|
angle *= 2.0f * idMath::M_RAD2DEG;
|
|
}
|
|
return idRotation( vec3_origin, vec, angle );
|
|
}
|
|
|
|
/*
|
|
=====================
|
|
idQuat::ToMat3
|
|
=====================
|
|
*/
|
|
idMat3 idQuat::ToMat3( void ) const {
|
|
idMat3 mat;
|
|
float wx, wy, wz;
|
|
float xx, yy, yz;
|
|
float xy, xz, zz;
|
|
float x2, y2, z2;
|
|
|
|
x2 = x + x;
|
|
y2 = y + y;
|
|
z2 = z + z;
|
|
|
|
xx = x * x2;
|
|
xy = x * y2;
|
|
xz = x * z2;
|
|
|
|
yy = y * y2;
|
|
yz = y * z2;
|
|
zz = z * z2;
|
|
|
|
wx = w * x2;
|
|
wy = w * y2;
|
|
wz = w * z2;
|
|
|
|
mat[ 0 ][ 0 ] = 1.0f - ( yy + zz );
|
|
mat[ 0 ][ 1 ] = xy - wz;
|
|
mat[ 0 ][ 2 ] = xz + wy;
|
|
|
|
mat[ 1 ][ 0 ] = xy + wz;
|
|
mat[ 1 ][ 1 ] = 1.0f - ( xx + zz );
|
|
mat[ 1 ][ 2 ] = yz - wx;
|
|
|
|
mat[ 2 ][ 0 ] = xz - wy;
|
|
mat[ 2 ][ 1 ] = yz + wx;
|
|
mat[ 2 ][ 2 ] = 1.0f - ( xx + yy );
|
|
|
|
return mat;
|
|
}
|
|
|
|
/*
|
|
=====================
|
|
idQuat::ToMat4
|
|
=====================
|
|
*/
|
|
idMat4 idQuat::ToMat4( void ) const {
|
|
return ToMat3().ToMat4();
|
|
}
|
|
|
|
/*
|
|
=====================
|
|
idQuat::ToCQuat
|
|
=====================
|
|
*/
|
|
idCQuat idQuat::ToCQuat( void ) const {
|
|
if ( w < 0.0f ) {
|
|
return idCQuat( -x, -y, -z );
|
|
}
|
|
return idCQuat( x, y, z );
|
|
}
|
|
|
|
/*
|
|
============
|
|
idQuat::ToAngularVelocity
|
|
============
|
|
*/
|
|
idVec3 idQuat::ToAngularVelocity( void ) const {
|
|
idVec3 vec;
|
|
float angle;
|
|
|
|
//
|
|
// FeaRog:
|
|
// Negative w basically indicates reverse rotation
|
|
// ACos will return a value in the range PI/2 <= x <= PI for negative w
|
|
// If this function doubled the angle (which is mathematically correct -> theta = 2*acos(w) )
|
|
// and then normalized it so it was in the range -PI/2 <= x <= PI/2
|
|
// then this function would return the correct value.
|
|
//
|
|
// However its easier & faster just to do the one branch and invert the axis
|
|
//
|
|
if ( w > 0.0f ) {
|
|
vec.x = x;
|
|
vec.y = y;
|
|
vec.z = z;
|
|
angle = idMath::ACos( w );
|
|
} else {
|
|
vec.x = -x;
|
|
vec.y = -y;
|
|
vec.z = -z;
|
|
angle = idMath::ACos( -w );
|
|
}
|
|
|
|
vec.Normalize();
|
|
return vec * angle;
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idQuat::ToString
|
|
=============
|
|
*/
|
|
const char *idQuat::ToString( int precision ) const {
|
|
return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idCQuat::ToAngles
|
|
=============
|
|
*/
|
|
idAngles idCQuat::ToAngles( void ) const {
|
|
return ToQuat().ToAngles();
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idCQuat::ToRotation
|
|
=============
|
|
*/
|
|
idRotation idCQuat::ToRotation( void ) const {
|
|
return ToQuat().ToRotation();
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idCQuat::ToMat3
|
|
=============
|
|
*/
|
|
idMat3 idCQuat::ToMat3( void ) const {
|
|
return ToQuat().ToMat3();
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idCQuat::ToMat4
|
|
=============
|
|
*/
|
|
idMat4 idCQuat::ToMat4( void ) const {
|
|
return ToQuat().ToMat4();
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idCQuat::ToString
|
|
=============
|
|
*/
|
|
const char *idCQuat::ToString( int precision ) const {
|
|
return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
|
|
}
|
|
|
|
/*
|
|
=====================
|
|
idQuat::Slerp
|
|
|
|
Spherical linear interpolation between two quaternions.
|
|
=====================
|
|
*/
|
|
idQuat &idQuat::Slerp( const idQuat &from, const idQuat &to, float t ) {
|
|
float cosom, absCosom, sinom, omega, scale0, scale1;
|
|
|
|
if ( t <= 0.0f ) {
|
|
*this = from;
|
|
return *this;
|
|
}
|
|
|
|
if ( t >= 1.0f ) {
|
|
*this = to;
|
|
return *this;
|
|
}
|
|
|
|
if ( from == to ) {
|
|
*this = to;
|
|
return *this;
|
|
}
|
|
|
|
cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;
|
|
absCosom = fabs( cosom );
|
|
|
|
if ( ( 1.0f - absCosom ) > 1e-6f ) {
|
|
#if 0
|
|
omega = acos( absCosom );
|
|
sinom = 1.0f / sin( omega );
|
|
scale0 = sin( ( 1.0f - t ) * omega ) * sinom;
|
|
scale1 = sin( t * omega ) * sinom;
|
|
#else
|
|
scale0 = 1.0f - absCosom * absCosom;
|
|
sinom = idMath::InvSqrt( scale0 );
|
|
omega = idMath::ATan16( scale0 * sinom, absCosom );
|
|
scale0 = idMath::Sin16( ( 1.0f - t ) * omega ) * sinom;
|
|
scale1 = idMath::Sin16( t * omega ) * sinom;
|
|
#endif
|
|
} else {
|
|
scale0 = 1.0f - t;
|
|
scale1 = t;
|
|
}
|
|
|
|
scale1 = ( cosom >= 0.0f ) ? scale1 : -scale1;
|
|
|
|
x = scale0 * from.x + scale1 * to.x;
|
|
y = scale0 * from.y + scale1 * to.y;
|
|
z = scale0 * from.z + scale1 * to.z;
|
|
w = scale0 * from.w + scale1 * to.w;
|
|
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
=====================
|
|
idQuat::SlerpFast
|
|
|
|
Approximation of spherical linear interpolation between two quaternions.
|
|
The interpolation traces out the exact same curve as Slerp but does not maintain a constant speed across the arc.
|
|
=====================
|
|
*/
|
|
idQuat &idQuat::SlerpFast( const idQuat &from, const idQuat &to, float t ) {
|
|
float cosom, scale0, scale1, s;
|
|
|
|
if ( t <= 0.0f ) {
|
|
*this = from;
|
|
return *this;
|
|
}
|
|
|
|
if ( t >= 1.0f ) {
|
|
*this = to;
|
|
return *this;
|
|
}
|
|
|
|
if ( from == to ) {
|
|
*this = to;
|
|
return *this;
|
|
}
|
|
|
|
cosom = from.x * to.x + from.y * to.y + from.z * to.z + from.w * to.w;
|
|
|
|
scale0 = 1.0f - t;
|
|
scale1 = ( cosom >= 0.0f ) ? t : -t;
|
|
|
|
x = scale0 * from.x + scale1 * to.x;
|
|
y = scale0 * from.y + scale1 * to.y;
|
|
z = scale0 * from.z + scale1 * to.z;
|
|
w = scale0 * from.w + scale1 * to.w;
|
|
|
|
s = idMath::InvSqrt( x * x + y * y + z * z + w * w );
|
|
|
|
x *= s;
|
|
y *= s;
|
|
z *= s;
|
|
w *= s;
|
|
|
|
return *this;
|
|
}
|