/ *   
 
   *   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0	N!kCQ00000<h
}W0~0Y00
 
           *    Nn0{peo00{ @ l i n k   N y A R R o t M a t r i x V 2 } x0yRW0~0W0_00
 
           *   8^0000L0S0n00000OF0S0h0o0B00~0[000{ @ l i n k   N y A R R o t M a t r i x } 000K00OD0~0Y00
 
           * / 
 
         p u b l i c   c l a s s   N y A R R o t V e c t o r 
 
         { 
 
                 / * *   0000 }1 * / 
 
                 p u b l i c   d o u b l e   v 1 ; 
 
                 / * *   0000 }2 * / 
 
                 p u b l i c   d o u b l e   v 2 ; 
 
                 / * *   0000 }3 * / 
 
                 p u b l i c   d o u b l e   v 3 ; 
 
 
 
                 / * * 
 
                   *   0000000g0Y00
 
                   *   Rg$P0c_0j0D00000000ubW0~0Y00
 
                   *   @ 
 
                   * / 
 
                 p u b l i c   N y A R R o t V e c t o r ( ) 
 
                 { 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo00A R T o o l K i t n0c h e c k _ r o t a t i o n k0vS_Y00{0W0~0Y00
 
                   *   s0}o0
Nfg0Y00( 2 d0n00000n0O0te) 
 
                   *   @ 
 
                   * / 
 
                 p u b l i c   s t a t i c   b o o l   c h e c k R o t a t i o n ( N y A R R o t V e c t o r   i o _ v e c 1 ,   N y A R R o t V e c t o r   i o _ v e c 2 ) 
 
                 { 
 
                         d o u b l e   w ; 
 
                         i n t   f ; 
 
 
 
                         d o u b l e   v e c 1 0   =   i o _ v e c 1 . v 1 ; 
 
                         d o u b l e   v e c 1 1   =   i o _ v e c 1 . v 2 ; 
 
                         d o u b l e   v e c 1 2   =   i o _ v e c 1 . v 3 ; 
 
                         d o u b l e   v e c 2 0   =   i o _ v e c 2 . v 1 ; 
 
                         d o u b l e   v e c 2 1   =   i o _ v e c 2 . v 2 ; 
 
                         d o u b l e   v e c 2 2   =   i o _ v e c 2 . v 3 ; 
 
 
 
                         d o u b l e   v e c 3 0   =   v e c 1 1   *   v e c 2 2   -   v e c 1 2   *   v e c 2 1 ; 
 
                         d o u b l e   v e c 3 1   =   v e c 1 2   *   v e c 2 0   -   v e c 1 0   *   v e c 2 2 ; 
 
                         d o u b l e   v e c 3 2   =   v e c 1 0   *   v e c 2 1   -   v e c 1 1   *   v e c 2 0 ; 
 
                         w   =   M a t h . S q r t ( v e c 3 0   *   v e c 3 0   +   v e c 3 1   *   v e c 3 1   +   v e c 3 2   *   v e c 3 2 ) ; 
 
                         i f   ( w   = =   0 . 0 ) 
 
                         { 
 
                                 r e t u r n   f a l s e ; 
 
                         } 
 
                         v e c 3 0   / =   w ; 
 
                         v e c 3 1   / =   w ; 
 
                         v e c 3 2   / =   w ; 
 
 
 
                         d o u b l e   c b   =   v e c 1 0   *   v e c 2 0   +   v e c 1 1   *   v e c 2 1   +   v e c 1 2   *   v e c 2 2 ; 
 
                         i f   ( c b   <   0 ) 
 
                         { 
 
                                 c b   =   - c b ; / / c b   * =   - 1 . 0 ; 	 	 	 
 
                         } 
 
                         d o u b l e   c a   =   ( M a t h . S q r t ( c b   +   1 . 0 )   +   M a t h . S q r t ( 1 . 0   -   c b ) )   *   0 . 5 ; 
 
 
 
                         i f   ( v e c 3 1   *   v e c 1 0   -   v e c 1 1   *   v e c 3 0   ! =   0 . 0 ) 
 
                         { 
 
                                 f   =   0 ; 
 
                         } 
 
                         e l s e 
 
                         { 
 
                                 i f   ( v e c 3 2   *   v e c 1 0   -   v e c 1 2   *   v e c 3 0   ! =   0 . 0 ) 
 
                                 { 
 
                                         w   =   v e c 1 1 ;   v e c 1 1   =   v e c 1 2 ;   v e c 1 2   =   w ; 
 
                                         w   =   v e c 3 1 ;   v e c 3 1   =   v e c 3 2 ;   v e c 3 2   =   w ; 
 
                                         f   =   1 ; 
 
                                 } 
 
                                 e l s e 
 
                                 { 
 
                                         w   =   v e c 1 0 ;   v e c 1 0   =   v e c 1 2 ;   v e c 1 2   =   w ; 
 
                                         w   =   v e c 3 0 ;   v e c 3 0   =   v e c 3 2 ;   v e c 3 2   =   w ; 
 
                                         f   =   2 ; 
 
                                 } 
 
                         } 
 
                         i f   ( v e c 3 1   *   v e c 1 0   -   v e c 1 1   *   v e c 3 0   = =   0 . 0 ) 
 
                         { 
 
                                 r e t u r n   f a l s e ; 
 
                         } 
 
 
 
                         d o u b l e   k 1 ,   k 2 ,   k 3 ,   k 4 ; 
 
                         d o u b l e   a ,   b ,   c ,   d ; 
 
                         d o u b l e   p 1 ,   q 1 ,   r 1 ; 
 
                         d o u b l e   p 2 ,   q 2 ,   r 2 ; 
 
                         d o u b l e   p 3 ,   q 3 ,   r 3 ; 
 
                         d o u b l e   p 4 ,   q 4 ,   r 4 ; 
 
 
 
 
 
                         k 1   =   ( v e c 1 1   *   v e c 3 2   -   v e c 3 1   *   v e c 1 2 )   /   ( v e c 3 1   *   v e c 1 0   -   v e c 1 1   *   v e c 3 0 ) ; 
 
                         k 2   =   ( v e c 3 1   *   c a )   /   ( v e c 3 1   *   v e c 1 0   -   v e c 1 1   *   v e c 3 0 ) ; 
 
                         k 3   =   ( v e c 1 0   *   v e c 3 2   -   v e c 3 0   *   v e c 1 2 )   /   ( v e c 3 0   *   v e c 1 1   -   v e c 1 0   *   v e c 3 1 ) ; 
 
                         k 4   =   ( v e c 3 0   *   c a )   /   ( v e c 3 0   *   v e c 1 1   -   v e c 1 0   *   v e c 3 1 ) ; 
 
 
 
                         a   =   k 1   *   k 1   +   k 3   *   k 3   +   1 ; 
 
                         b   =   k 1   *   k 2   +   k 3   *   k 4 ; 
 
                         c   =   k 2   *   k 2   +   k 4   *   k 4   -   1 ; 
 
 
 
                         d   =   b   *   b   -   a   *   c ; 
 
                         i f   ( d   <   0 ) 
 
                         { 
 
                                 r e t u r n   f a l s e ; 
 
                         } 
 
                         r 1   =   ( - b   +   M a t h . S q r t ( d ) )   /   a ; 
 
                         p 1   =   k 1   *   r 1   +   k 2 ; 
 
                         q 1   =   k 3   *   r 1   +   k 4 ; 
 
                         r 2   =   ( - b   -   M a t h . S q r t ( d ) )   /   a ; 
 
                         p 2   =   k 1   *   r 2   +   k 2 ; 
 
                         q 2   =   k 3   *   r 2   +   k 4 ; 
 
                         i f   ( f   = =   1 ) 
 
                         { 
 
                                 w   =   q 1 ;   q 1   =   r 1 ;   r 1   =   w ; 
 
                                 w   =   q 2 ;   q 2   =   r 2 ;   r 2   =   w ; 
 
                                 w   =   v e c 1 1 ;   v e c 1 1   =   v e c 1 2 ;   v e c 1 2   =   w ; 
 
                                 w   =   v e c 3 1 ;   v e c 3 1   =   v e c 3 2 ;   v e c 3 2   =   w ; 
 
                                 f   =   0 ; 
 
                         } 
 
                         i f   ( f   = =   2 ) 
 
                         { 
 
                                 w   =   p 1 ;   p 1   =   r 1 ;   r 1   =   w ; 
 
                                 w   =   p 2 ;   p 2   =   r 2 ;   r 2   =   w ; 
 
                                 w   =   v e c 1 0 ;   v e c 1 0   =   v e c 1 2 ;   v e c 1 2   =   w ; 
 
                                 w   =   v e c 3 0 ;   v e c 3 0   =   v e c 3 2 ;   v e c 3 2   =   w ; 
 
                                 f   =   0 ; 
 
                         } 
 
 
 
                         i f   ( v e c 3 1   *   v e c 2 0   -   v e c 2 1   *   v e c 3 0   ! =   0 . 0 ) 
 
                         { 
 
                                 f   =   0 ; 
 
                         } 
 
                         e l s e 
 
                         { 
 
                                 i f   ( v e c 3 2   *   v e c 2 0   -   v e c 2 2   *   v e c 3 0   ! =   0 . 0 ) 
 
                                 { 
 
                                         w   =   v e c 2 1 ;   v e c 2 1   =   v e c 2 2 ;   v e c 2 2   =   w ; 
 
                                         w   =   v e c 3 1 ;   v e c 3 1   =   v e c 3 2 ;   v e c 3 2   =   w ; 
 
                                         f   =   1 ; 
 
                                 } 
 
                                 e l s e 
 
                                 { 
 
                                         w   =   v e c 2 0 ;   v e c 2 0   =   v e c 2 2 ;   v e c 2 2   =   w ; 
 
                                         w   =   v e c 3 0 ;   v e c 3 0   =   v e c 3 2 ;   v e c 3 2   =   w ; 
 
                                         f   =   2 ; 
 
                                 } 
 
                         } 
 
                         i f   ( v e c 3 1   *   v e c 2 0   -   v e c 2 1   *   v e c 3 0   = =   0 . 0 ) 
 
                         { 
 
                                 r e t u r n   f a l s e ; 
 
                         } 
 
                         k 1   =   ( v e c 2 1   *   v e c 3 2   -   v e c 3 1   *   v e c 2 2 )   /   ( v e c 3 1   *   v e c 2 0   -   v e c 2 1   *   v e c 3 0 ) ; 
 
                         k 2   =   ( v e c 3 1   *   c a )   /   ( v e c 3 1   *   v e c 2 0   -   v e c 2 1   *   v e c 3 0 ) ; 
 
                         k 3   =   ( v e c 2 0   *   v e c 3 2   -   v e c 3 0   *   v e c 2 2 )   /   ( v e c 3 0   *   v e c 2 1   -   v e c 2 0   *   v e c 3 1 ) ; 
 
                         k 4   =   ( v e c 3 0   *   c a )   /   ( v e c 3 0   *   v e c 2 1   -   v e c 2 0   *   v e c 3 1 ) ; 
 
 
 
                         a   =   k 1   *   k 1   +   k 3   *   k 3   +   1 ; 
 
                         b   =   k 1   *   k 2   +   k 3   *   k 4 ; 
 
                         c   =   k 2   *   k 2   +   k 4   *   k 4   -   1 ; 
 
 
 
                         d   =   b   *   b   -   a   *   c ; 
 
                         i f   ( d   <   0 ) 
 
                         { 
 
                                 r e t u r n   f a l s e ; 
 
                         } 
 
                         r 3   =   ( - b   +   M a t h . S q r t ( d ) )   /   a ; 
 
                         p 3   =   k 1   *   r 3   +   k 2 ; 
 
                         q 3   =   k 3   *   r 3   +   k 4 ; 
 
                         r 4   =   ( - b   -   M a t h . S q r t ( d ) )   /   a ; 
 
                         p 4   =   k 1   *   r 4   +   k 2 ; 
 
                         q 4   =   k 3   *   r 4   +   k 4 ; 
 
                         i f   ( f   = =   1 ) 
 
                         { 
 
                                 w   =   q 3 ;   q 3   =   r 3 ;   r 3   =   w ; 
 
                                 w   =   q 4 ;   q 4   =   r 4 ;   r 4   =   w ; 
 
                                 w   =   v e c 2 1 ;   v e c 2 1   =   v e c 2 2 ;   v e c 2 2   =   w ; 
 
                                 w   =   v e c 3 1 ;   v e c 3 1   =   v e c 3 2 ;   v e c 3 2   =   w ; 
 
                                 f   =   0 ; 
 
                         } 
 
                         i f   ( f   = =   2 ) 
 
                         { 
 
                                 w   =   p 3 ;   p 3   =   r 3 ;   r 3   =   w ; 
 
                                 w   =   p 4 ;   p 4   =   r 4 ;   r 4   =   w ; 
 
                                 w   =   v e c 2 0 ;   v e c 2 0   =   v e c 2 2 ;   v e c 2 2   =   w ; 
 
                                 w   =   v e c 3 0 ;   v e c 3 0   =   v e c 3 2 ;   v e c 3 2   =   w ; 
 
                                 f   =   0 ; 
 
                         } 
 
 
 
                         d o u b l e   e 1   =   p 1   *   p 3   +   q 1   *   q 3   +   r 1   *   r 3 ; 
 
                         i f   ( e 1   <   0 ) 
 
                         { 
 
                                 e 1   =   - e 1 ; 
 
                         } 
 
                         d o u b l e   e 2   =   p 1   *   p 4   +   q 1   *   q 4   +   r 1   *   r 4 ; 
 
                         i f   ( e 2   <   0 ) 
 
                         { 
 
                                 e 2   =   - e 2 ; 
 
                         } 
 
                         d o u b l e   e 3   =   p 2   *   p 3   +   q 2   *   q 3   +   r 2   *   r 3 ; 
 
                         i f   ( e 3   <   0 ) 
 
                         { 
 
                                 e 3   =   - e 3 ; 
 
                         } 
 
                         d o u b l e   e 4   =   p 2   *   p 4   +   q 2   *   q 4   +   r 2   *   r 4 ; 
 
                         i f   ( e 4   <   0 ) 
 
                         { 
 
                                 e 4   =   - e 4 ; 
 
                         } 
 
                         i f   ( e 1   <   e 2 ) 
 
                         { 
 
                                 i f   ( e 1   <   e 3 ) 
 
                                 { 
 
                                         i f   ( e 1   <   e 4 ) 
 
                                         { 
 
                                                 i o _ v e c 1 . v 1   =   p 1 ; 
 
                                                 i o _ v e c 1 . v 2   =   q 1 ; 
 
                                                 i o _ v e c 1 . v 3   =   r 1 ; 
 
                                                 i o _ v e c 2 . v 1   =   p 3 ; 
 
                                                 i o _ v e c 2 . v 2   =   q 3 ; 
 
                                                 i o _ v e c 2 . v 3   =   r 3 ; 
 
                                         } 
 
                                         e l s e 
 
                                         { 
 
                                                 i o _ v e c 1 . v 1   =   p 2 ; 
 
                                                 i o _ v e c 1 . v 2   =   q 2 ; 
 
                                                 i o _ v e c 1 . v 3   =   r 2 ; 
 
                                                 i o _ v e c 2 . v 1   =   p 4 ; 
 
                                                 i o _ v e c 2 . v 2   =   q 4 ; 
 
                                                 i o _ v e c 2 . v 3   =   r 4 ; 
 
                                         } 
 
                                 } 
 
                                 e l s e 
 
                                 { 
 
                                         i f   ( e 3   <   e 4 ) 
 
                                         { 
 
                                                 i o _ v e c 1 . v 1   =   p 2 ; 
 
                                                 i o _ v e c 1 . v 2   =   q 2 ; 
 
                                                 i o _ v e c 1 . v 3   =   r 2 ; 
 
                                                 i o _ v e c 2 . v 1   =   p 3 ; 
 
                                                 i o _ v e c 2 . v 2   =   q 3 ; 
 
                                                 i o _ v e c 2 . v 3   =   r 3 ; 
 
                                         } 
 
                                         e l s e 
 
                                         { 
 
                                                 i o _ v e c 1 . v 1   =   p 2 ; 
 
                                                 i o _ v e c 1 . v 2   =   q 2 ; 
 
                                                 i o _ v e c 1 . v 3   =   r 2 ; 
 
                                                 i o _ v e c 2 . v 1   =   p 4 ; 
 
                                                 i o _ v e c 2 . v 2   =   q 4 ; 
 
                                                 i o _ v e c 2 . v 3   =   r 4 ; 
 
                                         } 
 
                                 } 
 
                         } 
 
                         e l s e 
 
                         { 
 
                                 i f   ( e 2   <   e 3 ) 
 
                                 { 
 
                                         i f   ( e 2   <   e 4 ) 
 
                                         { 
 
                                                 i o _ v e c 1 . v 1   =   p 1 ; 
 
                                                 i o _ v e c 1 . v 2   =   q 1 ; 
 
                                                 i o _ v e c 1 . v 3   =   r 1 ; 
 
                                                 i o _ v e c 2 . v 1   =   p 4 ; 
 
                                                 i o _ v e c 2 . v 2   =   q 4 ; 
 
                                                 i o _ v e c 2 . v 3   =   r 4 ; 
 
                                         } 
 
                                         e l s e 
 
                                         { 
 
                                                 i o _ v e c 1 . v 1   =   p 2 ; 
 
                                                 i o _ v e c 1 . v 2   =   q 2 ; 
 
                                                 i o _ v e c 1 . v 3   =   r 2 ; 
 
                                                 i o _ v e c 2 . v 1   =   p 4 ; 
 
                                                 i o _ v e c 2 . v 2   =   q 4 ; 
 
                                                 i o _ v e c 2 . v 3   =   r 4 ; 
 
                                         } 
 
                                 } 
 
                                 e l s e 
 
                                 { 
 
                                         i f   ( e 3   <   e 4 ) 
 
                                         { 
 
                                                 i o _ v e c 1 . v 1   =   p 2 ; 
 
                                                 i o _ v e c 1 . v 2   =   q 2 ; 
 
                                                 i o _ v e c 1 . v 3   =   r 2 ; 
 
                                                 i o _ v e c 2 . v 1   =   p 3 ; 
 
                                                 i o _ v e c 2 . v 2   =   q 3 ; 
 
                                                 i o _ v e c 2 . v 3   =   r 3 ; 
 
                                         } 
 
                                         e l s e 
 
                                         { 
 
                                                 i o _ v e c 1 . v 1   =   p 2 ; 
 
                                                 i o _ v e c 1 . v 2   =   q 2 ; 
 
                                                 i o _ v e c 1 . v 3   =   r 2 ; 
 
                                                 i o _ v e c 2 . v 1   =   p 4 ; 
 
                                                 i o _ v e c 2 . v 2   =   q 4 ; 
 
                                                 i o _ v e c 2 . v 3   =   r 4 ; 
 
                                         } 
 
                                 } 
 
                         } 
 
                         r e t u r n   t r u e ; 
 
                 } 
 
         } 
 
 } 