/ *   
 
   *   P R O J E C T :   N y A R T o o l k i t C S ( E x t e n s i o n ) 
 
   *   - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 
 
   *   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03 x 3 LR0<h
}W0~0Y00
 
           * / 
 
         p u b l i c   c l a s s   N y A R D o u b l e M a t r i x 3 3   :   I N y A R D o u b l e M a t r i x 
 
         { 
 
                 / * *   LRn0 }$Pg0Y00* / 
 
                 p u b l i c   d o u b l e   m 0 0 ; 
 
                 / * *   LRn0 }$Pg0Y00* / 
 
                 p u b l i c   d o u b l e   m 0 1 ; 
 
                 / * *   LRn0 }$Pg0Y00* / 
 
                 p u b l i c   d o u b l e   m 0 2 ; 
 
                 / * *   LRn0 }$Pg0Y00* / 
 
                 p u b l i c   d o u b l e   m 1 0 ; 
 
                 / * *   LRn0 }$Pg0Y00* / 
 
                 p u b l i c   d o u b l e   m 1 1 ; 
 
                 / * *   LRn0 }$Pg0Y00* / 
 
                 p u b l i c   d o u b l e   m 1 2 ; 
 
                 / * *   LRn0 }$Pg0Y00* / 
 
                 p u b l i c   d o u b l e   m 2 0 ; 
 
                 / * *   LRn0 }$Pg0Y00* / 
 
                 p u b l i c   d o u b l e   m 2 1 ; 
 
                 / * *   LRn0 }$Pg0Y00* / 
 
                 p u b l i c   d o u b l e   m 2 2 ; 
 
                 / * * 
 
                   *   S0n0peo00000000n0MR0ubW0f0ԏW0~0Y00
 
                   *   @ p a r a m   i _ n u m b e r 
 
                   *   MRn0wU0
 
                   *   @ r e t u r n 
 
                   *   eW0D0000000MR
 
                   * / 
 
                 p u b l i c   s t a t i c   N y A R D o u b l e M a t r i x 3 3 [ ]   c r e a t e A r r a y ( i n t   i _ n u m b e r ) 
 
                 { 
 
                         N y A R D o u b l e M a t r i x 3 3 [ ]   r e t   =   n e w   N y A R D o u b l e M a t r i x 3 3 [ i _ n u m b e r ] ; 
 
                         f o r   ( i n t   i   =   0 ;   i   <   i _ n u m b e r ;   i + + ) 
 
                         { 
 
                                 r e t [ i ]   =   n e w   N y A R D o u b l e M a t r i x 3 3 ( ) ; 
 
                         } 
 
                         r e t u r n   r e t ; 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo00 }pe9 n0MR00LRk0000W0~0Y00
 
                   * / 
 
                 p u b l i c   v o i d   s e t V a l u e ( d o u b l e [ ]   i _ v a l u e ) 
 
                 { 
 
                         t h i s . m 0 0   =   i _ v a l u e [ 0 ] ; 
 
                         t h i s . m 0 1   =   i _ v a l u e [ 1 ] ; 
 
                         t h i s . m 0 2   =   i _ v a l u e [ 2 ] ; 
 
                         t h i s . m 1 0   =   i _ v a l u e [ 3 ] ; 
 
                         t h i s . m 1 1   =   i _ v a l u e [ 4 ] ; 
 
                         t h i s . m 1 2   =   i _ v a l u e [ 5 ] ; 
 
                         t h i s . m 2 0   =   i _ v a l u e [ 6 ] ; 
 
                         t h i s . m 2 1   =   i _ v a l u e [ 7 ] ; 
 
                         t h i s . m 2 2   =   i _ v a l u e [ 8 ] ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo00000000n0Q[0000000k0000W0~0Y00
 
                   *   @ p a r a m   i _ v a l u e 
 
                   *   000CQn0000000
 
                   * / 
 
                 p u b l i c   v o i d   s e t V a l u e ( N y A R D o u b l e M a t r i x 3 3   i _ v a l u e ) 
 
                 { 
 
                         t h i s . m 0 0   =   i _ v a l u e . m 0 0 ; 
 
                         t h i s . m 0 1   =   i _ v a l u e . m 0 1 ; 
 
                         t h i s . m 0 2   =   i _ v a l u e . m 0 2 ; 
 
                         t h i s . m 1 0   =   i _ v a l u e . m 1 0 ; 
 
                         t h i s . m 1 1   =   i _ v a l u e . m 1 1 ; 
 
                         t h i s . m 1 2   =   i _ v a l u e . m 1 2 ; 
 
                         t h i s . m 2 0   =   i _ v a l u e . m 2 0 ; 
 
                         t h i s . m 2 1   =   i _ v a l u e . m 2 1 ; 
 
                         t h i s . m 2 2   =   i _ v a l u e . m 2 2 ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo00 }pe9 n0MRk00LRn0Q[0000W0~0Y00
 
                   * / 
 
                 p u b l i c   v o i d   g e t V a l u e ( d o u b l e [ ]   o _ v a l u e ) 
 
                 { 
 
                         o _ v a l u e [ 0 ]   =   t h i s . m 0 0 ; 
 
                         o _ v a l u e [ 1 ]   =   t h i s . m 0 1 ; 
 
                         o _ v a l u e [ 2 ]   =   t h i s . m 0 2 ; 
 
                         o _ v a l u e [ 3 ]   =   t h i s . m 1 0 ; 
 
                         o _ v a l u e [ 4 ]   =   t h i s . m 1 1 ; 
 
                         o _ v a l u e [ 5 ]   =   t h i s . m 1 2 ; 
 
                         o _ v a l u e [ 6 ]   =   t h i s . m 2 0 ; 
 
                         o _ v a l u e [ 7 ]   =   t h i s . m 2 1 ; 
 
                         o _ v a l u e [ 8 ]   =   t h i s . m 2 2 ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo00LR0{W0f00000000k0000W0~0Y00
 
                   *   @ p a r a m   i _ s r c 
 
                   *   LR0{Y000000000t h i s 0c[g0M0~0Y00
 
                   *   @ r e t u r n 
 
                   *   LR0_000h00t r u e 0ԏW0~0Y00
 
                   * / 
 
                 p u b l i c   b o o l   i n v e r s e ( N y A R D o u b l e M a t r i x 3 3   i _ s r c ) 
 
                 { 
 
                         d o u b l e   a 1 1 ,   a 1 2 ,   a 1 3 ,   a 2 1 ,   a 2 2 ,   a 2 3 ,   a 3 1 ,   a 3 2 ,   a 3 3 ; 
 
                         d o u b l e   b 1 1 ,   b 1 2 ,   b 1 3 ,   b 2 1 ,   b 2 2 ,   b 2 3 ,   b 3 1 ,   b 3 2 ,   b 3 3 ; 
 
                         a 1 1   =   i _ s r c . m 0 0 ;   a 1 2   =   i _ s r c . m 0 1 ;   a 1 3   =   i _ s r c . m 0 2 ; 
 
                         a 2 1   =   i _ s r c . m 1 0 ;   a 2 2   =   i _ s r c . m 1 1 ;   a 2 3   =   i _ s r c . m 1 2 ; 
 
                         a 3 1   =   i _ s r c . m 2 0 ;   a 3 2   =   i _ s r c . m 2 1 ;   a 3 3   =   i _ s r c . m 2 2 ; 
 
 
 
                         b 1 1   =   a 2 2   *   a 3 3   -   a 2 3   *   a 3 2 ; 
 
                         b 1 2   =   a 3 2   *   a 1 3   -   a 3 3   *   a 1 2 ; 
 
                         b 1 3   =   a 1 2   *   a 2 3   -   a 1 3   *   a 2 2 ; 
 
 
 
                         b 2 1   =   a 2 3   *   a 3 1   -   a 2 1   *   a 3 3 ; 
 
                         b 2 2   =   a 3 3   *   a 1 1   -   a 3 1   *   a 1 3 ; 
 
                         b 2 3   =   a 1 3   *   a 2 1   -   a 1 1   *   a 2 3 ; 
 
 
 
                         b 3 1   =   a 2 1   *   a 3 2   -   a 2 2   *   a 3 1 ; 
 
                         b 3 2   =   a 3 1   *   a 1 2   -   a 3 2   *   a 1 1 ; 
 
                         b 3 3   =   a 1 1   *   a 2 2   -   a 1 2   *   a 2 1 ; 
 
 
 
                         d o u b l e   d e t _ 1   =   a 1 1   *   b 1 1   +   a 2 1   *   b 1 2   +   a 3 1   *   b 1 3 ; 
 
                         i f   ( d e t _ 1   = =   0 ) 
 
                         { 
 
                                 r e t u r n   f a l s e ; 
 
                         } 
 
                         d e t _ 1   =   1   /   d e t _ 1 ; 
 
 
 
                         t h i s . m 0 0   =   b 1 1   *   d e t _ 1 ; 
 
                         t h i s . m 0 1   =   b 1 2   *   d e t _ 1 ; 
 
                         t h i s . m 0 2   =   b 1 3   *   d e t _ 1 ; 
 
 
 
                         t h i s . m 1 0   =   b 2 1   *   d e t _ 1 ; 
 
                         t h i s . m 1 1   =   b 2 2   *   d e t _ 1 ; 
 
                         t h i s . m 1 2   =   b 2 3   *   d e t _ 1 ; 
 
 
 
                         t h i s . m 2 0   =   b 3 1   *   d e t _ 1 ; 
 
                         t h i s . m 2 1   =   b 3 2   *   d e t _ 1 ; 
 
                         t h i s . m 2 2   =   b 3 3   *   d e t _ 1 ; 
 
 
 
                         r e t u r n   t r u e ; 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo00LR0VLRh0W0f00Z X Y |n0҉^$P0{W0~0Y00
 
                   *   @ p a r a m   o _ o u t 
 
                   *  0҉^$P0SQ0S0000000g0Y00
 
                   *   ҉^$Pn0{Vo000 - P I g0Y00
 
                   * / 
 
                 p u b l i c   v o i d   g e t Z X Y A n g l e ( N y A R D o u b l e P o i n t 3 d   o _ o u t ) 
 
                 { 
 
                         d o u b l e   s i n a   =   t h i s . m 2 1 ; 
 
                         i f   ( s i n a   > =   1 . 0 ) 
 
                         { 
 
                                 o _ o u t . x   =   M a t h . P I   /   2 ; 
 
                                 o _ o u t . y   =   0 ; 
 
                                 o _ o u t . z   =   M a t h . A t a n 2 ( - t h i s . m 1 0 ,   t h i s . m 0 0 ) ; 
 
                         } 
 
                         e l s e   i f   ( s i n a   < =   - 1 . 0 ) 
 
                         { 
 
                                 o _ o u t . x   =   - M a t h . P I   /   2 ; 
 
                                 o _ o u t . y   =   0 ; 
 
                                 o _ o u t . z   =   M a t h . A t a n 2 ( - t h i s . m 1 0 ,   t h i s . m 0 0 ) ; 
 
                         } 
 
                         e l s e 
 
                         { 
 
                                 o _ o u t . x   =   M a t h . A s i n ( s i n a ) ; 
 
                                 o _ o u t . z   =   M a t h . A t a n 2 ( - t h i s . m 0 1 ,   t h i s . m 1 1 ) ; 
 
                                 o _ o u t . y   =   M a t h . A t a n 2 ( - t h i s . m 2 0 ,   t h i s . m 2 2 ) ; 
 
                         } 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo00LR0VLRh0W0f00Z X Y |n0҉^$P0000W0~0Y00
 
                   *   @ p a r a m   i _ a n g l e 
 
                   *  0000Y00҉^$Pg0Y00
 
                   * / 
 
                 p u b l i c   v o i d   s e t Z X Y A n g l e ( N y A R D o u b l e P o i n t 3 d   i _ a n g l e ) 
 
                 { 
 
                         s e t Z X Y A n g l e ( i _ a n g l e . x ,   i _ a n g l e . y ,   i _ a n g l e . z ) ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo00LR0VLRh0W0f00Z X Y |n0҉^$P0000W0~0Y00
 
                   *   @ p a r a m   i _ x 
 
                   *  0X n0҉^$P
 
                   *   @ p a r a m   i _ y 
 
                   *  0X n0҉^$P
 
                   *   @ p a r a m   i _ z 
 
                   *  0X n0҉^$P
 
                   * / 
 
                 p u b l i c   v o i d   s e t Z X Y 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 ) ; 
 
                         t h i s . m 0 0   =   c o s c   *   c o s b   -   s i n c   *   s i n a   *   s i n b ; 
 
                         t h i s . m 0 1   =   - s i n c   *   c o s a ; 
 
                         t h i s . m 0 2   =   c o s c   *   s i n b   +   s i n c   *   s i n a   *   c o s b ; 
 
                         t h i s . m 1 0   =   s i n c   *   c o s b   +   c o s c   *   s i n a   *   s i n b ; 
 
                         t h i s . m 1 1   =   c o s c   *   c o s a ; 
 
                         t h i s . m 1 2   =   s i n c   *   s i n b   -   c o s c   *   s i n a   *   c o s b ; 
 
                         t h i s . m 2 0   =   - c o s a   *   s i n b ; 
 
                         t h i s . m 2 1   =   s i n a ; 
 
                         t h i s . m 2 2   =   c o s b   *   c o s a ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo00000000k0XSMOLR0000W0~0Y00
 
                   * / 
 
                 p u b l i c   v o i d   l o a d I d e n t i t y ( ) 
 
                 { 
 
                         t h i s . m 0 0   =   t h i s . m 1 1   =   t h i s . m 2 2   =   1 ; 
 
                         t h i s . m 0 1   =   t h i s . m 0 2   = 
 
                         t h i s . m 1 0   =   t h i s . m 1 2   = 
 
                         t h i s . m 2 0   =   t h i s . m 2 1   =   0 ; 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo003 !kCQ^j0^j	YcW0~0Y00
 
                   *   @ p a r a m   i _ p o s i t i o n 
 
                   *   	YcY00	N!kCQ^j
 
                   *   @ p a r a m   o _ o u t 
 
                   *   	Yc_n0^j0SQ0S0000000
 
                   * / 
 
                 p u b l i c   v o i d   t r a n s f o r m V e r t e x ( N y A R D o u b l e P o i n t 3 d   i _ p o s i t i o n ,   N y A R D o u b l e P o i n t 3 d   o _ o u t ) 
 
                 { 
 
                         t r a n s f o r m V e r t e x ( i _ p o s i t i o n . x ,   i _ p o s i t i o n . y ,   i _ p o s i t i o n . z ,   o _ o u t ) ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo003 !kCQ^j0^j	YcW0~0Y00
 
                   *   @ p a r a m   i _ x 
 
                   *   	YcY00	N!kCQ^j( X ) 
 
                   *   @ p a r a m   i _ y 
 
                   *   	YcY00	N!kCQ^j( Y ) 
 
                   *   @ p a r a m   i _ z 
 
                   *   	YcY00	N!kCQ^j( Z ) 
 
                   *   @ p a r a m   o _ o u t 
 
                   *   	Yc_n0^j0SQ0S0000000
 
                   * / 
 
                 p u b l i c   v o i d   t r a n s f o r m V e r t e x ( d o u b l e   i _ x ,   d o u b l e   i _ y ,   d o u b l e   i _ z ,   N y A R D o u b l e P o i n t 3 d   o _ o u t ) 
 
                 { 
 
                         o _ o u t . x   =   t h i s . m 0 0   *   i _ x   +   t h i s . m 0 1   *   i _ y   +   t h i s . m 0 2   *   i _ z ; 
 
                         o _ o u t . y   =   t h i s . m 1 0   *   i _ x   +   t h i s . m 1 1   *   i _ y   +   t h i s . m 1 2   *   i _ z ; 
 
                         o _ o u t . z   =   t h i s . m 2 0   *   i _ x   +   t h i s . m 2 1   *   i _ y   +   t h i s . m 2 2   *   i _ z ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo00LRTXn0cQ0{0W0f00000000k0<h
}W0~0Y00
 
                   *  0i _ m a t _ l h0i _ m a t _ r k0o00t h i s 0c[W0j0D0g0O0`0U0D00
 
                   *   @ p a r a m   i _ m a t _ l 
 
                   *   ]bRn0LR
 
                   *   @ p a r a m   i _ m a t _ r 
 
                   *   SbRn0LR
 
                   * / 
 
                 p u b l i c   v o i d   m u l ( N y A R D o u b l e M a t r i x 3 3   i _ m a t _ l ,   N y A R D o u b l e M a t r i x 3 3   i _ m a t _ r ) 
 
                 { 
 
                         / / a s s e r t ( t h i s ! = i _ m a t _ l ) ; 
 
                         / / a s s e r t ( t h i s ! = i _ m a t _ r ) ; 
 
                         t h i s . m 0 0   =   i _ m a t _ l . m 0 0   *   i _ m a t _ r . m 0 0   +   i _ m a t _ l . m 0 1   *   i _ m a t _ r . m 1 0   +   i _ m a t _ l . m 0 2   *   i _ m a t _ r . m 2 0 ; 
 
                         t h i s . m 0 1   =   i _ m a t _ l . m 0 0   *   i _ m a t _ r . m 0 1   +   i _ m a t _ l . m 0 1   *   i _ m a t _ r . m 1 1   +   i _ m a t _ l . m 0 2   *   i _ m a t _ r . m 2 1 ; 
 
                         t h i s . m 0 2   =   i _ m a t _ l . m 0 0   *   i _ m a t _ r . m 0 2   +   i _ m a t _ l . m 0 1   *   i _ m a t _ r . m 1 2   +   i _ m a t _ l . m 0 2   *   i _ m a t _ r . m 2 2 ; 
 
 
 
                         t h i s . m 1 0   =   i _ m a t _ l . m 1 0   *   i _ m a t _ r . m 0 0   +   i _ m a t _ l . m 1 1   *   i _ m a t _ r . m 1 0   +   i _ m a t _ l . m 1 2   *   i _ m a t _ r . m 2 0 ; 
 
                         t h i s . m 1 1   =   i _ m a t _ l . m 1 0   *   i _ m a t _ r . m 0 1   +   i _ m a t _ l . m 1 1   *   i _ m a t _ r . m 1 1   +   i _ m a t _ l . m 1 2   *   i _ m a t _ r . m 2 1 ; 
 
                         t h i s . m 1 2   =   i _ m a t _ l . m 1 0   *   i _ m a t _ r . m 0 2   +   i _ m a t _ l . m 1 1   *   i _ m a t _ r . m 1 2   +   i _ m a t _ l . m 1 2   *   i _ m a t _ r . m 2 2 ; 
 
 
 
                         t h i s . m 2 0   =   i _ m a t _ l . m 2 0   *   i _ m a t _ r . m 0 0   +   i _ m a t _ l . m 2 1   *   i _ m a t _ r . m 1 0   +   i _ m a t _ l . m 2 2   *   i _ m a t _ r . m 2 0 ; 
 
                         t h i s . m 2 1   =   i _ m a t _ l . m 2 0   *   i _ m a t _ r . m 0 1   +   i _ m a t _ l . m 2 1   *   i _ m a t _ r . m 1 1   +   i _ m a t _ l . m 2 2   *   i _ m a t _ r . m 2 1 ; 
 
                         t h i s . m 2 2   =   i _ m a t _ l . m 2 0   *   i _ m a t _ r . m 0 2   +   i _ m a t _ l . m 2 1   *   i _ m a t _ r . m 1 2   +   i _ m a t _ l . m 2 2   *   i _ m a t _ r . m 2 2 ; 
 
 
 
                         r e t u r n ; 
 
                 } 
 
         } 
 
 } 