| GL Studio C++ Runtime API

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