/ *   
 
   *   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ؚSW0_0A R T o o l K i t 1uegn0YRLR giSQt0[ňW0~0Y0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 O p t i m i z e _ O 2   :   I N y A R R o t M a t r i x O p t i m i z e 
 
         { 
 
                 p r i v a t e   r e a d o n l y   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   _ p r o j e c t i o n _ m a t _ r e f ; 
 
                 / * * 
 
                   *   0000000g0Y00
 
                   *   \q_	Yc000000n0Sgq$P0-[W0f000000000ubW0~0Y00
 
                   *   @ p a r a m   i _ p r o j e c t i o n _ m a t _ r e f 
 
                   *   \q_	Yc000000n0Sgq$P0
 
                   * / 
 
                 p u b l i c   N y A R R o t M a t r i x O p t i m i z e _ O 2 ( 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 _ p r o j e c t i o n _ m a t _ r e f ) 
 
                 { 
 
                         t h i s . _ p r o j e c t i o n _ m a t _ r e f   =   i _ p r o j e c t i o n _ m a t _ r e f ; 
 
                         r e t u r n ; 
 
                 } 
 
                 p r i v a t e   r e a d o n l y   d o u b l e [ ] [ ]   _ _ m o d i f y M a t r i x _ d o u b l e 1 D   =   A r r a y U t i l s . n e w D o u b l e 2 d A r r a y ( 8 ,   3 ) ; 
 
                 / * * 
 
                   *   S0n0peo00VLR0 giSW0~0Y00
 
                   *   A R T o o l K i t n0a r G e t R o t k0vS_W0~0Y00
 
                   * / 
 
                 p u b l i c   d o u b l e   m o d i f y M a t r i x ( N y A R R o t M a t r i x _ A R T o o l K i t   i o _ r o t ,   N y A R D o u b l e P o i n t 3 d   t r a n s ,   N y A R D o u b l e P o i n t 3 d [ ]   i _ v e r t e x 3 d ,   N y A R D o u b l e P o i n t 2 d [ ]   i _ v e r t e x 2 d ) 
 
                 { 
 
                         d o u b l e   f a c t o r ; 
 
                         d o u b l e   a 2 ,   b 2 ,   c 2 ; 
 
                         d o u b l e   h ,   x ,   y ; 
 
                         d o u b l e   e r r ,   m i n e r r   =   0 ; 
 
                         i n t   t 1 ,   t 2 ,   t 3 ; 
 
                         i n t   b e s t _ i d x   =   0 ; 
 
 
 
                         f a c t o r   =   1 0 . 0   *   M a t h . P I   /   1 8 0 . 0 ; 
 
                         d o u b l e   r o t 0 ,   r o t 1 ,   r o t 2 ; 
 
                         d o u b l e   c o m b o 0 0 ,   c o m b o 0 1 ,   c o m b o 0 2 ,   c o m b o 0 3 ,   c o m b o 1 0 ,   c o m b o 1 1 ,   c o m b o 1 2 ,   c o m b o 1 3 ,   c o m b o 2 0 ,   c o m b o 2 1 ,   c o m b o 2 2 ,   c o m b o 2 3 ; 
 
                         d o u b l e   c o m b o 0 2 _ 2 ,   c o m b o 0 2 _ 5 ,   c o m b o 0 2 _ 8 ,   c o m b o 0 2 _ 1 1 ; 
 
                         d o u b l e   c o m b o 2 2 _ 2 ,   c o m b o 2 2 _ 5 ,   c o m b o 2 2 _ 8 ,   c o m b o 2 2 _ 1 1 ; 
 
                         d o u b l e   c o m b o 1 2 _ 2 ,   c o m b o 1 2 _ 5 ,   c o m b o 1 2 _ 8 ,   c o m b o 1 2 _ 1 1 ; 
 
                         / /   v e r t e x U\
 
                         d o u b l e   V X 0 0 ,   V X 0 1 ,   V X 0 2 ,   V X 1 0 ,   V X 1 1 ,   V X 1 2 ,   V X 2 0 ,   V X 2 1 ,   V X 2 2 ,   V X 3 0 ,   V X 3 1 ,   V X 3 2 ; 
 
                         V X 0 0   =   i _ v e r t e x 3 d [ 0 ] . x ; 
 
                         V X 0 1   =   i _ v e r t e x 3 d [ 0 ] . y ; 
 
                         V X 0 2   =   i _ v e r t e x 3 d [ 0 ] . z ; 
 
                         V X 1 0   =   i _ v e r t e x 3 d [ 1 ] . x ; 
 
                         V X 1 1   =   i _ v e r t e x 3 d [ 1 ] . y ; 
 
                         V X 1 2   =   i _ v e r t e x 3 d [ 1 ] . z ; 
 
                         V X 2 0   =   i _ v e r t e x 3 d [ 2 ] . x ; 
 
                         V X 2 1   =   i _ v e r t e x 3 d [ 2 ] . y ; 
 
                         V X 2 2   =   i _ v e r t e x 3 d [ 2 ] . z ; 
 
                         V X 3 0   =   i _ v e r t e x 3 d [ 3 ] . x ; 
 
                         V X 3 1   =   i _ v e r t e x 3 d [ 3 ] . y ; 
 
                         V X 3 2   =   i _ v e r t e x 3 d [ 3 ] . z ; 
 
                         d o u b l e   P 2 D 0 0 ,   P 2 D 0 1 ,   P 2 D 1 0 ,   P 2 D 1 1 ,   P 2 D 2 0 ,   P 2 D 2 1 ,   P 2 D 3 0 ,   P 2 D 3 1 ; 
 
                         P 2 D 0 0   =   i _ v e r t e x 2 d [ 0 ] . x ; 
 
                         P 2 D 0 1   =   i _ v e r t e x 2 d [ 0 ] . y ; 
 
                         P 2 D 1 0   =   i _ v e r t e x 2 d [ 1 ] . x ; 
 
                         P 2 D 1 1   =   i _ v e r t e x 2 d [ 1 ] . y ; 
 
                         P 2 D 2 0   =   i _ v e r t e x 2 d [ 2 ] . x ; 
 
                         P 2 D 2 1   =   i _ v e r t e x 2 d [ 2 ] . y ; 
 
                         P 2 D 3 0   =   i _ v e r t e x 2 d [ 3 ] . x ; 
 
                         P 2 D 3 1   =   i _ v e r t e x 2 d [ 3 ] . y ; 
 
                         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   p r j m a t   =   t h i s . _ p r o j e c t i o n _ m a t _ r e f ; 
 
                         d o u b l e   C P 0   =   p r j m a t . m 0 0 ,   C P 1   =   p r j m a t . m 0 1 ,   C P 2   =   p r j m a t . m 0 2 ,   C P 4   =   p r j m a t . m 1 0 ,   C P 5   =   p r j m a t . m 1 1 ,   C P 6   =   p r j m a t . m 1 2 ,   C P 8   =   p r j m a t . m 2 0 ,   C P 9   =   p r j m a t . m 2 1 ,   C P 1 0   =   p r j m a t . m 2 2 ; 
 
                         c o m b o 0 3   =   C P 0   *   t r a n s . x   +   C P 1   *   t r a n s . y   +   C P 2   *   t r a n s . z   +   p r j m a t . m 0 3 ; 
 
                         c o m b o 1 3   =   C P 4   *   t r a n s . x   +   C P 5   *   t r a n s . y   +   C P 6   *   t r a n s . z   +   p r j m a t . m 1 3 ; 
 
                         c o m b o 2 3   =   C P 8   *   t r a n s . x   +   C P 9   *   t r a n s . y   +   C P 1 0   *   t r a n s . z   +   p r j m a t . m 2 3 ; 
 
                         d o u b l e   C A C A ,   S A S A ,   S A C A ,   C A ,   S A ; 
 
                         d o u b l e   C A C A C B ,   S A C A C B ,   S A S A C B ,   C A S B ,   S A S B ; 
 
                         d o u b l e   S A C A S C ,   S A C A C B S C ,   S A C A C B C C ,   S A C A C C ; 
 
                         d o u b l e [ ] [ ]   d o u b l e 1 D   =   t h i s . _ _ m o d i f y M a t r i x _ d o u b l e 1 D ; 
 
 
 
                         d o u b l e [ ]   a _ f a c t o r   =   d o u b l e 1 D [ 1 ] ; 
 
                         d o u b l e [ ]   s i n b   =   d o u b l e 1 D [ 2 ] ; 
 
                         d o u b l e [ ]   c o s b   =   d o u b l e 1 D [ 3 ] ; 
 
                         d o u b l e [ ]   b _ f a c t o r   =   d o u b l e 1 D [ 4 ] ; 
 
                         d o u b l e [ ]   s i n c   =   d o u b l e 1 D [ 5 ] ; 
 
                         d o u b l e [ ]   c o s c   =   d o u b l e 1 D [ 6 ] ; 
 
                         d o u b l e [ ]   c _ f a c t o r   =   d o u b l e 1 D [ 7 ] ; 
 
                         d o u b l e   w ,   w 2 ; 
 
                         d o u b l e   w s i n ,   w c o s ; 
 
 
 
                         N y A R D o u b l e P o i n t 3 d   a n g l e   =   i o _ r o t . g e t A n g l e ( ) ; 
 
                         a 2   =   a n g l e . x ; 
 
                         b 2   =   a n g l e . y ; 
 
                         c 2   =   a n g l e . z ; 
 
 
 
                         / /   c o m b o n03 Lv0HQk0{
 
                         f o r   ( i n t   i   =   0 ;   i   <   1 0 ;   i + + ) 
 
                         { 
 
                                 m i n e r r   =   1 0 0 0 0 0 0 0 0 0 . 0 ; 
 
                                 / /   s i n - c o s 00000{( S00L0Yk0Q[00h0o0& 0) 
 
                                 f o r   ( i n t   j   =   0 ;   j   <   3 ;   j + + ) 
 
                                 { 
 
                                         w 2   =   f a c t o r   *   ( j   -   1 ) ; 
 
                                         w   =   a 2   +   w 2 ; 
 
                                         a _ f a c t o r [ j ]   =   w ; 
 
                                         w   =   b 2   +   w 2 ; 
 
                                         b _ f a c t o r [ j ]   =   w ; 
 
                                         s i n b [ j ]   =   M a t h . S i n ( w ) ; 
 
                                         c o s b [ j ]   =   M a t h . C o s ( w ) ; 
 
                                         w   =   c 2   +   w 2 ; 
 
                                         c _ f a c t o r [ j ]   =   w ; 
 
                                         s i n c [ j ]   =   M a t h . S i n ( w ) ; 
 
                                         c o s c [ j ]   =   M a t h . C o s ( w ) ; 
 
                                 } 
 
                                 / / 
 
                                 f o r   ( t 1   =   0 ;   t 1   <   3 ;   t 1 + + ) 
 
                                 { 
 
                                         S A   =   M a t h . S i n ( a _ f a c t o r [ t 1 ] ) ; 
 
                                         C A   =   M a t h . C o s ( a _ f a c t o r [ t 1 ] ) ; 
 
                                         / /   O p t i m i z e 
 
                                         C A C A   =   C A   *   C A ; 
 
                                         S A S A   =   S A   *   S A ; 
 
                                         S A C A   =   S A   *   C A ; 
 
                                         f o r   ( t 2   =   0 ;   t 2   <   3 ;   t 2 + + ) 
 
                                         { 
 
                                                 w s i n   =   s i n b [ t 2 ] ; 
 
                                                 w c o s   =   c o s b [ t 2 ] ; 
 
                                                 C A C A C B   =   C A C A   *   w c o s ; 
 
                                                 S A C A C B   =   S A C A   *   w c o s ; 
 
                                                 S A S A C B   =   S A S A   *   w c o s ; 
 
                                                 C A S B   =   C A   *   w s i n ; 
 
                                                 S A S B   =   S A   *   w s i n ; 
 
                                                 / /   c o m b o n0{1 
 
                                                 c o m b o 0 2   =   C P 0   *   C A S B   +   C P 1   *   S A S B   +   C P 2   *   w c o s ; 
 
                                                 c o m b o 1 2   =   C P 4   *   C A S B   +   C P 5   *   S A S B   +   C P 6   *   w c o s ; 
 
                                                 c o m b o 2 2   =   C P 8   *   C A S B   +   C P 9   *   S A S B   +   C P 1 0   *   w c o s ; 
 
 
 
                                                 c o m b o 0 2 _ 2   =   c o m b o 0 2   *   V X 0 2   +   c o m b o 0 3 ; 
 
                                                 c o m b o 0 2 _ 5   =   c o m b o 0 2   *   V X 1 2   +   c o m b o 0 3 ; 
 
                                                 c o m b o 0 2 _ 8   =   c o m b o 0 2   *   V X 2 2   +   c o m b o 0 3 ; 
 
                                                 c o m b o 0 2 _ 1 1   =   c o m b o 0 2   *   V X 3 2   +   c o m b o 0 3 ; 
 
                                                 c o m b o 1 2 _ 2   =   c o m b o 1 2   *   V X 0 2   +   c o m b o 1 3 ; 
 
                                                 c o m b o 1 2 _ 5   =   c o m b o 1 2   *   V X 1 2   +   c o m b o 1 3 ; 
 
                                                 c o m b o 1 2 _ 8   =   c o m b o 1 2   *   V X 2 2   +   c o m b o 1 3 ; 
 
                                                 c o m b o 1 2 _ 1 1   =   c o m b o 1 2   *   V X 3 2   +   c o m b o 1 3 ; 
 
                                                 c o m b o 2 2 _ 2   =   c o m b o 2 2   *   V X 0 2   +   c o m b o 2 3 ; 
 
                                                 c o m b o 2 2 _ 5   =   c o m b o 2 2   *   V X 1 2   +   c o m b o 2 3 ; 
 
                                                 c o m b o 2 2 _ 8   =   c o m b o 2 2   *   V X 2 2   +   c o m b o 2 3 ; 
 
                                                 c o m b o 2 2 _ 1 1   =   c o m b o 2 2   *   V X 3 2   +   c o m b o 2 3 ; 
 
                                                 f o r   ( t 3   =   0 ;   t 3   <   3 ;   t 3 + + ) 
 
                                                 { 
 
                                                         w s i n   =   s i n c [ t 3 ] ; 
 
                                                         w c o s   =   c o s c [ t 3 ] ; 
 
                                                         S A C A S C   =   S A C A   *   w s i n ; 
 
                                                         S A C A C C   =   S A C A   *   w c o s ; 
 
                                                         S A C A C B S C   =   S A C A C B   *   w s i n ; 
 
                                                         S A C A C B C C   =   S A C A C B   *   w c o s ; 
 
 
 
                                                         r o t 0   =   C A C A C B   *   w c o s   +   S A S A   *   w c o s   +   S A C A C B S C   -   S A C A S C ; 
 
                                                         r o t 1   =   S A C A C B C C   -   S A C A C C   +   S A S A C B   *   w s i n   +   C A C A   *   w s i n ; 
 
                                                         r o t 2   =   - C A S B   *   w c o s   -   S A S B   *   w s i n ; 
 
                                                         c o m b o 0 0   =   C P 0   *   r o t 0   +   C P 1   *   r o t 1   +   C P 2   *   r o t 2 ; 
 
                                                         c o m b o 1 0   =   C P 4   *   r o t 0   +   C P 5   *   r o t 1   +   C P 6   *   r o t 2 ; 
 
                                                         c o m b o 2 0   =   C P 8   *   r o t 0   +   C P 9   *   r o t 1   +   C P 1 0   *   r o t 2 ; 
 
 
 
                                                         r o t 0   =   - C A C A C B   *   w s i n   -   S A S A   *   w s i n   +   S A C A C B C C   -   S A C A C C ; 
 
                                                         r o t 1   =   - S A C A C B S C   +   S A C A S C   +   S A S A C B   *   w c o s   +   C A C A   *   w c o s ; 
 
                                                         r o t 2   =   C A S B   *   w s i n   -   S A S B   *   w c o s ; 
 
                                                         c o m b o 0 1   =   C P 0   *   r o t 0   +   C P 1   *   r o t 1   +   C P 2   *   r o t 2 ; 
 
                                                         c o m b o 1 1   =   C P 4   *   r o t 0   +   C P 5   *   r o t 1   +   C P 6   *   r o t 2 ; 
 
                                                         c o m b o 2 1   =   C P 8   *   r o t 0   +   C P 9   *   r o t 1   +   C P 1 0   *   r o t 2 ; 
 
                                                         / / 
 
                                                         e r r   =   0 . 0 ; 
 
                                                         h   =   c o m b o 2 0   *   V X 0 0   +   c o m b o 2 1   *   V X 0 1   +   c o m b o 2 2 _ 2 ; 
 
                                                         x   =   P 2 D 0 0   -   ( c o m b o 0 0   *   V X 0 0   +   c o m b o 0 1   *   V X 0 1   +   c o m b o 0 2 _ 2 )   /   h ; 
 
                                                         y   =   P 2 D 0 1   -   ( c o m b o 1 0   *   V X 0 0   +   c o m b o 1 1   *   V X 0 1   +   c o m b o 1 2 _ 2 )   /   h ; 
 
                                                         e r r   + =   x   *   x   +   y   *   y ; 
 
                                                         h   =   c o m b o 2 0   *   V X 1 0   +   c o m b o 2 1   *   V X 1 1   +   c o m b o 2 2 _ 5 ; 
 
                                                         x   =   P 2 D 1 0   -   ( c o m b o 0 0   *   V X 1 0   +   c o m b o 0 1   *   V X 1 1   +   c o m b o 0 2 _ 5 )   /   h ; 
 
                                                         y   =   P 2 D 1 1   -   ( c o m b o 1 0   *   V X 1 0   +   c o m b o 1 1   *   V X 1 1   +   c o m b o 1 2 _ 5 )   /   h ; 
 
                                                         e r r   + =   x   *   x   +   y   *   y ; 
 
                                                         h   =   c o m b o 2 0   *   V X 2 0   +   c o m b o 2 1   *   V X 2 1   +   c o m b o 2 2 _ 8 ; 
 
                                                         x   =   P 2 D 2 0   -   ( c o m b o 0 0   *   V X 2 0   +   c o m b o 0 1   *   V X 2 1   +   c o m b o 0 2 _ 8 )   /   h ; 
 
                                                         y   =   P 2 D 2 1   -   ( c o m b o 1 0   *   V X 2 0   +   c o m b o 1 1   *   V X 2 1   +   c o m b o 1 2 _ 8 )   /   h ; 
 
                                                         e r r   + =   x   *   x   +   y   *   y ; 
 
                                                         h   =   c o m b o 2 0   *   V X 3 0   +   c o m b o 2 1   *   V X 3 1   +   c o m b o 2 2 _ 1 1 ; 
 
                                                         x   =   P 2 D 3 0   -   ( c o m b o 0 0   *   V X 3 0   +   c o m b o 0 1   *   V X 3 1   +   c o m b o 0 2 _ 1 1 )   /   h ; 
 
                                                         y   =   P 2 D 3 1   -   ( c o m b o 1 0   *   V X 3 0   +   c o m b o 1 1   *   V X 3 1   +   c o m b o 1 2 _ 1 1 )   /   h ; 
 
                                                         e r r   + =   x   *   x   +   y   *   y ; 
 
                                                         i f   ( e r r   <   m i n e r r ) 
 
                                                         { 
 
                                                                 m i n e r r   =   e r r ; 
 
                                                                 a 2   =   a _ f a c t o r [ t 1 ] ; 
 
                                                                 b 2   =   b _ f a c t o r [ t 2 ] ; 
 
                                                                 c 2   =   c _ f a c t o r [ t 3 ] ; 
 
                                                                 b e s t _ i d x   =   t 1   +   t 2   *   3   +   t 3   *   9 ; 
 
                                                         } 
 
                                                 } 
 
                                         } 
 
                                 } 
 
                                 i f   ( b e s t _ i d x   = =   ( 1   +   3   +   9 ) ) 
 
                                 { 
 
                                         f a c t o r   * =   0 . 5 ; 
 
                                 } 
 
                         } 
 
                         i o _ r o t . s e t A n g l e ( a 2 ,   b 2 ,   c 2 ) ; 
 
                         / *   p r i n t f ( " f a c t o r   =   % 1 0 . 5 f \ n " ,   f a c t o r * 1 8 0 . 0 / M D _ P I ) ;   * / 
 
                         r e t u r n   m i n e r r   /   4 ; 
 
                 } 
 
         } 
 
 } 