/ *   
 
   *   P R O J E C T :   N y A R T o o l k i t C S 
 
   *   - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 
 
   *   T h i s   w o r k   i s   b a s e d   o n   t h e   o r i g i n a l   A R T o o l K i t   d e v e l o p e d   b y 
 
   *       H i r o k a z u   K a t o 
 
   *       M a r k   B i l l i n g h u r s t 
 
   *       H I T L a b ,   U n i v e r s i t y   o f   W a s h i n g t o n ,   S e a t t l e 
 
   *   h t t p : / / w w w . h i t l . w a s h i n g t o n . e d u / a r t o o l k i t / 
 
   * 
 
   *   T h e   N y A R T o o l k i t C S   i s   J a v a   e d i t i o n   A R T o o l K i t   c l a s s   l i b r a r y . 
 
   *   C o p y r i g h t   ( C ) 2 0 0 8 - 2 0 0 9   R y o   I i z u k a 
 
   * 
 
   *   T h i s   p r o g r a m   i s   f r e e   s o f t w a r e :   y o u   c a n   r e d i s t r i b u t e   i t   a n d / o r   m o d i f y 
 
   *   i t   u n d e r   t h e   t e r m s   o f   t h e   G N U   G e n e r a l   P u b l i c   L i c e n s e   a s   p u b l i s h e d   b y 
 
   *   t h e   F r e e   S o f t w a r e   F o u n d a t i o n ,   e i t h e r   v e r s i o n   3   o f   t h e   L i c e n s e ,   o r 
 
   *   ( a t   y o u r   o p t i o n )   a n y   l a t e r   v e r s i o n . 
 
   *   
 
   *   T h i s   p r o g r a m   i s   d i s t r i b u t e d   i n   t h e   h o p e   t h a t   i t   w i l l   b e   u s e f u l , 
 
   *   b u t   W I T H O U T   A N Y   W A R R A N T Y ;   w i t h o u t   e v e n   t h e   i m p l i e d   w a r r a n t y   o f 
 
   *   M E R C H A N T A B I L I T Y   o r   F I T N E S S   F O R   A   P A R T I C U L A R   P U R P O S E .     S e e   t h e 
 
   *   G N U   G e n e r a l   P u b l i c   L i c e n s e   f o r   m o r e   d e t a i l s . 
 
   * 
 
   *   Y o u   s h o u l d   h a v e   r e c e i v e d   a   c o p y   o f   t h e   G N U   G e n e r a l   P u b l i c   L i c e n s e 
 
   *   a l o n g   w i t h   t h i s   p r o g r a m .     I f   n o t ,   s e e   < h t t p : / / w w w . g n u . o r g / l i c e n s e s / > . 
 
   *   
 
   *   F o r   f u r t h e r   i n f o r m a t i o n   p l e a s e   c o n t a c t . 
 
   * 	 h t t p : / / n y a t l a . j p / n y a t o o l k i t / 
 
   * 	 < a i r m a i l ( a t ) e b o n y . p l a l a . o r . j p >   o r   < n y a t l a ( a t ) n y a t l a . j p > 
 
   *   
 
   * / 
 
 u s i n g   S y s t e m ; 
 
 n a m e s p a c e   j p . n y a t l a . n y a r t o o l k i t . c s . c o r e 
 
 { 
 
 
 
 
 
 
 
 
 
 
 
         / * * 
 
           *   S0n0000o00A R T o o l K i t h0TX0{P}g0QRY000LR000g0Y00
 
           * / 
 
         p u b l i c   c l a s s   N y A R R o t M a t r i x _ A R T o o l K i t   :   N y A R R o t M a t r i x 
 
         { 
 
                 / * * 
 
                   *   0000000g0Y00
 
                   *   SgqY00\q_	Yc0000000c[W0f000000000ubW0~0Y00
 
                   *   @ p a r a m   i _ m a t r i x 
 
                   *   SgqY00\q_	Yc000000
 
                   *   @ 
 
                   * / 
 
                 p u b l i c   N y A R R o t M a t r i x _ A R T o o l K i t ( N y A R P e r s p e c t i v e P r o j e c t i o n M a t r i x   i _ m a t r i x ) : b a s e ( i _ m a t r i x ) 
 
                 { 
 
                         
 
                         t h i s . _ a n g l e   =   n e w   N y A R D o u b l e P o i n t 3 d ( ) ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * *   A R T o o l k i t 0000n0҉^$Pg0Y00* / 
 
                 p r o t e c t e d   N y A R D o u b l e P o i n t 3 d   _ a n g l e ; 
 
 
 
                 / / o v e r r i d e 
 
                 p u b l i c   s e a l e d   o v e r r i d e   b o o l   i n i t R o t B y S q u a r e ( N y A R L i n e a r [ ]   i _ l i n e a r ,   N y A R D o u b l e P o i n t 2 d [ ]   i _ s q v e r t e x ) 
 
                 { 
 
                         b o o l   r e t   =   b a s e . i n i t R o t B y S q u a r e ( i _ l i n e a r ,   i _ s q v e r t e x ) ; 
 
                         i f   ( r e t ) 
 
                         { 
 
                                 / / M a t r i x K00a n g l e 0000
 
                                 t h i s . u p d a t e A n g l e F r o m M a t r i x ( ) ; 
 
                         } 
 
                         r e t u r n   r e t ; 
 
                 } 
 
                 / * * 
 
                   *   A R T o o l K i t 0000n0҉^$Pn0Sgq$P0ԏW0~0Y00
 
                   *   @ r e t u r n 
 
                   *   [ r e a d   o n l y ] ҉^$P
 
                   * / 
 
                 p u b l i c   N y A R D o u b l e P o i n t 3 d   g e t A n g l e ( ) 
 
                 { 
 
                         r e t u r n   t h i s . _ a n g l e ; 
 
                 } 
 
                 / * * 
 
                   *   i _ b u f k0{ @ l i n k   # g e t A n g l e ( ) } n0P}g0000W0f0ԏW0~0Y00
 
                   *   @ r e t u r n 
 
                   *   W0_0{ @ l i n k   # g e t A n g l e ( ) } n0$P
 
                   * / 
 
                 p u b l i c   N y A R D o u b l e P o i n t 3 d   g e t A n g l e ( N y A R D o u b l e P o i n t 3 d   i _ b u f ) 
 
                 { 
 
                         i _ b u f . s e t V a l u e ( t h i s . _ a n g l e ) ; 
 
                         r e t u r n   i _ b u f ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   A R T o o l K i t 0000n0҉^$P00LRk0000W0~0Y00
 
                   *   @ p a r a m   i _ x 
 
                   *   X n0Vϑ0
 
                   *   @ p a r a m   i _ y 
 
                   *   Y n0Vϑ0
 
                   *   @ p a r a m   i _ z 
 
                   *   Z n0Vϑ0
 
                   * / 
 
                 p u b l i c   v i r t u a l   v o i d   s e t A n g l e ( d o u b l e   i _ x ,   d o u b l e   i _ y ,   d o u b l e   i _ z ) 
 
                 { 
 
                         d o u b l e   s i n a   =   M a t h . S i n ( i _ x ) ; 
 
                         d o u b l e   c o s a   =   M a t h . C o s ( i _ x ) ; 
 
                         d o u b l e   s i n b   =   M a t h . S i n ( i _ y ) ; 
 
                         d o u b l e   c o s b   =   M a t h . C o s ( i _ y ) ; 
 
                         d o u b l e   s i n c   =   M a t h . S i n ( i _ z ) ; 
 
                         d o u b l e   c o s c   =   M a t h . C o s ( i _ z ) ; 
 
                         / /   O p t i m i z e 
 
                         d o u b l e   C A C A   =   c o s a   *   c o s a ; 
 
                         d o u b l e   S A S A   =   s i n a   *   s i n a ; 
 
                         d o u b l e   S A C A   =   s i n a   *   c o s a ; 
 
                         d o u b l e   S A S B   =   s i n a   *   s i n b ; 
 
                         d o u b l e   C A S B   =   c o s a   *   s i n b ; 
 
                         d o u b l e   S A C A C B   =   S A C A   *   c o s b ; 
 
 
 
                         t h i s . m 0 0   =   C A C A   *   c o s b   *   c o s c   +   S A S A   *   c o s c   +   S A C A C B   *   s i n c   -   S A C A   *   s i n c ; 
 
                         t h i s . m 0 1   =   - C A C A   *   c o s b   *   s i n c   -   S A S A   *   s i n c   +   S A C A C B   *   c o s c   -   S A C A   *   c o s c ; 
 
                         t h i s . m 0 2   =   C A S B ; 
 
                         t h i s . m 1 0   =   S A C A C B   *   c o s c   -   S A C A   *   c o s c   +   S A S A   *   c o s b   *   s i n c   +   C A C A   *   s i n c ; 
 
                         t h i s . m 1 1   =   - S A C A C B   *   s i n c   +   S A C A   *   s i n c   +   S A S A   *   c o s b   *   c o s c   +   C A C A   *   c o s c ; 
 
                         t h i s . m 1 2   =   S A S B ; 
 
                         t h i s . m 2 0   =   - C A S B   *   c o s c   -   S A S B   *   s i n c ; 
 
                         t h i s . m 2 1   =   C A S B   *   s i n c   -   S A S B   *   c o s c ; 
 
                         t h i s . m 2 2   =   c o s b ; 
 
                         u p d a t e A n g l e F r o m M a t r i x ( ) ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   s(Wn0M a t r i x K00a n g k e 0_CQY000
 
                   *   @ p a r a m   o _ a n g l e 
 
                   * / 
 
                 p r i v a t e   v o i d   u p d a t e A n g l e F r o m M a t r i x ( ) 
 
                 { 
 
                         d o u b l e   a ,   b ,   c ; 
 
                         d o u b l e   s i n a ,   c o s a ,   s i n b ,   c o s b ,   s i n c ,   c o s c ; 
 
 
 
                         i f   ( t h i s . m 2 2   >   1 . 0 ) 
 
                         { / /   < O p t i m i z e / > i f (   r o t [ 2 ] [ 2 ]   >   1 . 0   )   { 
 
                                 c o s b   =   1 . 0 ; / /   < O p t i m i z e / > r o t [ 2 ] [ 2 ]   =   1 . 0 ; 
 
                         } 
 
                         e l s e   i f   ( t h i s . m 2 2   <   - 1 . 0 ) 
 
                         { / /   < O p t i m i z e / > } e l s e   i f (   r o t [ 2 ] [ 2 ]   <   - 1 . 0   )   { 
 
                                 c o s b   =   - 1 . 0 ; / /   < O p t i m i z e / > r o t [ 2 ] [ 2 ]   =   - 1 . 0 ; 
 
                         } 
 
                         e l s e 
 
                         { 
 
                                 c o s b   =   t h i s . m 2 2 ; / /   < O p t i m i z e / > c o s b   =   r o t [ 2 ] [ 2 ] ; 
 
                         } 
 
                         b   =   M a t h . A c o s ( c o s b ) ; 
 
                         s i n b   =   M a t h . S i n ( b ) ; 
 
                         d o u b l e   r o t 0 2   =   t h i s . m 0 2 ; 
 
                         d o u b l e   r o t 1 2   =   t h i s . m 1 2 ; 
 
                         i f   ( b   > =   0 . 0 0 0 0 0 1   | |   b   < =   - 0 . 0 0 0 0 0 1 ) 
 
                         { 
 
                                 c o s a   =   r o t 0 2   /   s i n b ; / /   < O p t i m i z e / > c o s a   =   r o t [ 0 ] [ 2 ]   /   s i n b ; 
 
                                 s i n a   =   r o t 1 2   /   s i n b ; / /   < O p t i m i z e / > s i n a   =   r o t [ 1 ] [ 2 ]   /   s i n b ; 
 
                                 i f   ( c o s a   >   1 . 0 ) 
 
                                 { 
 
                                         c o s a   =   1 . 0 ; 
 
                                         s i n a   =   0 . 0 ; 
 
                                 } 
 
                                 i f   ( c o s a   <   - 1 . 0 ) 
 
                                 { 
 
                                         c o s a   =   - 1 . 0 ; 
 
                                         s i n a   =   0 . 0 ; 
 
                                 } 
 
                                 i f   ( s i n a   >   1 . 0 ) 
 
                                 { 
 
                                         s i n a   =   1 . 0 ; 
 
                                         c o s a   =   0 . 0 ; 
 
                                 } 
 
                                 i f   ( s i n a   <   - 1 . 0 ) 
 
                                 { 
 
                                         s i n a   =   - 1 . 0 ; 
 
                                         c o s a   =   0 . 0 ; 
 
                                 } 
 
                                 a   =   M a t h . A c o s ( c o s a ) ; 
 
                                 i f   ( s i n a   <   0 ) 
 
                                 { 
 
                                         a   =   - a ; 
 
                                 } 
 
                                 d o u b l e   t m p   =   ( r o t 0 2   *   r o t 0 2   +   r o t 1 2   *   r o t 1 2 ) ; 
 
                                 s i n c   =   ( t h i s . m 2 1   *   r o t 0 2   -   t h i s . m 2 0   *   r o t 1 2 )   /   t m p ; 
 
                                 c o s c   =   - ( r o t 0 2   *   t h i s . m 2 0   +   r o t 1 2   *   t h i s . m 2 1 )   /   t m p ; 
 
 
 
                                 i f   ( c o s c   >   1 . 0 ) 
 
                                 { 
 
                                         c o s c   =   1 . 0 ; 
 
                                         s i n c   =   0 . 0 ; 
 
                                 } 
 
                                 i f   ( c o s c   <   - 1 . 0 ) 
 
                                 { 
 
                                         c o s c   =   - 1 . 0 ; 
 
                                         s i n c   =   0 . 0 ; 
 
                                 } 
 
                                 i f   ( s i n c   >   1 . 0 ) 
 
                                 { 
 
                                         s i n c   =   1 . 0 ; 
 
                                         c o s c   =   0 . 0 ; 
 
                                 } 
 
                                 i f   ( s i n c   <   - 1 . 0 ) 
 
                                 { 
 
                                         s i n c   =   - 1 . 0 ; 
 
                                         c o s c   =   0 . 0 ; 
 
                                 } 
 
                                 c   =   M a t h . A c o s ( c o s c ) ; 
 
                                 i f   ( s i n c   <   0 ) 
 
                                 { 
 
                                         c   =   - c ; 
 
                                 } 
 
                         } 
 
                         e l s e 
 
                         { 
 
                                 a   =   b   =   0 . 0 ; 
 
                                 c o s a   =   c o s b   =   1 . 0 ; 
 
                                 s i n a   =   s i n b   =   0 . 0 ; 
 
                                 c o s c   =   t h i s . m 0 0 ; / / c o s c   =   r o t [ 0 ] ; / /   < O p t i m i z e / > c o s c   =   r o t [ 0 ] [ 0 ] ; 
 
                                 s i n c   =   t h i s . m 0 1 ; / / s i n c   =   r o t [ 1 ] ; / /   < O p t i m i z e / > s i n c   =   r o t [ 1 ] [ 0 ] ; 
 
                                 i f   ( c o s c   >   1 . 0 ) 
 
                                 { 
 
                                         c o s c   =   1 . 0 ; 
 
                                         s i n c   =   0 . 0 ; 
 
                                 } 
 
                                 i f   ( c o s c   <   - 1 . 0 ) 
 
                                 { 
 
                                         c o s c   =   - 1 . 0 ; 
 
                                         s i n c   =   0 . 0 ; 
 
                                 } 
 
                                 i f   ( s i n c   >   1 . 0 ) 
 
                                 { 
 
                                         s i n c   =   1 . 0 ; 
 
                                         c o s c   =   0 . 0 ; 
 
                                 } 
 
                                 i f   ( s i n c   <   - 1 . 0 ) 
 
                                 { 
 
                                         s i n c   =   - 1 . 0 ; 
 
                                         c o s c   =   0 . 0 ; 
 
                                 } 
 
                                 c   =   M a t h . A c o s ( c o s c ) ; 
 
                                 i f   ( s i n c   <   0 ) 
 
                                 { 
 
                                         c   =   - c ; 
 
                                 } 
 
                         } 
 
                         / / a n g l e n0fe
 
                         t h i s . _ a n g l e . x   =   a ; / /   w a . v a l u e = a ; / / * w a   =   a ; 
 
                         t h i s . _ a n g l e . y   =   b ; / /   w b . v a l u e = b ; / / * w b   =   b ; 
 
                         t h i s . _ a n g l e . z   =   c ; / /   w c . v a l u e = c ; / / * w c   =   c ; 
 
                         r e t u r n ; 
 
                 } 
 
         } 
 
 } 