GL Studio C++ Runtime API
gls_quaternion.h
Go to the documentation of this file.
1/// \file gls_quaternion.h
2/// \brief The disti::GlsQuaternion class.
3///
4/// \par Copyright Information
5///
6/// Copyright (c) 2017 by The DiSTI Corporation.<br>
7/// 12249 Science Drive; Suite 300<br>
8/// Orlando, Florida 32826<br>
9/// USA<br>
10/// <br>
11/// All rights reserved.<br>
12///
13/// This Software contains proprietary trade secrets of DiSTI and may not be
14/// reproduced, in whole or part, in any form, or by any means of electronic,
15/// mechanical, or otherwise, without the written permission of DiSTI. Said
16/// permission may be derived through the purchase of applicable DiSTI product
17/// licenses which detail the distribution rights of this content and any
18/// Derivative Works based on this or other copyrighted DiSTI Software.
19///
20/// NO WARRANTY. THE SOFTWARE IS PROVIDED "AS-IS," WITHOUT WARRANTY OF ANY KIND,
21/// AND ANY USE OF THIS SOFTWARE PRODUCT IS AT YOUR OWN RISK. TO THE MAXIMUM EXTENT
22/// PERMITTED BY APPLICABLE LAW, DISTI AND ITS SUPPLIERS DISCLAIM ALL WARRANTIES
23/// AND CONDITIONS, EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
24/// IMPLIED WARRANTIES AND CONDITIONS OF MERCHANTABILITY AND/OR FITNESS FOR A
25/// PARTICULAR PURPOSE, TITLE, AND NON-INFRINGEMENT, WITH REGARD TO THE SOFTWARE.
26///
27/// LIMITATION OF LIABILITY. TO THE MAXIMUM EXTENT PERMITTED BY APPLICABLE LAW,
28/// IN NO EVENT SHALL DISTI OR ITS SUPPLIERS BE LIABLE FOR ANY SPECIAL, INCIDENTAL,
29/// INDIRECT, OR CONSEQUENTIAL DAMAGES WHATSOEVER (INCLUDING, WITHOUT LIMITATION,
30/// DAMAGES FOR LOSS OF BUSINESS PROFITS, BUSINESS INTERRUPTION, LOSS OF BUSINESS
31/// INFORMATION, OR ANY OTHER PECUNIARY LOSS) ARISING OUT OF THE USE OF OR
32/// INABILITY TO USE THE SOFTWARE, EVEN IF DISTI HAS BEEN ADVISED OF THE POSSIBILITY
33/// OF SUCH DAMAGES. DISTI'S ENTIRE LIABILITY AND YOUR EXCLUSIVE REMEDY SHALL NOT
34/// EXCEED FIVE DOLLARS (US$5.00).
35///
36/// The aforementioned terms and restrictions are governed by the laws of the
37/// State of Florida and the United States of America.
38
39#ifndef GLS_QUATERNION_H
40#define GLS_QUATERNION_H
41
42#include <iostream>
43
45#include "vertex.h"
46
47namespace disti
48{
49template<class Type>
50class GlsMatrixAffine;
51
52/** The GlsQuaternion class */
53template<class Type>
55{
56public:
57 //------------------------------------------------------------------------
58 /**
59 * Default constructor. This will initialize the quaternion to zero.
60 */
61 //------------------------------------------------------------------------
63 {
64 _v[ 0 ] = 0;
65 _v[ 1 ] = 0;
66 _v[ 2 ] = 0;
67 _v[ 3 ] = 0;
68 }
69
70 //------------------------------------------------------------------------
71 /**
72 * Constructor. Sets quaternion from 4 constants.
73 * \param x Initial X value
74 * \param y Initial Y value
75 * \param z Initial Z value
76 * \param w Initial W value
77 */
78 //------------------------------------------------------------------------
79 GlsQuaternion( Type x, Type y, Type z, Type w )
80 {
81 _v[ 0 ] = x;
82 _v[ 1 ] = y;
83 _v[ 2 ] = z;
84 _v[ 3 ] = w;
85 }
86
87 //------------------------------------------------------------------------
88 /**
89 * Constructor. Sets quaternion from angle about an arbitrary axis
90 * \param angle Rotation angle in radians
91 * \param axis Unit vector representing arbitrary axis
92 */
93 //------------------------------------------------------------------------
94 GlsQuaternion( Type angle, const Vector& axis )
95 {
96 SetFromAngleAxis( angle, axis );
97 }
98
99 //------------------------------------------------------------------------
100 /**
101 * Constructor. Sets quaternion from 3 angles about 3 axes
102 * \param angle1 Rotation angle about first axis in radians
103 * \param axis1 Unit vector representing first arbitrary axis
104 * \param angle2 Rotation angle about second axis in radians
105 * \param axis2 Unit vector representing second arbitrary axis
106 * \param angle3 Rotation angle about third axis in radians
107 * \param axis3 Unit vector representing third arbitrary axis
108 */
109 //------------------------------------------------------------------------
110 GlsQuaternion( Type angle1, const Vector& axis1,
111 Type angle2, const Vector& axis2,
112 Type angle3, const Vector& axis3 )
113 {
114 SetFromAngleAxis( angle1, axis1, angle2, axis2, angle3, axis3 );
115 }
116
117 //------------------------------------------------------------------------
118 /**
119 * Constructor. Sets quaternion from 3 angles (Euler Angles)
120 * \param angleX Rotation angle about X axis in radians
121 * \param angleY Rotation angle about Y axis in radians
122 * \param angleZ Rotation angle about Z axis in radians
123 */
124 //------------------------------------------------------------------------
125 GlsQuaternion( Type angleX, Type angleY, Type angleZ )
126 {
127 SetFromEulerAngles( angleX, angleY, angleZ );
128 }
129
130 //------------------------------------------------------------------------
131 /**
132 * Constructor. Sets quaternion from 3 angles (Euler Angles)
133 * \param anglesVec A vector where each component of the vector is an
134 * Euler angle component in radians.
135 */
136 //------------------------------------------------------------------------
137 GlsQuaternion( const Vector& anglesVec )
138 {
139 SetFromEulerAngles( anglesVec.x, anglesVec.y, anglesVec.z );
140 }
141
142 //------------------------------------------------------------------------
143 /**
144 * Constructor. Sets quaternion from a rotation matrix
145 * \param matrix An affine matrix representing a rotation matrix
146 */
147 //------------------------------------------------------------------------
149 {
150 SetFromRotationMatrix( matrix );
151 }
152
153 //------------------------------------------------------------------------
154 /**
155 * Accessor method providing access to quaternion data.
156 * \return A pointer to the quaternion data.
157 */
158 //------------------------------------------------------------------------
159 Type* Data()
160 {
161 return _v;
162 }
163
164 //------------------------------------------------------------------------
165 /**
166 * Accessor method providing access to quaternion data.
167 * \return A pointer to the quaternion data.
168 */
169 //------------------------------------------------------------------------
170 const Type* Data() const
171 {
172 return _v;
173 }
174
175 //------------------------------------------------------------------------
176 /**
177 * Boolean equivalence operator
178 * \param q Quaternion to compare against
179 * \return True if the two quaternions are exactly equal
180 */
181 //------------------------------------------------------------------------
182 bool operator==( const GlsQuaternion& q ) const
183 {
184 return _v[ 0 ] == q._v[ 0 ] && _v[ 1 ] == q._v[ 1 ] && _v[ 2 ] == q._v[ 2 ] && _v[ 3 ] == q._v[ 3 ];
185 }
186
187 //------------------------------------------------------------------------
188 /**
189 * Boolean non-equivalence operator
190 * \param q Quaternion to compare against
191 * \return True if the two quaternions are not exactly equal
192 */
193 //------------------------------------------------------------------------
194 bool operator!=( const GlsQuaternion& q ) const
195 {
196 return _v[ 0 ] != q._v[ 0 ] || _v[ 1 ] != q._v[ 1 ] || _v[ 2 ] != q._v[ 2 ] || _v[ 3 ] != q._v[ 3 ];
197 }
198
199 //------------------------------------------------------------------------
200 /**
201 * Boolean less than operator
202 * \param q Quaternion to compare against
203 * \return True if the this quaternion is less than the provided one
204 */
205 //------------------------------------------------------------------------
206 bool operator<( const GlsQuaternion& q ) const
207 {
208 if( _v[ 0 ] < q._v[ 0 ] )
209 return true;
210 else if( _v[ 0 ] > q._v[ 0 ] )
211 return false;
212 else if( _v[ 1 ] < q._v[ 1 ] )
213 return true;
214 else if( _v[ 1 ] > q._v[ 1 ] )
215 return false;
216 else if( _v[ 2 ] < q._v[ 2 ] )
217 return true;
218 else if( _v[ 2 ] > q._v[ 2 ] )
219 return false;
220 else
221 return ( _v[ 3 ] < q._v[ 3 ] );
222 }
223
224 //------------------------------------------------------------------------
225 /**
226 * Scalar multiplication operator
227 * \param s A scalar to multiply against
228 * \return This quaternion multiplied by the scalar
229 */
230 //------------------------------------------------------------------------
231 const GlsQuaternion operator*( Type s ) const
232 {
233 return GlsQuaternion( _v[ 0 ] * s, _v[ 1 ] * s, _v[ 2 ] * s, _v[ 3 ] * s );
234 }
235
236 //------------------------------------------------------------------------
237 /**
238 * Scalar multiplication operator and assignment operator
239 * \param s A scalar to multiply against
240 * \return This quaternion multiplied by the scalar
241 */
242 //------------------------------------------------------------------------
244 {
245 _v[ 0 ] *= s;
246 _v[ 1 ] *= s;
247 _v[ 2 ] *= s;
248 _v[ 3 ] *= s;
249 return *this; // enable nesting
250 }
251
252 /// The quaternion multiplication operator.
253 /// \param q A quaternion to multiply against.
254 /// \return This quaternion multiplied by the quaternion.
255 const GlsQuaternion operator*( const GlsQuaternion& q ) const
256 {
257 return GlsQuaternion( q._v[ 3 ] * _v[ 0 ] + q._v[ 0 ] * _v[ 3 ] - q._v[ 1 ] * _v[ 2 ] + q._v[ 2 ] * _v[ 1 ],
258 q._v[ 3 ] * _v[ 1 ] + q._v[ 0 ] * _v[ 2 ] + q._v[ 1 ] * _v[ 3 ] - q._v[ 2 ] * _v[ 0 ],
259 q._v[ 3 ] * _v[ 2 ] - q._v[ 0 ] * _v[ 1 ] + q._v[ 1 ] * _v[ 0 ] + q._v[ 2 ] * _v[ 3 ],
260 q._v[ 3 ] * _v[ 3 ] - q._v[ 0 ] * _v[ 0 ] - q._v[ 1 ] * _v[ 1 ] - q._v[ 2 ] * _v[ 2 ] );
261 }
262
263 /// The quaternion multiplication operator.
264 /// \param q A quaternion to multiply against.
265 /// \return This quaternion multiplied by the quaternion.
267 {
268 Type x = q._v[ 3 ] * _v[ 0 ] + q._v[ 0 ] * _v[ 3 ] - q._v[ 1 ] * _v[ 2 ] + q._v[ 2 ] * _v[ 1 ];
269 Type y = q._v[ 3 ] * _v[ 1 ] + q._v[ 0 ] * _v[ 2 ] + q._v[ 1 ] * _v[ 3 ] - q._v[ 2 ] * _v[ 0 ];
270 Type z = q._v[ 3 ] * _v[ 2 ] - q._v[ 0 ] * _v[ 1 ] + q._v[ 1 ] * _v[ 0 ] + q._v[ 2 ] * _v[ 3 ];
271 _v[ 3 ] = q._v[ 3 ] * _v[ 3 ] - q._v[ 0 ] * _v[ 0 ] - q._v[ 1 ] * _v[ 1 ] - q._v[ 2 ] * _v[ 2 ];
272
273 _v[ 2 ] = z;
274 _v[ 1 ] = y;
275 _v[ 0 ] = x;
276
277 return ( *this ); // enable nesting
278 }
279
280 //------------------------------------------------------------------------
281 /**
282 * Scalar division operator
283 * \param s A scalar to divide
284 * \return This quaternion divided by the scalar
285 */
286 //------------------------------------------------------------------------
287 inline GlsQuaternion operator/( Type s ) const
288 {
289 Type factor = 1.0 / s;
290 return GlsQuaternion( _v[ 0 ] * factor, _v[ 1 ] * factor, _v[ 2 ] * factor, _v[ 3 ] * factor );
291 }
292
293 //------------------------------------------------------------------------
294 /**
295 * Scalar division and assignment operator
296 * \param s A scalar to divide
297 * \return This quaternion divided by the scalar
298 */
299 //------------------------------------------------------------------------
300 inline GlsQuaternion& operator/=( Type s )
301 {
302 Type factor = 1 / s;
303 _v[ 0 ] *= factor;
304 _v[ 1 ] *= factor;
305 _v[ 2 ] *= factor;
306 _v[ 3 ] *= factor;
307 return *this;
308 }
309
310 //------------------------------------------------------------------------
311 /**
312 * Quaternion division operator
313 * \param q A quaternion to division by
314 * \return This quaternion divided by the quaternion
315 */
316 //------------------------------------------------------------------------
317 inline const GlsQuaternion operator/( const GlsQuaternion& q ) const
318 {
319 return ( ( *this ) * q.Inverse() );
320 }
321
322 //------------------------------------------------------------------------
323 /**
324 * Quaternion division operator
325 * \param q A quaternion to division by
326 * \return This quaternion divided by the quaternion
327 */
328 //------------------------------------------------------------------------
330 {
331 ( *this ) = ( *this ) * q.Inverse();
332 return ( *this ); // enable nesting
333 }
334
335 //------------------------------------------------------------------------
336 /**
337 * Quaternion addition operator
338 * \param q A quaternion to add
339 * \return This quaternion plus the supplied quaternion
340 */
341 //------------------------------------------------------------------------
342 const GlsQuaternion operator+( const GlsQuaternion& q ) const
343 {
344 return GlsQuaternion( _v[ 0 ] + q._v[ 0 ], _v[ 1 ] + q._v[ 1 ],
345 _v[ 2 ] + q._v[ 2 ], _v[ 3 ] + q._v[ 3 ] );
346 }
347
348 //------------------------------------------------------------------------
349 /**
350 * Quaternion addition and assignment operator
351 * \param q A quaternion to add
352 * \return This quaternion plus the supplied quaternion
353 */
354 //------------------------------------------------------------------------
356 {
357 _v[ 0 ] += q._v[ 0 ];
358 _v[ 1 ] += q._v[ 1 ];
359 _v[ 2 ] += q._v[ 2 ];
360 _v[ 3 ] += q._v[ 3 ];
361 return *this; // enable nesting
362 }
363
364 //------------------------------------------------------------------------
365 /**
366 * Quaternion subtrtaction operator
367 * \param q A quaternion to subtract
368 * \return This quaternion minus the supplied quaternion
369 */
370 //------------------------------------------------------------------------
371 const GlsQuaternion operator-( const GlsQuaternion& q ) const
372 {
373 return GlsQuaternion( _v[ 0 ] - q._v[ 0 ], _v[ 1 ] - q._v[ 1 ],
374 _v[ 2 ] - q._v[ 2 ], _v[ 3 ] - q._v[ 3 ] );
375 }
376
377 //------------------------------------------------------------------------
378 /**
379 * Quaternion subtraction and assignment operator
380 * \param q A quaternion to subtract
381 * \return This quaternion minus the supplied quaternion
382 */
383 //------------------------------------------------------------------------
385 {
386 _v[ 0 ] -= q._v[ 0 ];
387 _v[ 1 ] -= q._v[ 1 ];
388 _v[ 2 ] -= q._v[ 2 ];
389 _v[ 3 ] -= q._v[ 3 ];
390 return *this; // enable nesting
391 }
392
393 /// Negation operator - returns the negative of the quaternion.
394 /// Basically just calls operator - () on the Vec4
395 /// \return The negated quaternion (this * -1).
397 {
398 return GlsQuaternion( -_v[ 0 ], -_v[ 1 ], -_v[ 2 ], -_v[ 3 ] );
399 }
400
401 //------------------------------------------------------------------------
402 /**
403 * Returns the magnitude of the quaternion
404 * \return The magnitude of the quaternion
405 */
406 //------------------------------------------------------------------------
407 Type Magnitude() const
408 {
409 return sqrt( _v[ 0 ] * _v[ 0 ] + _v[ 1 ] * _v[ 1 ] + _v[ 2 ] * _v[ 2 ] + _v[ 3 ] * _v[ 3 ] );
410 }
411
412 //------------------------------------------------------------------------
413 /**
414 * Returns the magnitude squared of the quaternion
415 * \return The magnitude squared of the quaternion
416 */
417 //------------------------------------------------------------------------
418 Type MagnitudeSquared() const
419 {
420 return _v[ 0 ] * _v[ 0 ] + _v[ 1 ] * _v[ 1 ] + _v[ 2 ] * _v[ 2 ] + _v[ 3 ] * _v[ 3 ];
421 }
422
423 //------------------------------------------------------------------------
424 /**
425 * Returns the conjugate of the quaternion
426 * \return The conjugate of the quaternion
427 */
428 //------------------------------------------------------------------------
430 {
431 return GlsQuaternion( -_v[ 0 ], -_v[ 1 ], -_v[ 2 ], _v[ 3 ] );
432 }
433
434 //------------------------------------------------------------------------
435 /**
436 * Returns the inverse of the quaternion
437 * \return The inverse of the quaternion
438 */
439 //------------------------------------------------------------------------
440 const GlsQuaternion Inverse() const
441 {
442 return Conjugate() / MagnitudeSquared();
443 }
444
445 //------------------------------------------------------------------------
446 /**
447 * Returns the dot product of the quaternion and a supplied quaternion
448 * \param q A quaternion to multiply against this quaternion
449 * \return The dot product of the quaternion and a supplied quaternion
450 */
451 //------------------------------------------------------------------------
452 Type DotProduct( const GlsQuaternion& q ) const
453 {
454 return ( ( _v[ 0 ] * q._v[ 0 ] ) + ( _v[ 1 ] * q._v[ 1 ] ) + ( _v[ 2 ] * q._v[ 2 ] ) + ( _v[ 3 ] * q._v[ 3 ] ) );
455 }
456
457 //------------------------------------------------------------------------
458 /**
459 * Normalizes this quaternion
460 */
461 //------------------------------------------------------------------------
463 {
464 *this /= Magnitude();
465 }
466
467 //------------------------------------------------------------------------
468 /**
469 * Sets the component values for this quaternion
470 * \param x The X value
471 * \param y The Y value
472 * \param z The Z value
473 * \param w The W value
474 */
475 //------------------------------------------------------------------------
476 void Set( Type x, Type y, Type z, Type w )
477 {
478 _v[ 0 ] = x;
479 _v[ 1 ] = y;
480 _v[ 2 ] = z;
481 _v[ 3 ] = w;
482 }
483
484 //------------------------------------------------------------------------
485 /**
486 * Sets quaternion from angle about an arbitrary axis
487 * \param angle Rotation angle in radians
488 * \param axis Unit vector representing arbitrary axis
489 */
490 //------------------------------------------------------------------------
491 void SetFromAngleAxis( Type angle, const Vector& axis )
492 {
493 Type half_angle = angle * (Type)0.5;
494 Type sin_half_angle = sin( half_angle );
495
496 _v[ 0 ] = sin_half_angle * axis.x;
497 _v[ 1 ] = sin_half_angle * axis.y;
498 _v[ 2 ] = sin_half_angle * axis.z;
499 _v[ 3 ] = cos( half_angle );
500 }
501
502 //------------------------------------------------------------------------
503 /**
504 * Sets quaternion from three angle-axis pairs.
505 * \param angle1 Rotation angle in radians
506 * \param axis1 Unit vector representing arbitrary axis
507 * \param angle2 Rotation angle in radians
508 * \param axis2 Unit vector representing arbitrary axis
509 * \param angle3 Rotation angle in radians
510 * \param axis3 Unit vector representing arbitrary axis
511 */
512 //------------------------------------------------------------------------
513 void SetFromAngleAxis( Type angle1, const Vector& axis1,
514 Type angle2, const Vector& axis2,
515 Type angle3, const Vector& axis3 )
516 {
517 GlsQuaternion q1( angle1, axis1 );
518 GlsQuaternion q2( angle2, axis2 );
519 SetFromAngleAxis( angle3, axis3 );
520
521 *this = q1 * q2 * *this;
522 }
523
524 //------------------------------------------------------------------------
525 /**
526 * Sets quaternion from the Euler angles.
527 * \param angleX The X Euler angle in radians
528 * \param angleY The Y Euler angle in radians
529 * \param angleZ The Z Euler angle in radians
530 */
531 //------------------------------------------------------------------------
532 void SetFromEulerAngles( Type angleX, Type angleY, Type angleZ )
533 {
534 GlsQuaternion<Type> qx, qy, qz;
535
536 // precompute half angles
537 Type xOver2 = angleX * (Type)0.5;
538 Type yOver2 = angleY * (Type)0.5;
539 Type zOver2 = angleZ * (Type)0.5;
540
541 // set the pitch quat
542 qx._v[ 0 ] = sin( xOver2 );
543 qx._v[ 1 ] = (Type)0.0;
544 qx._v[ 2 ] = (Type)0.0;
545 qx._v[ 3 ] = cos( xOver2 );
546
547 // set the yaw quat
548 qy._v[ 0 ] = (Type)0.0;
549 qy._v[ 1 ] = sin( yOver2 );
550 qy._v[ 2 ] = (Type)0.0;
551 qy._v[ 3 ] = cos( yOver2 );
552
553 // set the roll quat
554 qz._v[ 0 ] = (Type)0.0;
555 qz._v[ 1 ] = (Type)0.0;
556 qz._v[ 2 ] = sin( zOver2 );
557 qz._v[ 3 ] = cos( zOver2 );
558
559 // compose the three in x y z order...
560 *this = qx * qy * qz;
561
562 this->Normalize();
563 }
564
565 //------------------------------------------------------------------------
566 /**
567 * Sets this quaternion to a rotation that rotates 'to' into 'from'.
568 * \param to The rotation destination
569 * \param from The rotation start point
570 */
571 //------------------------------------------------------------------------
572 void SetFromVectors( const Vector& to, const Vector& from )
573 {
574 // dot product vec1*vec2
575 Type cosangle = from.DotProduct( to ) / ( from.Magnitude() * to.Magnitude() );
576
577 if( IsVeryCloseToZero( cosangle - 1 ) )
578 {
579 // cosangle is close to 1, so the vectors are close to being coincident
580 // Need to generate an angle of zero with any vector we like
581 // We'll choose (1,0,0)
582 SetFromAngleAxis( 0.0, Vector( 1.0, 0.0, 0.0 ) );
583 }
584 else if( IsVeryCloseToZero( cosangle + 1.0 ) )
585 {
586 // vectors are close to being opposite, so will need to find a
587 // vector orthongonal to from to rotate about.
588 Vector tmp;
589 if( fabs( from.x ) < fabs( from.y ) )
590 if( fabs( from.x ) < fabs( from.z ) )
591 tmp = Vector( 1.0, 0.0, 0.0 ); // use x axis.
592 else
593 tmp = Vector( 0.0, 0.0, 1.0 );
594 else if( fabs( from.y ) < fabs( from.z ) )
595 tmp = Vector( 0.0, 1.0, 0.0 );
596 else
597 tmp = Vector( 0.0, 0.0, 1.0 );
598
599 // find orthogonal axis.
600 Vector axis( from.CrossProduct( tmp ) );
601 axis.Normalize();
602
603 _v[ 0 ] = axis.x; // sin of half angle of PI is 1.0.
604 _v[ 1 ] = axis.y; // sin of half angle of PI is 1.0.
605 _v[ 2 ] = axis.z; // sin of half angle of PI is 1.0.
606 _v[ 3 ] = 0; // cos of half angle of PI is zero.
607 }
608 else
609 {
610 // This is the usual situation - take a cross-product of vec1 and vec2
611 // and that is the axis around which to rotate.
612 Vector axis( from.CrossProduct( to ) );
613 axis.Normalize();
614 Type angle = acos( cosangle );
615 SetFromAngleAxis( angle, axis );
616 }
617 }
618
619 //------------------------------------------------------------------------
620 /**
621 * Sets this quaternion from the specified rotation matrix.
622 * \param matrix The rotation matrix
623 */
624 //------------------------------------------------------------------------
626 {
627 Type tr, s;
628 Type tq[ 4 ];
629 int i, j, k;
630 const Type* mat = const_cast<GlsMatrixAffine<Type>&>( matrix ).Data();
631
632 int nxt[ 3 ] = { 1, 2, 0 };
633
634 tr = mat[ 0 * 4 + 0 ] + mat[ 1 * 4 + 1 ] + mat[ 2 * 4 + 2 ];
635
636 // check the diagonal
637 if( tr > 0.0 )
638 {
639 s = (Type)sqrt( tr + 1.0 );
640 _v[ 3 ] = s / 2.0f;
641 s = 0.5f / s;
642 _v[ 0 ] = ( mat[ 1 * 4 + 2 ] - mat[ 2 * 4 + 1 ] ) * s;
643 _v[ 1 ] = ( mat[ 2 * 4 + 0 ] - mat[ 0 * 4 + 2 ] ) * s;
644 _v[ 2 ] = ( mat[ 0 * 4 + 1 ] - mat[ 1 * 4 + 0 ] ) * s;
645 }
646 else
647 {
648 // diagonal is negative
649 i = 0;
650 if( mat[ 1 * 4 + 1 ] > mat[ 0 * 4 + 0 ] )
651 i = 1;
652 if( mat[ 2 * 4 + 2 ] > mat[ i * 4 + i ] )
653 i = 2;
654 j = nxt[ i ];
655 k = nxt[ j ];
656
657 s = (Type)sqrt( ( mat[ i * 4 + i ] - ( mat[ j * 4 + j ] + mat[ k * 4 + k ] ) ) + 1.0 );
658
659 tq[ i ] = s * 0.5f;
660
661 if( s != 0.0f )
662 s = 0.5f / s;
663
664 tq[ 3 ] = ( mat[ j * 4 + k ] - mat[ k * 4 + j ] ) * s;
665 tq[ j ] = ( mat[ i * 4 + j ] + mat[ j * 4 + i ] ) * s;
666 tq[ k ] = ( mat[ i * 4 + k ] + mat[ k * 4 + i ] ) * s;
667
668 _v[ 0 ] = tq[ 0 ];
669 _v[ 1 ] = tq[ 1 ];
670 _v[ 2 ] = tq[ 2 ];
671 _v[ 3 ] = tq[ 3 ];
672 }
673 }
674
675 /** Returns the rotation about the X axis
676 * \return The rotation about the X axis
677 */
678 Type GetRoll() const
679 {
680 return atan2( 2 * ( _v[ 1 ] * _v[ 2 ] + _v[ 3 ] * _v[ 0 ] ), _v[ 3 ] * _v[ 3 ] - _v[ 0 ] * _v[ 0 ] - _v[ 1 ] * _v[ 1 ] + _v[ 2 ] * _v[ 2 ] );
681 }
682
683 /** Returns the rotation about the Y axis
684 * \return The rotation about the Y axis
685 */
686 Type GetPitch() const
687 {
688 Type tmp = -2 * ( _v[ 0 ] * _v[ 2 ] - _v[ 3 ] * _v[ 1 ] );
689
690 if( tmp > 1.0 )
691 {
692 tmp = 1.0;
693 }
694 else if( tmp < -1.0 )
695 {
696 tmp = -1.0;
697 }
698
699 return asin( tmp );
700 }
701 /** Returns the rotation about the Z axis
702 * \return The rotation about the Z axis
703 */
704 Type GetYaw() const
705 {
706 return atan2( 2 * ( _v[ 0 ] * _v[ 1 ] + _v[ 3 ] * _v[ 2 ] ), _v[ 3 ] * _v[ 3 ] + _v[ 0 ] * _v[ 0 ] - _v[ 1 ] * _v[ 1 ] - _v[ 2 ] * _v[ 2 ] );
707 }
708
709 /** Returns the Euler Angles as a Vector (Warning: SLOW)
710 * \return The Euler Angles as a Vector
711 */
713 {
714 // This is slow
715 return Vector( float( GetRoll() ), float( GetPitch() ), float( GetYaw() ) );
716 }
717
718 /** Gets the rotation angle (radians) and the axis about which the angle is rotated.
719 * \param angle Returns the angle of rotation of the quaternion
720 * \param axis Returns the axis of rotation of the quaternion
721 */
722 void GetAngleAxis( Type* angle, Vector* axis )
723 {
724// set sure we don't get a NaN result from acos...
725#ifdef _WIN32
726 if( abs( _v[ 3 ] ) > (Type)1.0 )
727#else
728 if( fabs( (double)_v[ 3 ] ) > 1.0 )
729#endif
730 {
731 this->Normalize();
732 }
733
734 // set the angle - aCos is mathematically defined to be between 0 and PI
735 Type rad = acos( _v[ 3 ] ) * (Type)2.0;
736
737 if( angle )
738 {
739 *angle = rad;
740 }
741
742 // set the axis: (use sin(rad) instead of asin(w))
743 if( axis )
744 {
745 Type sin_half_angle = sin( rad * (Type)0.5 );
746 if( sin_half_angle >= (Type)0.0001 ) // because (PI >= rad >= 0)
747 {
748 Type sin_half_angle_inv = Type( 1.0 ) / sin_half_angle;
749 axis->x = float( _v[ 0 ] * sin_half_angle_inv );
750 axis->y = float( _v[ 1 ] * sin_half_angle_inv );
751 axis->z = float( _v[ 2 ] * sin_half_angle_inv );
752 axis->Normalize();
753 }
754
755 // avoid NAN
756 else
757 {
758 // one of the terms should be a 1,
759 // so we can maintain unit-ness
760 // in case w is 0 (which here w is 0)
761 axis->x = (float)1.0;
762 axis->y = (float)0.0;
763 axis->z = (float)0.0;
764 }
765 }
766 }
767
768 /** Returns a rotation matrix representation of the quaternion.
769 * \return The rotation matrix computed from the quaternion
770 */
772 {
773 Type wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
774 Type mat[ 4 ][ 4 ];
775
776 x2 = _v[ 0 ] + _v[ 0 ];
777 y2 = _v[ 1 ] + _v[ 1 ];
778 z2 = _v[ 2 ] + _v[ 2 ];
779
780 xx = _v[ 0 ] * x2;
781 xy = _v[ 0 ] * y2;
782 xz = _v[ 0 ] * z2;
783
784 yy = _v[ 1 ] * y2;
785 yz = _v[ 1 ] * z2;
786 zz = _v[ 2 ] * z2;
787
788 wx = _v[ 3 ] * x2;
789 wy = _v[ 3 ] * y2;
790 wz = _v[ 3 ] * z2;
791
792 mat[ 0 ][ 0 ] = 1.0f - ( yy + zz );
793 mat[ 0 ][ 1 ] = xy - wz;
794 mat[ 0 ][ 2 ] = xz + wy;
795 mat[ 0 ][ 3 ] = 0.0f;
796
797 mat[ 1 ][ 0 ] = xy + wz;
798 mat[ 1 ][ 1 ] = 1.0f - ( xx + zz );
799 mat[ 1 ][ 2 ] = yz - wx;
800 mat[ 1 ][ 3 ] = 0.0f;
801
802 mat[ 2 ][ 0 ] = xz - wy;
803 mat[ 2 ][ 1 ] = yz + wx;
804 mat[ 2 ][ 2 ] = 1.0f - ( xx + yy );
805 mat[ 2 ][ 3 ] = 0.0f;
806
807 mat[ 3 ][ 0 ] = 0;
808 mat[ 3 ][ 1 ] = 0;
809 mat[ 3 ][ 2 ] = 0;
810 mat[ 3 ][ 3 ] = 1;
811
812 return GlsMatrixAffine<Type>( mat );
813 }
814
815 /** Sets this quaternion to an interpolated rotation between the
816 * from and to vectors using spherical linear interpolation (slerp).
817 * \param t Percent of interpelation (0.0 -> 1.0)
818 * \param from The starting quaternion
819 * \param to The ending quaternion
820 * \param adjustSign Whether or not to negate the sign of the to quaternion
821 */
822 void SetFromSlerp( float t, const GlsQuaternion& from, const GlsQuaternion& to, bool adjustSign = true )
823 {
824 const GlsQuaternion& p = from;
825
826 // calc cosine theta
827 Type cosom = from.DotProduct( to );
828
829 // adjust signs (if necessary)
831 if( adjustSign && ( cosom < (Type)0.0 ) )
832 {
833 cosom = -cosom;
834 q._v[ 0 ] = -to._v[ 0 ]; // Reverse all signs
835 q._v[ 1 ] = -to._v[ 1 ];
836 q._v[ 2 ] = -to._v[ 2 ];
837 q._v[ 3 ] = -to._v[ 3 ];
838 }
839 else
840 {
841 q = to;
842 }
843
844 // Calculate coefficients
845 Type sclp, sclq;
846 if( ( (Type)1.0 - cosom ) > (Type)0.0001 ) // 0.0001 -> some epsillon
847 {
848 // Standard case (slerp)
849 Type omega, sinom;
850 omega = acos( cosom ); // extract theta from dot product's cos theta
851 sinom = sin( omega );
852 sclp = sin( ( (Type)1.0 - t ) * omega ) / sinom;
853 sclq = sin( t * omega ) / sinom;
854 }
855 else
856 {
857 // Very close, do linear interp (because it's faster)
858 sclp = (Type)1.0 - t;
859 sclq = t;
860 }
861
862 _v[ 0 ] = sclp * p._v[ 0 ] + sclq * q._v[ 0 ];
863 _v[ 1 ] = sclp * p._v[ 1 ] + sclq * q._v[ 1 ];
864 _v[ 2 ] = sclp * p._v[ 2 ] + sclq * q._v[ 2 ];
865 _v[ 3 ] = sclp * p._v[ 3 ] + sclq * q._v[ 3 ];
866 }
867
868protected:
869 Type _v[ 4 ]; ///< The four components of the quaternion.
870
871 /// Circular dependancies prevent us from using VeryCloseToZero in util.h.
872 /// \param value The value to compare.
873 /// \return True if the value if close enough to zero.
874 static bool IsVeryCloseToZero( double value ) { return fabs( value ) < 0.0001; }
875};
876
877typedef GlsQuaternion<float> GlsQuaternionF; ///< Typedef for a quaternion of floats.
878typedef GlsQuaternion<double> GlsQuaternionD; ///< Typedef for a quaternion of doubles.
879
880/// Stream out operator
881/// \param outstr The stream to write to.
882/// \param quat The value to write to the stream.
883/// \return The stream in its current state.
884template<class Type>
885std::ostream& operator<<( std::ostream& outstr, const GlsQuaternion<Type>& quat )
886{
887 outstr.precision( MaxDigits10<Type>::value );
888
889 outstr.width( 1 );
890 outstr << quat.Data()[ 0 ] << " ";
891 outstr.width( 8 );
892 outstr << quat.Data()[ 1 ] << " ";
893 outstr.width( 8 );
894 outstr << quat.Data()[ 2 ] << " ";
895 outstr.width( 8 );
896 outstr << quat.Data()[ 3 ];
897
898 return outstr;
899}
900
901/// Stream in operator
902/// \param instr The stream to read from.
903/// \param quat The returned value read from the stream.
904/// \return The stream in its current state.
905template<class Type>
906std::istream& operator>>( std::istream& instr, GlsQuaternion<Type>& quat )
907{
908 instr >> quat.Data()[ 0 ] >> quat.Data()[ 1 ] >> quat.Data()[ 2 ] >> quat.Data()[ 3 ];
909 return instr;
910}
911
912} // end namespace disti
913
914#endif
Definition: gls_matrix_affine.h:60
Definition: gls_quaternion.h:55
void Set(Type x, Type y, Type z, Type w)
Definition: gls_quaternion.h:476
Type GetRoll() const
Definition: gls_quaternion.h:678
bool operator==(const GlsQuaternion &q) const
Definition: gls_quaternion.h:182
void SetFromSlerp(float t, const GlsQuaternion &from, const GlsQuaternion &to, bool adjustSign=true)
Definition: gls_quaternion.h:822
void SetFromVectors(const Vector &to, const Vector &from)
Definition: gls_quaternion.h:572
GlsQuaternion & operator+=(const GlsQuaternion &q)
Definition: gls_quaternion.h:355
GlsQuaternion(Type angle, const Vector &axis)
Definition: gls_quaternion.h:94
GlsQuaternion & operator/=(const GlsQuaternion &q)
Definition: gls_quaternion.h:329
static bool IsVeryCloseToZero(double value)
Definition: gls_quaternion.h:874
const GlsQuaternion operator/(const GlsQuaternion &q) const
Definition: gls_quaternion.h:317
GlsQuaternion & operator-=(const GlsQuaternion &q)
Definition: gls_quaternion.h:384
GlsQuaternion & operator/=(Type s)
Definition: gls_quaternion.h:300
Type * Data()
Definition: gls_quaternion.h:159
GlsQuaternion operator/(Type s) const
Definition: gls_quaternion.h:287
void SetFromAngleAxis(Type angle1, const Vector &axis1, Type angle2, const Vector &axis2, Type angle3, const Vector &axis3)
Definition: gls_quaternion.h:513
Type _v[4]
The four components of the quaternion.
Definition: gls_quaternion.h:869
const GlsQuaternion operator+(const GlsQuaternion &q) const
Definition: gls_quaternion.h:342
const Type * Data() const
Definition: gls_quaternion.h:170
void GetAngleAxis(Type *angle, Vector *axis)
Definition: gls_quaternion.h:722
const GlsQuaternion Inverse() const
Definition: gls_quaternion.h:440
Type GetYaw() const
Definition: gls_quaternion.h:704
Type MagnitudeSquared() const
Definition: gls_quaternion.h:418
GlsQuaternion(Type x, Type y, Type z, Type w)
Definition: gls_quaternion.h:79
void SetFromEulerAngles(Type angleX, Type angleY, Type angleZ)
Definition: gls_quaternion.h:532
GlsQuaternion(const GlsMatrixAffine< Type > &matrix)
Definition: gls_quaternion.h:148
GlsQuaternion & operator*=(const GlsQuaternion &q)
Definition: gls_quaternion.h:266
const GlsQuaternion operator*(const GlsQuaternion &q) const
Definition: gls_quaternion.h:255
const GlsQuaternion operator*(Type s) const
Definition: gls_quaternion.h:231
GlsQuaternion(const Vector &anglesVec)
Definition: gls_quaternion.h:137
Vector GetEulerAngles()
Definition: gls_quaternion.h:712
void SetFromRotationMatrix(const GlsMatrixAffine< Type > &matrix)
Definition: gls_quaternion.h:625
GlsQuaternion(Type angleX, Type angleY, Type angleZ)
Definition: gls_quaternion.h:125
GlsQuaternion(Type angle1, const Vector &axis1, Type angle2, const Vector &axis2, Type angle3, const Vector &axis3)
Definition: gls_quaternion.h:110
bool operator!=(const GlsQuaternion &q) const
Definition: gls_quaternion.h:194
const GlsQuaternion operator-() const
Definition: gls_quaternion.h:396
void SetFromAngleAxis(Type angle, const Vector &axis)
Definition: gls_quaternion.h:491
GlsQuaternion()
Definition: gls_quaternion.h:62
Type GetPitch() const
Definition: gls_quaternion.h:686
GlsQuaternion Conjugate() const
Definition: gls_quaternion.h:429
GlsQuaternion & operator*=(Type s)
Definition: gls_quaternion.h:243
Type DotProduct(const GlsQuaternion &q) const
Definition: gls_quaternion.h:452
Type Magnitude() const
Definition: gls_quaternion.h:407
GlsMatrixAffine< Type > GetRotationMatrix()
Definition: gls_quaternion.h:771
void Normalize()
Definition: gls_quaternion.h:462
bool operator<(const GlsQuaternion &q) const
Definition: gls_quaternion.h:206
const GlsQuaternion operator-(const GlsQuaternion &q) const
Definition: gls_quaternion.h:371
Definition: vertex.h:84
VertexNoColor CrossProduct(const VertexNoColor &w) const
Definition: vertex.h:348
float DotProduct(const VertexNoColor &w) const
Definition: vertex.h:359
float y
The Y component.
Definition: vertex.h:87
float x
The X component.
Definition: vertex.h:86
float Magnitude() const
Definition: vertex.h:329
void Normalize()
Definition: vertex.h:310
float z
The Z component.
Definition: vertex.h:88
Macros and helper code to determine what subset of C++11/14/17 is available.
Definition: bmpimage.h:46
GlsQuaternion< float > GlsQuaternionF
Typedef for a quaternion of floats.
Definition: gls_quaternion.h:877
std::ostream & operator<<(std::ostream &outstr, const AttributeName &name)
GlsQuaternion< double > GlsQuaternionD
Typedef for a quaternion of doubles.
Definition: gls_quaternion.h:878
VertexNoColor Vector
Definition: gls_font_base.h:68
std::istream & operator>>(std::istream &instr, GlsColor &color)
Definition: gls_cpp_lang_support.h:498
The disti::Vertex class. A class for manipulating 3D vertices.