Rotation in Three Dimensions - 3D Math Primer for Graphics and Game Development - page 290

/ / I n p u t q u a t e r n i o n

f l o a t w, x , y , z ;

/ / Output E u l e r a n g l e s ( r a d i a n s )

f l o a t h , p , b ;

/ / E x t r a c t s i n ( p i t c h )

f l o a t s p = − 2.0 f ∗ ( y ∗ z − w ∗ x ) ;

/ / Check f o r Gimbal l o c k , g i v i n g s l i g h t t o l e r a n c e

/ / f o r n u m e r i c a l i m p r e c i s i o n

i f ( f a b s ( s p ) > 0 . 9 9 9 9 f ) {

/ / L o o k i n g s t r a i g h t up o r down

p = 1.570796 f ∗ s p ; / / p i /2

/ / Compute heading , slam bank t o z e r o

h = a t a n 2 ( − x ∗ z + w ∗ y , 0 . 5 f − y ∗ y − z ∗ z ) ;

b = 0 . 0 f ;

} e l s e {

/ / Compute a n g l e s

p = a s i n ( s p ) ;

h = a t a n 2 ( x ∗ z + w ∗ y , 0 . 5 f − x ∗ x − y ∗ y ) ;

b = a t a n 2 ( x ∗ y + w ∗ z , 0 . 5 f − x ∗ x − z ∗ z ) ;

}

Listing 8.6
Converting an object-to-upright quaternion to Euler angles

To convert an upright-to-object quaternion to Euler angle format, we
use nearly identical code, only with the x, y, and z values negated, since
we assume the upright-to-object quaternion is the conjugate of the object-
to-upright quaternion.

/ / E x t r a c t s i n ( p i t c h )

f l o a t s p = − 2.0 f ∗ ( y ∗ z + w ∗ x ) ;

/ / Check f o r Gimbal l o c k , g i v i n g s l i g h t t o l e r a n c e

/ / f o r n u m e r i c a l i m p r e c i s i o n

i f ( f a b s ( s p ) > 0 . 9 9 9 9 f ) {

/ / L o o k i n g s t r a i g h t up o r down

p = 1.570796 f ∗ s p ; / / p i /2

/ / Compute heading , slam bank t o z e r o

h = a t a n 2 ( − x ∗ z − w ∗ y , 0 . 5 f − y ∗ y − z ∗ z ) ;

b = 0 . 0 f ;

} e l s e {

/ / Compute a n g l e s

p = a s i n ( s p ) ;

h = a t a n 2 ( x ∗ z − w ∗ y , 0 . 5 f − x ∗ x − y ∗ y ) ;

b = a t a n 2 ( x ∗ y − w ∗ z , 0 . 5 f − x ∗ x − z ∗ z ) ;

}

Listing 8.7
Converting an upright-to-object quaternion to Euler angles