17 #ifndef _VECTORMATH_QUAT_AOS_CPP_H
18 #define _VECTORMATH_QUAT_AOS_CPP_H
23 #ifndef _VECTORMATH_INTERNAL_FUNCTIONS
24 #define _VECTORMATH_INTERNAL_FUNCTIONS
28 namespace Vectormath {
39 inline Quat::Quat(
float _x,
float _y,
float _z,
float _w )
47 inline Quat::Quat(
const Vector3 & xyz,
float _w )
71 return Quat( 0.0f, 0.0f, 0.0f, 1.0f );
74 inline const Quat
lerp(
float t,
const Quat & quat0,
const Quat & quat1 )
76 return ( quat0 + ( ( quat1 - quat0 ) * t ) );
79 inline const Quat
slerp(
float t,
const Quat & unitQuat0,
const Quat & unitQuat1 )
82 float recipSinAngle, scale0, scale1, cosAngle, angle;
83 cosAngle =
dot( unitQuat0, unitQuat1 );
84 if ( cosAngle < 0.0f ) {
86 start = ( -unitQuat0 );
91 angle = acosf( cosAngle );
92 recipSinAngle = ( 1.0f / sinf( angle ) );
93 scale0 = ( sinf( ( ( 1.0f - t ) * angle ) ) * recipSinAngle );
94 scale1 = ( sinf( ( t * angle ) ) * recipSinAngle );
96 scale0 = ( 1.0f - t );
99 return ( ( start * scale0 ) + ( unitQuat1 * scale1 ) );
102 inline const Quat
squad(
float t,
const Quat & unitQuat0,
const Quat & unitQuat1,
const Quat & unitQuat2,
const Quat & unitQuat3 )
105 tmp0 =
slerp( t, unitQuat0, unitQuat3 );
106 tmp1 =
slerp( t, unitQuat1, unitQuat2 );
107 return slerp( ( ( 2.0f * t ) * ( 1.0f - t ) ), tmp0, tmp1 );
110 inline void loadXYZW( Quat & quat,
const float * fptr )
112 quat = Quat( fptr[0], fptr[1], fptr[2], fptr[3] );
115 inline void storeXYZW(
const Quat & quat,
float * fptr )
117 fptr[0] = quat.getX();
118 fptr[1] = quat.getY();
119 fptr[2] = quat.getZ();
120 fptr[3] = quat.getW();
142 return Vector3(
mX,
mY,
mZ );
191 *(&
mX + idx) = value;
242 *
this = *
this + quat;
248 *
this = *
this - quat;
254 *
this = *
this * scalar;
270 *
this = *
this / scalar;
284 inline const Quat
operator *(
float scalar,
const Quat & quat )
286 return quat * scalar;
289 inline float dot(
const Quat & quat0,
const Quat & quat1 )
292 result = ( quat0.getX() * quat1.getX() );
293 result = ( result + ( quat0.getY() * quat1.getY() ) );
294 result = ( result + ( quat0.getZ() * quat1.getZ() ) );
295 result = ( result + ( quat0.getW() * quat1.getW() ) );
299 inline float norm(
const Quat & quat )
302 result = ( quat.getX() * quat.getX() );
303 result = ( result + ( quat.getY() * quat.getY() ) );
304 result = ( result + ( quat.getZ() * quat.getZ() ) );
305 result = ( result + ( quat.getW() * quat.getW() ) );
309 inline float length(
const Quat & quat )
311 return ::sqrtf(
norm( quat ) );
314 inline const Quat
normalize(
const Quat & quat )
316 float lenSqr, lenInv;
317 lenSqr =
norm( quat );
318 lenInv = ( 1.0f / sqrtf( lenSqr ) );
320 ( quat.getX() * lenInv ),
321 ( quat.getY() * lenInv ),
322 ( quat.getZ() * lenInv ),
323 ( quat.getW() * lenInv )
327 inline const Quat
Quat::rotation(
const Vector3 & unitVec0,
const Vector3 & unitVec1 )
329 float cosHalfAngleX2, recipCosHalfAngleX2;
330 cosHalfAngleX2 = sqrtf( ( 2.0f * ( 1.0f +
dot( unitVec0, unitVec1 ) ) ) );
331 recipCosHalfAngleX2 = ( 1.0f / cosHalfAngleX2 );
332 return Quat( (
cross( unitVec0, unitVec1 ) * recipCosHalfAngleX2 ), ( cosHalfAngleX2 * 0.5f ) );
335 inline const Quat
Quat::rotation(
float radians,
const Vector3 & unitVec )
338 angle = ( radians * 0.5f );
341 return Quat( ( unitVec * s ), c );
347 angle = ( radians * 0.5f );
350 return Quat( s, 0.0f, 0.0f, c );
356 angle = ( radians * 0.5f );
359 return Quat( 0.0f, s, 0.0f, c );
365 angle = ( radians * 0.5f );
368 return Quat( 0.0f, 0.0f, s, c );
374 ( ( ( (
mW * quat.mX ) + (
mX * quat.mW ) ) + (
mY * quat.mZ ) ) - (
mZ * quat.mY ) ),
375 ( ( ( (
mW * quat.mY ) + (
mY * quat.mW ) ) + (
mZ * quat.mX ) ) - (
mX * quat.mZ ) ),
376 ( ( ( (
mW * quat.mZ ) + (
mZ * quat.mW ) ) + (
mX * quat.mY ) ) - (
mY * quat.mX ) ),
377 ( ( ( (
mW * quat.mW ) - (
mX * quat.mX ) ) - (
mY * quat.mY ) ) - (
mZ * quat.mZ ) )
383 *
this = *
this * quat;
387 inline const Vector3
rotate(
const Quat & quat,
const Vector3 & vec )
389 float tmpX, tmpY, tmpZ, tmpW;
390 tmpX = ( ( ( quat.getW() * vec.getX() ) + ( quat.getY() * vec.getZ() ) ) - ( quat.getZ() * vec.getY() ) );
391 tmpY = ( ( ( quat.getW() * vec.getY() ) + ( quat.getZ() * vec.getX() ) ) - ( quat.getX() * vec.getZ() ) );
392 tmpZ = ( ( ( quat.getW() * vec.getZ() ) + ( quat.getX() * vec.getY() ) ) - ( quat.getY() * vec.getX() ) );
393 tmpW = ( ( ( quat.getX() * vec.getX() ) + ( quat.getY() * vec.getY() ) ) + ( quat.getZ() * vec.getZ() ) );
395 ( ( ( ( tmpW * quat.getX() ) + ( tmpX * quat.getW() ) ) - ( tmpY * quat.getZ() ) ) + ( tmpZ * quat.getY() ) ),
396 ( ( ( ( tmpW * quat.getY() ) + ( tmpY * quat.getW() ) ) - ( tmpZ * quat.getX() ) ) + ( tmpX * quat.getZ() ) ),
397 ( ( ( ( tmpW * quat.getZ() ) + ( tmpZ * quat.getW() ) ) - ( tmpX * quat.getY() ) ) + ( tmpY * quat.getX() ) )
401 inline const Quat
conj(
const Quat & quat )
403 return Quat( -quat.getX(), -quat.getY(), -quat.getZ(), quat.getW() );
406 inline const Quat
select(
const Quat & quat0,
const Quat & quat1,
bool select1 )
409 ( select1 )? quat1.getX() : quat0.getX(),
410 ( select1 )? quat1.getY() : quat0.getY(),
411 ( select1 )? quat1.getZ() : quat0.getZ(),
412 ( select1 )? quat1.getW() : quat0.getW()
416 #ifdef _VECTORMATH_DEBUG
418 inline void print(
const Quat & quat )
420 printf(
"( %f %f %f %f )\n", quat.getX(), quat.getY(), quat.getZ(), quat.getW() );
423 inline void print(
const Quat & quat,
const char * name )
425 printf(
"%s: ( %f %f %f %f )\n", name, quat.getX(), quat.getY(), quat.getZ(), quat.getW() );
const Quat normalize(const Quat &quat)
static const Quat rotationY(float radians)
Quat & operator*=(const Quat &quat)
Quat & operator-=(const Quat &quat)
const Quat operator+(const Quat &quat) const
Quat & operator=(const Quat &quat)
#define _VECTORMATH_SLERP_TOL
const Vector3 rotate(const Quat &quat, const Vector3 &vec)
float & operator[](int idx)
static const Quat identity()
static const Quat rotationZ(float radians)
static const Quat rotation(const Vector3 &unitVec0, const Vector3 &unitVec1)
static const Quat rotationX(float radians)
const Quat operator*(const Quat &quat) const
float norm(const Quat &quat)
const Quat operator/(float scalar) const
const Quat operator-() const
const Vector3 getXYZ() const
void loadXYZW(Quat &quat, const float *fptr)
const Quat conj(const Quat &quat)
Quat & operator/=(float scalar)
void storeXYZW(const Quat &quat, float *fptr)
const Quat lerp(float t, const Quat &quat0, const Quat &quat1)
const Matrix3 select(const Matrix3 &mat0, const Matrix3 &mat1, bool select1)
Quat & setElem(int idx, float value)
float getElem(int idx) const
const Quat squad(float t, const Quat &unitQuat0, const Quat &unitQuat1, const Quat &unitQuat2, const Quat &unitQuat3)
float dot(const Quat &quat0, const Quat &quat1)
const Vector3 cross(const Vector3 &vec0, const Vector3 &vec1)
Quat & setXYZ(const Vector3 &vec)
float length(const Quat &quat)
const Matrix3 operator*(float scalar, const Matrix3 &mat)
Quat & operator+=(const Quat &quat)
const Quat slerp(float t, const Quat &unitQuat0, const Quat &unitQuat1)