Polar Coordinate Systems - 3D Math Primer for Graphics and Game Development - page 210

/ / At t h e o r i g i n − slam a n g l e s t o z e r o

h e a d i n g = p i t c h = 0 . 0 f ;

} e l s e {

/ / Handle n e g a t i v e d i s t a n c e

i f ( r < 0 . 0 f ) {

r = − r ;
h e a d i n g += P I ;

p i t c h = − p i t c h ;

}

/ / P i t c h o u t o f r a n g e ?

i f ( f a b s ( p i t c h ) > PIOVERTWO ) {

/ / O f f s e t by 90 d e g r e e s

p i t c h += PIOVERTWO ;

/ / Wrap i n r a n g e 0 . . . TWOPI

p i t c h − = f l o o r ( p i t c h / TWOPI ) ∗ TWOPI ;

/ / Out o f r a n g e ?

i f ( p i t c h > P I ) {

/ / F l i p h e a d i n g

h e a d i n g += P I ;

/ / Undo o f f s e t and a l s o s e t p i t c h = 180 − p i t c h

p i t c h = 3 . 0 f ∗ P I / 2 . 0 f − p i t c h ; / / p = 270 d e g r e e s − p

} e l s e {

/ / Undo o f f s e t , s h i f t i n g p i t c h i n r a n g e

/ / − 90 d e g r e e s . . . +90 d e g r e e s

p i t c h − = PIOVERTWO ;

}

}

/ / Gimbal l o c k ? T e s t u s i n g a r e l a t i v e l y s m a l l t o l e r a n c e

/ / here , c l o s e t o t h e l i m i t s o f s i n g l e p r e c i s i o n .

i f ( f a b s ( p i t c h ) > = PIOVERTWO ∗ 0 . 9 9 9 9 ) {

h e a d i n g = 0 . 0 f ;

} e l s e {

/ / Wrap heading , a v o i d i n g math when p o s s i b l e

/ / t o p r e s e r v e p r e c i s i o n

i f ( f a b s ( h e a d i n g ) > P I ) {

/ / O f f s e t by P I

h e a d i n g += P I ;

/ / Wrap i n r a n g e 0 . . . TWOPI

h e a d i n g − = f l o o r ( h e a d i n g / TWOPI ) ∗ TWOPI ;

/ / Undo o f f s e t , s h i f t i n g a n g l e back i n r a n g e − P I . . . P I

h e a d i n g − = P I ;

}

}

}

Listing 7.3
Converting spherical coordinates to canonical form