Bullet Collision Detection & Physics Library
scalar/quat_aos.h
Go to the documentation of this file.
1 /*
2  Copyright (C) 2009 Sony Computer Entertainment Inc.
3  All rights reserved.
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 
15 */
16 
17 #ifndef _VECTORMATH_QUAT_AOS_CPP_H
18 #define _VECTORMATH_QUAT_AOS_CPP_H
19 
20 //-----------------------------------------------------------------------------
21 // Definitions
22 
23 #ifndef _VECTORMATH_INTERNAL_FUNCTIONS
24 #define _VECTORMATH_INTERNAL_FUNCTIONS
25 
26 #endif
27 
28 namespace Vectormath {
29 namespace Aos {
30 
31 inline Quat::Quat( const Quat & quat )
32 {
33  mX = quat.mX;
34  mY = quat.mY;
35  mZ = quat.mZ;
36  mW = quat.mW;
37 }
38 
39 inline Quat::Quat( float _x, float _y, float _z, float _w )
40 {
41  mX = _x;
42  mY = _y;
43  mZ = _z;
44  mW = _w;
45 }
46 
47 inline Quat::Quat( const Vector3 & xyz, float _w )
48 {
49  this->setXYZ( xyz );
50  this->setW( _w );
51 }
52 
53 inline Quat::Quat( const Vector4 & vec )
54 {
55  mX = vec.getX();
56  mY = vec.getY();
57  mZ = vec.getZ();
58  mW = vec.getW();
59 }
60 
61 inline Quat::Quat( float scalar )
62 {
63  mX = scalar;
64  mY = scalar;
65  mZ = scalar;
66  mW = scalar;
67 }
68 
69 inline const Quat Quat::identity( )
70 {
71  return Quat( 0.0f, 0.0f, 0.0f, 1.0f );
72 }
73 
74 inline const Quat lerp( float t, const Quat & quat0, const Quat & quat1 )
75 {
76  return ( quat0 + ( ( quat1 - quat0 ) * t ) );
77 }
78 
79 inline const Quat slerp( float t, const Quat & unitQuat0, const Quat & unitQuat1 )
80 {
81  Quat start;
82  float recipSinAngle, scale0, scale1, cosAngle, angle;
83  cosAngle = dot( unitQuat0, unitQuat1 );
84  if ( cosAngle < 0.0f ) {
85  cosAngle = -cosAngle;
86  start = ( -unitQuat0 );
87  } else {
88  start = unitQuat0;
89  }
90  if ( cosAngle < _VECTORMATH_SLERP_TOL ) {
91  angle = acosf( cosAngle );
92  recipSinAngle = ( 1.0f / sinf( angle ) );
93  scale0 = ( sinf( ( ( 1.0f - t ) * angle ) ) * recipSinAngle );
94  scale1 = ( sinf( ( t * angle ) ) * recipSinAngle );
95  } else {
96  scale0 = ( 1.0f - t );
97  scale1 = t;
98  }
99  return ( ( start * scale0 ) + ( unitQuat1 * scale1 ) );
100 }
101 
102 inline const Quat squad( float t, const Quat & unitQuat0, const Quat & unitQuat1, const Quat & unitQuat2, const Quat & unitQuat3 )
103 {
104  Quat tmp0, tmp1;
105  tmp0 = slerp( t, unitQuat0, unitQuat3 );
106  tmp1 = slerp( t, unitQuat1, unitQuat2 );
107  return slerp( ( ( 2.0f * t ) * ( 1.0f - t ) ), tmp0, tmp1 );
108 }
109 
110 inline void loadXYZW( Quat & quat, const float * fptr )
111 {
112  quat = Quat( fptr[0], fptr[1], fptr[2], fptr[3] );
113 }
114 
115 inline void storeXYZW( const Quat & quat, float * fptr )
116 {
117  fptr[0] = quat.getX();
118  fptr[1] = quat.getY();
119  fptr[2] = quat.getZ();
120  fptr[3] = quat.getW();
121 }
122 
123 inline Quat & Quat::operator =( const Quat & quat )
124 {
125  mX = quat.mX;
126  mY = quat.mY;
127  mZ = quat.mZ;
128  mW = quat.mW;
129  return *this;
130 }
131 
132 inline Quat & Quat::setXYZ( const Vector3 & vec )
133 {
134  mX = vec.getX();
135  mY = vec.getY();
136  mZ = vec.getZ();
137  return *this;
138 }
139 
140 inline const Vector3 Quat::getXYZ( ) const
141 {
142  return Vector3( mX, mY, mZ );
143 }
144 
145 inline Quat & Quat::setX( float _x )
146 {
147  mX = _x;
148  return *this;
149 }
150 
151 inline float Quat::getX( ) const
152 {
153  return mX;
154 }
155 
156 inline Quat & Quat::setY( float _y )
157 {
158  mY = _y;
159  return *this;
160 }
161 
162 inline float Quat::getY( ) const
163 {
164  return mY;
165 }
166 
167 inline Quat & Quat::setZ( float _z )
168 {
169  mZ = _z;
170  return *this;
171 }
172 
173 inline float Quat::getZ( ) const
174 {
175  return mZ;
176 }
177 
178 inline Quat & Quat::setW( float _w )
179 {
180  mW = _w;
181  return *this;
182 }
183 
184 inline float Quat::getW( ) const
185 {
186  return mW;
187 }
188 
189 inline Quat & Quat::setElem( int idx, float value )
190 {
191  *(&mX + idx) = value;
192  return *this;
193 }
194 
195 inline float Quat::getElem( int idx ) const
196 {
197  return *(&mX + idx);
198 }
199 
200 inline float & Quat::operator []( int idx )
201 {
202  return *(&mX + idx);
203 }
204 
205 inline float Quat::operator []( int idx ) const
206 {
207  return *(&mX + idx);
208 }
209 
210 inline const Quat Quat::operator +( const Quat & quat ) const
211 {
212  return Quat(
213  ( mX + quat.mX ),
214  ( mY + quat.mY ),
215  ( mZ + quat.mZ ),
216  ( mW + quat.mW )
217  );
218 }
219 
220 inline const Quat Quat::operator -( const Quat & quat ) const
221 {
222  return Quat(
223  ( mX - quat.mX ),
224  ( mY - quat.mY ),
225  ( mZ - quat.mZ ),
226  ( mW - quat.mW )
227  );
228 }
229 
230 inline const Quat Quat::operator *( float scalar ) const
231 {
232  return Quat(
233  ( mX * scalar ),
234  ( mY * scalar ),
235  ( mZ * scalar ),
236  ( mW * scalar )
237  );
238 }
239 
240 inline Quat & Quat::operator +=( const Quat & quat )
241 {
242  *this = *this + quat;
243  return *this;
244 }
245 
246 inline Quat & Quat::operator -=( const Quat & quat )
247 {
248  *this = *this - quat;
249  return *this;
250 }
251 
252 inline Quat & Quat::operator *=( float scalar )
253 {
254  *this = *this * scalar;
255  return *this;
256 }
257 
258 inline const Quat Quat::operator /( float scalar ) const
259 {
260  return Quat(
261  ( mX / scalar ),
262  ( mY / scalar ),
263  ( mZ / scalar ),
264  ( mW / scalar )
265  );
266 }
267 
268 inline Quat & Quat::operator /=( float scalar )
269 {
270  *this = *this / scalar;
271  return *this;
272 }
273 
274 inline const Quat Quat::operator -( ) const
275 {
276  return Quat(
277  -mX,
278  -mY,
279  -mZ,
280  -mW
281  );
282 }
283 
284 inline const Quat operator *( float scalar, const Quat & quat )
285 {
286  return quat * scalar;
287 }
288 
289 inline float dot( const Quat & quat0, const Quat & quat1 )
290 {
291  float result;
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() ) );
296  return result;
297 }
298 
299 inline float norm( const Quat & quat )
300 {
301  float result;
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() ) );
306  return result;
307 }
308 
309 inline float length( const Quat & quat )
310 {
311  return ::sqrtf( norm( quat ) );
312 }
313 
314 inline const Quat normalize( const Quat & quat )
315 {
316  float lenSqr, lenInv;
317  lenSqr = norm( quat );
318  lenInv = ( 1.0f / sqrtf( lenSqr ) );
319  return Quat(
320  ( quat.getX() * lenInv ),
321  ( quat.getY() * lenInv ),
322  ( quat.getZ() * lenInv ),
323  ( quat.getW() * lenInv )
324  );
325 }
326 
327 inline const Quat Quat::rotation( const Vector3 & unitVec0, const Vector3 & unitVec1 )
328 {
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 ) );
333 }
334 
335 inline const Quat Quat::rotation( float radians, const Vector3 & unitVec )
336 {
337  float s, c, angle;
338  angle = ( radians * 0.5f );
339  s = sinf( angle );
340  c = cosf( angle );
341  return Quat( ( unitVec * s ), c );
342 }
343 
344 inline const Quat Quat::rotationX( float radians )
345 {
346  float s, c, angle;
347  angle = ( radians * 0.5f );
348  s = sinf( angle );
349  c = cosf( angle );
350  return Quat( s, 0.0f, 0.0f, c );
351 }
352 
353 inline const Quat Quat::rotationY( float radians )
354 {
355  float s, c, angle;
356  angle = ( radians * 0.5f );
357  s = sinf( angle );
358  c = cosf( angle );
359  return Quat( 0.0f, s, 0.0f, c );
360 }
361 
362 inline const Quat Quat::rotationZ( float radians )
363 {
364  float s, c, angle;
365  angle = ( radians * 0.5f );
366  s = sinf( angle );
367  c = cosf( angle );
368  return Quat( 0.0f, 0.0f, s, c );
369 }
370 
371 inline const Quat Quat::operator *( const Quat & quat ) const
372 {
373  return Quat(
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 ) )
378  );
379 }
380 
381 inline Quat & Quat::operator *=( const Quat & quat )
382 {
383  *this = *this * quat;
384  return *this;
385 }
386 
387 inline const Vector3 rotate( const Quat & quat, const Vector3 & vec )
388 {
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() ) );
394  return Vector3(
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() ) )
398  );
399 }
400 
401 inline const Quat conj( const Quat & quat )
402 {
403  return Quat( -quat.getX(), -quat.getY(), -quat.getZ(), quat.getW() );
404 }
405 
406 inline const Quat select( const Quat & quat0, const Quat & quat1, bool select1 )
407 {
408  return Quat(
409  ( select1 )? quat1.getX() : quat0.getX(),
410  ( select1 )? quat1.getY() : quat0.getY(),
411  ( select1 )? quat1.getZ() : quat0.getZ(),
412  ( select1 )? quat1.getW() : quat0.getW()
413  );
414 }
415 
416 #ifdef _VECTORMATH_DEBUG
417 
418 inline void print( const Quat & quat )
419 {
420  printf( "( %f %f %f %f )\n", quat.getX(), quat.getY(), quat.getZ(), quat.getW() );
421 }
422 
423 inline void print( const Quat & quat, const char * name )
424 {
425  printf( "%s: ( %f %f %f %f )\n", name, quat.getX(), quat.getY(), quat.getZ(), quat.getW() );
426 }
427 
428 #endif
429 
430 } // namespace Aos
431 } // namespace Vectormath
432 
433 #endif
const Quat normalize(const Quat &quat)
float getY() const
static const Quat rotationY(float radians)
float getW() const
Quat & operator*=(const Quat &quat)
Quat & operator-=(const Quat &quat)
const Quat operator+(const Quat &quat) const
Quat & setW(float w)
Quat & operator=(const Quat &quat)
#define _VECTORMATH_SLERP_TOL
Definition: neon/vec_aos.h:23
Quat & setX(float x)
const Vector3 rotate(const Quat &quat, const Vector3 &vec)
float & operator[](int idx)
static const Quat identity()
Definition: neon/quat_aos.h:68
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)
Definition: neon/quat_aos.h:73
const Matrix3 select(const Matrix3 &mat0, const Matrix3 &mat1, bool select1)
Definition: neon/mat_aos.h:409
Quat & setElem(int idx, float value)
float getZ() const
float getElem(int idx) const
Quat & setY(float y)
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)
Quat & setZ(float z)
const Vector3 cross(const Vector3 &vec0, const Vector3 &vec1)
Definition: neon/vec_aos.h:473
Quat & setXYZ(const Vector3 &vec)
float length(const Quat &quat)
const Matrix3 operator*(float scalar, const Matrix3 &mat)
Definition: neon/mat_aos.h:257
float getX() const
Quat & operator+=(const Quat &quat)
const Quat slerp(float t, const Quat &unitQuat0, const Quat &unitQuat1)
Definition: neon/quat_aos.h:78