/ *   
 
   *   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 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 ; 
 
 u s i n g   S y s t e m . D i a g n o s t i c s ; 
 
 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0N y A R T o o l k i t e_n0YRLRO p t i m i z e r g0Y00
 
           *   < p > 000000  - 
 
           *   YRLR0X , Y , Z n0V⎹eTk0d0D0f0OP_RW0f00]00^00]L0 g\k0j00p0Bl0~0Y00
 
           *   NMOL0pB004XTo00MRVn0P}gk0яD0$P0c(uY00S0h0g000000n0W0~0Y00
 
           *   < / p > 
 
           * / 
 
         p u b l i c   c l a s s   N y A R P a r t i a l D i f f e r e n t i a t i o n 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 P a r t i a l D i f f e r e n t i a t i o n O p t i m i z e ( 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 ; 
 
                 } 
 
                 / * 
 
                   *   \q_	Yc_  W,g_  o x = ( c o s c   *   c o s b   -   s i n c   *   s i n a   *   s i n b ) * i x + ( - s i n c   *   c o s a ) * i y + ( c o s c   *   s i n b   +   s i n c   *   s i n a   *   c o s b ) * i z + i _ t r a n s . x ;   o y = ( s i n c   *   c o s b   +   c o s c   *   s i n a   * 
 
                   *   s i n b ) * i x + ( c o s c   *   c o s a ) * i y + ( s i n c   *   s i n b   -   c o s c   *   s i n a   *   c o s b ) * i z + i _ t r a n s . y ;   o z = ( - c o s a   *   s i n b ) * i x + ( s i n a ) * i y + ( c o s b   *   c o s a ) * i z + i _ t r a n s . z ; 
 
                   *   
 
                   *   d o u b l e   o x = ( c o s c   *   c o s b ) * i x + ( - s i n c   *   s i n a   *   s i n b ) * i x + ( - s i n c   *   c o s a ) * i y + ( c o s c   *   s i n b ) * i z   +   ( s i n c   *   s i n a   *   c o s b ) * i z + i _ t r a n s . x ;   d o u b l e   o y = ( s i n c   *   c o s b ) * i x 
 
                   *   + ( c o s c   *   s i n a   *   s i n b ) * i x + ( c o s c   *   c o s a ) * i y + ( s i n c   *   s i n b ) * i z + ( -   c o s c   *   s i n a   *   c o s b ) * i z + i _ t r a n s . y ;   d o u b l e   o z = ( - c o s a   *   s i n b ) * i x + ( s i n a ) * i y + ( c o s b   * 
 
                   *   c o s a ) * i z + i _ t r a n s . z ; 
 
                   *   
 
                   *   s i n a , c o s a k0d0D0f0O0  c x = ( c p 0 0 * ( - s i n c * s i n b * i x + s i n c * c o s b * i z ) + c p 0 1 * ( c o s c * s i n b * i x - c o s c * c o s b * i z ) + c p 0 2 * ( i y ) ) * s i n a 
 
                   *   + ( c p 0 0 * ( - s i n c * i y ) + c p 0 1 * ( ( c o s c * i y ) ) + c p 0 2 * ( - s i n b * i x + c o s b * i z ) ) * c o s a 
 
                   *   + ( c p 0 0 * ( i _ t r a n s . x + c o s c * c o s b * i x + c o s c * s i n b * i z ) + c p 0 1 * ( ( i _ t r a n s . y + s i n c * c o s b * i x + s i n c * s i n b * i z ) ) + c p 0 2 * ( i _ t r a n s . z ) ) ; 
 
                   *   c y = ( c p 1 1 * ( c o s c * s i n b * i x - c o s c * c o s b * i z ) + c p 1 2 * ( i y ) ) * s i n a   + ( c p 1 1 * ( ( c o s c * i y ) ) + c p 1 2 * ( - s i n b * i x + c o s b * i z ) ) * c o s a 
 
                   *   + ( c p 1 1 * ( ( i _ t r a n s . y + s i n c * c o s b * i x + s i n c * s i n b * i z ) ) + c p 1 2 * ( i _ t r a n s . z ) ) ;   c h = ( i y ) * s i n a   + ( - s i n b * i x + c o s b * i z ) * c o s a   + i _ t r a n s . z ;   s i n b , c o s b   h x = ( c p 0 0 * ( - s i n c   * 
 
                   *   s i n a * i x + c o s c * i z ) + c p 0 1 * ( c o s c   *   s i n a * i x + s i n c * i z ) + c p 0 2 * ( - c o s a * i x ) ) * s i n b   + ( c p 0 1 * ( s i n c * i x - c o s c   *   s i n a * i z ) + c p 0 0 * ( c o s c * i x + s i n c   *   s i n a * i z ) + c p 0 2 * ( c o s a * i z ) ) * c o s b 
 
                   *   + ( c p 0 0 * ( i _ t r a n s . x + ( - s i n c * c o s a ) * i y ) + c p 0 1 * ( i _ t r a n s . y + ( c o s c   *   c o s a ) * i y ) + c p 0 2 * ( i _ t r a n s . z + ( s i n a ) * i y ) ) ;   d o u b l e   h y = ( c p 1 1 * ( c o s c   * 
 
                   *   s i n a * i x + s i n c * i z ) + c p 1 2 * ( - c o s a * i x ) ) * s i n b   + ( c p 1 1 * ( s i n c * i x - c o s c   *   s i n a * i z ) + c p 1 2 * ( c o s a * i z ) ) * c o s b   + ( c p 1 1 * ( i _ t r a n s . y + ( c o s c   * 
 
                   *   c o s a ) * i y ) + c p 1 2 * ( i _ t r a n s . z + ( s i n a ) * i y ) ) ;   d o u b l e   h   = ( ( - c o s a * i x ) * s i n b   + ( c o s a * i z ) * c o s b   + i _ t r a n s . z + ( s i n a ) * i y ) ;   00000ԏ_  L = 2 * ( d [ n ] * e [ n ] + a [ n ] * b [ n ] ) 
 
                   *   J = 2 * ( d [ n ] * f [ n ] + a [ n ] * c [ n ] ) / L   K = 2 * ( - e [ n ] * f [ n ] + b [ n ] * c [ n ] ) / L   M = ( - e [ n ] ^ 2 + d [ n ] ^ 2 - b [ n ] ^ 2 + a [ n ] ^ 2 ) / L   OP_R_  + J * c o s ( x )   + K * s i n ( x )   - s i n ( x ) ^ 2   + c o s ( x ) ^ 2 
 
                   *   + 2 * M * c o s ( x ) * s i n ( x ) 
 
                   * / 
 
                 p r i v a t e   d o u b l e   o p t i m i z e P a r a m X ( d o u b l e   s i n b ,   d o u b l e   c o s b ,   d o u b l e   s i n c ,   d o u b l e   c o s c ,   N y A R D o u b l e P o i n t 3 d   i _ 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 ,   i n t   i _ n u m b e r _ o f _ v e r t e x ,   d o u b l e   i _ h i n t _ a n g l e ) 
 
                 { 
 
                         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   c p   =   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   L ,   J ,   K ,   M ,   N ,   O ; 
 
                         L   =   J   =   K   =   M   =   N   =   O   =   0 ; 
 
                         d o u b l e   c p 0 0   =   c p . m 0 0 ; 
 
                         d o u b l e   c p 0 1   =   c p . m 0 1 ; 
 
                         d o u b l e   c p 0 2   =   c p . m 0 2 ; 
 
                         d o u b l e   c p 1 1   =   c p . m 1 1 ; 
 
                         d o u b l e   c p 1 2   =   c p . m 1 2 ; 
 
 
 
                         f o r   ( i n t   i   =   0 ;   i   <   i _ n u m b e r _ o f _ v e r t e x ;   i + + ) 
 
                         { 
 
                                 d o u b l e   i x ,   i y ,   i z ; 
 
                                 i x   =   i _ v e r t e x 3 d [ i ] . x ; 
 
                                 i y   =   i _ v e r t e x 3 d [ i ] . y ; 
 
                                 i z   =   i _ v e r t e x 3 d [ i ] . z ; 
 
 
 
                                 d o u b l e   X 0   =   ( c p 0 0   *   ( - s i n c   *   s i n b   *   i x   +   s i n c   *   c o s b   *   i z )   +   c p 0 1   *   ( c o s c   *   s i n b   *   i x   -   c o s c   *   c o s b   *   i z )   +   c p 0 2   *   ( i y ) ) ; 
 
                                 d o u b l e   X 1   =   ( c p 0 0   *   ( - s i n c   *   i y )   +   c p 0 1   *   ( ( c o s c   *   i y ) )   +   c p 0 2   *   ( - s i n b   *   i x   +   c o s b   *   i z ) ) ; 
 
                                 d o u b l e   X 2   =   ( c p 0 0   *   ( i _ t r a n s . x   +   c o s c   *   c o s b   *   i x   +   c o s c   *   s i n b   *   i z )   +   c p 0 1   *   ( ( i _ t r a n s . y   +   s i n c   *   c o s b   *   i x   +   s i n c   *   s i n b   *   i z ) )   +   c p 0 2   *   ( i _ t r a n s . z ) ) ; 
 
                                 d o u b l e   Y 0   =   ( c p 1 1   *   ( c o s c   *   s i n b   *   i x   -   c o s c   *   c o s b   *   i z )   +   c p 1 2   *   ( i y ) ) ; 
 
                                 d o u b l e   Y 1   =   ( c p 1 1   *   ( ( c o s c   *   i y ) )   +   c p 1 2   *   ( - s i n b   *   i x   +   c o s b   *   i z ) ) ; 
 
                                 d o u b l e   Y 2   =   ( c p 1 1   *   ( ( i _ t r a n s . y   +   s i n c   *   c o s b   *   i x   +   s i n c   *   s i n b   *   i z ) )   +   c p 1 2   *   ( i _ t r a n s . z ) ) ; 
 
                                 d o u b l e   H 0   =   ( i y ) ; 
 
                                 d o u b l e   H 1   =   ( - s i n b   *   i x   +   c o s b   *   i z ) ; 
 
                                 d o u b l e   H 2   =   i _ t r a n s . z ; 
 
 
 
                                 d o u b l e   V X   =   i _ v e r t e x 2 d [ i ] . x ; 
 
                                 d o u b l e   V Y   =   i _ v e r t e x 2 d [ i ] . y ; 
 
 
 
                                 d o u b l e   a ,   b ,   c ,   d ,   e ,   f ; 
 
                                 a   =   ( V X   *   H 0   -   X 0 ) ; 
 
                                 b   =   ( V X   *   H 1   -   X 1 ) ; 
 
                                 c   =   ( V X   *   H 2   -   X 2 ) ; 
 
                                 d   =   ( V Y   *   H 0   -   Y 0 ) ; 
 
                                 e   =   ( V Y   *   H 1   -   Y 1 ) ; 
 
                                 f   =   ( V Y   *   H 2   -   Y 2 ) ; 
 
 
 
                                 L   + =   d   *   e   +   a   *   b ; 
 
                                 N   + =   d   *   d   +   a   *   a ; 
 
                                 J   + =   d   *   f   +   a   *   c ; 
 
                                 M   + =   e   *   e   +   b   *   b ; 
 
                                 K   + =   e   *   f   +   b   *   c ; 
 
                                 O   + =   f   *   f   +   c   *   c ; 
 
 
 
                         } 
 
                         L   * =   2 ; 
 
                         J   * =   2 ; 
 
                         K   * =   2 ; 
 
 
 
                         r e t u r n   g e t M i n i m u m E r r o r A n g l e F r o m P a r a m ( L ,   J ,   K ,   M ,   N ,   O ,   i _ h i n t _ a n g l e ) ; 
 
 
 
 
 
                 } 
 
 
 
                 p r i v a t e   d o u b l e   o p t i m i z e P a r a m Y ( d o u b l e   s i n a ,   d o u b l e   c o s a ,   d o u b l e   s i n c ,   d o u b l e   c o s c ,   N y A R D o u b l e P o i n t 3 d   i _ 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 ,   i n t   i _ n u m b e r _ o f _ v e r t e x ,   d o u b l e   i _ h i n t _ a n g l e ) 
 
                 { 
 
                         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   c p   =   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   L ,   J ,   K ,   M ,   N ,   O ; 
 
                         L   =   J   =   K   =   M   =   N   =   O   =   0 ; 
 
                         d o u b l e   c p 0 0   =   c p . m 0 0 ; 
 
                         d o u b l e   c p 0 1   =   c p . m 0 1 ; 
 
                         d o u b l e   c p 0 2   =   c p . m 0 2 ; 
 
                         d o u b l e   c p 1 1   =   c p . m 1 1 ; 
 
                         d o u b l e   c p 1 2   =   c p . m 1 2 ; 
 
 
 
                         f o r   ( i n t   i   =   0 ;   i   <   i _ n u m b e r _ o f _ v e r t e x ;   i + + ) 
 
                         { 
 
                                 d o u b l e   i x ,   i y ,   i z ; 
 
                                 i x   =   i _ v e r t e x 3 d [ i ] . x ; 
 
                                 i y   =   i _ v e r t e x 3 d [ i ] . y ; 
 
                                 i z   =   i _ v e r t e x 3 d [ i ] . z ; 
 
 
 
                                 d o u b l e   X 0   =   ( c p 0 0   *   ( - s i n c   *   s i n a   *   i x   +   c o s c   *   i z )   +   c p 0 1   *   ( c o s c   *   s i n a   *   i x   +   s i n c   *   i z )   +   c p 0 2   *   ( - c o s a   *   i x ) ) ; 
 
                                 d o u b l e   X 1   =   ( c p 0 1   *   ( s i n c   *   i x   -   c o s c   *   s i n a   *   i z )   +   c p 0 0   *   ( c o s c   *   i x   +   s i n c   *   s i n a   *   i z )   +   c p 0 2   *   ( c o s a   *   i z ) ) ; 
 
                                 d o u b l e   X 2   =   ( c p 0 0   *   ( i _ t r a n s . x   +   ( - s i n c   *   c o s a )   *   i y )   +   c p 0 1   *   ( i _ t r a n s . y   +   ( c o s c   *   c o s a )   *   i y )   +   c p 0 2   *   ( i _ t r a n s . z   +   ( s i n a )   *   i y ) ) ; 
 
                                 d o u b l e   Y 0   =   ( c p 1 1   *   ( c o s c   *   s i n a   *   i x   +   s i n c   *   i z )   +   c p 1 2   *   ( - c o s a   *   i x ) ) ; 
 
                                 d o u b l e   Y 1   =   ( c p 1 1   *   ( s i n c   *   i x   -   c o s c   *   s i n a   *   i z )   +   c p 1 2   *   ( c o s a   *   i z ) ) ; 
 
                                 d o u b l e   Y 2   =   ( c p 1 1   *   ( i _ t r a n s . y   +   ( c o s c   *   c o s a )   *   i y )   +   c p 1 2   *   ( i _ t r a n s . z   +   ( s i n a )   *   i y ) ) ; 
 
                                 d o u b l e   H 0   =   ( - c o s a   *   i x ) ; 
 
                                 d o u b l e   H 1   =   ( c o s a   *   i z ) ; 
 
                                 d o u b l e   H 2   =   i _ t r a n s . z   +   ( s i n a )   *   i y ; 
 
 
 
                                 d o u b l e   V X   =   i _ v e r t e x 2 d [ i ] . x ; 
 
                                 d o u b l e   V Y   =   i _ v e r t e x 2 d [ i ] . y ; 
 
 
 
                                 d o u b l e   a ,   b ,   c ,   d ,   e ,   f ; 
 
                                 a   =   ( V X   *   H 0   -   X 0 ) ; 
 
                                 b   =   ( V X   *   H 1   -   X 1 ) ; 
 
                                 c   =   ( V X   *   H 2   -   X 2 ) ; 
 
                                 d   =   ( V Y   *   H 0   -   Y 0 ) ; 
 
                                 e   =   ( V Y   *   H 1   -   Y 1 ) ; 
 
                                 f   =   ( V Y   *   H 2   -   Y 2 ) ; 
 
 
 
                                 L   + =   d   *   e   +   a   *   b ; 
 
                                 N   + =   d   *   d   +   a   *   a ; 
 
                                 J   + =   d   *   f   +   a   *   c ; 
 
                                 M   + =   e   *   e   +   b   *   b ; 
 
                                 K   + =   e   *   f   +   b   *   c ; 
 
                                 O   + =   f   *   f   +   c   *   c ; 
 
 
 
                         } 
 
                         L   * =   2 ; 
 
                         J   * =   2 ; 
 
                         K   * =   2 ; 
 
                         r e t u r n   g e t M i n i m u m E r r o r A n g l e F r o m P a r a m ( L ,   J ,   K ,   M ,   N ,   O ,   i _ h i n t _ a n g l e ) ; 
 
 
 
                 } 
 
 
 
                 p r i v a t e   d o u b l e   o p t i m i z e P a r a m Z ( d o u b l e   s i n a ,   d o u b l e   c o s a ,   d o u b l e   s i n b ,   d o u b l e   c o s b ,   N y A R D o u b l e P o i n t 3 d   i _ 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 ,   i n t   i _ n u m b e r _ o f _ v e r t e x ,   d o u b l e   i _ h i n t _ a n g l e ) 
 
                 { 
 
                         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   c p   =   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   L ,   J ,   K ,   M ,   N ,   O ; 
 
                         L   =   J   =   K   =   M   =   N   =   O   =   0 ; 
 
                         d o u b l e   c p 0 0   =   c p . m 0 0 ; 
 
                         d o u b l e   c p 0 1   =   c p . m 0 1 ; 
 
                         d o u b l e   c p 0 2   =   c p . m 0 2 ; 
 
                         d o u b l e   c p 1 1   =   c p . m 1 1 ; 
 
                         d o u b l e   c p 1 2   =   c p . m 1 2 ; 
 
 
 
                         f o r   ( i n t   i   =   0 ;   i   <   i _ n u m b e r _ o f _ v e r t e x ;   i + + ) 
 
                         { 
 
                                 d o u b l e   i x ,   i y ,   i z ; 
 
                                 i x   =   i _ v e r t e x 3 d [ i ] . x ; 
 
                                 i y   =   i _ v e r t e x 3 d [ i ] . y ; 
 
                                 i z   =   i _ v e r t e x 3 d [ i ] . z ; 
 
 
 
                                 d o u b l e   X 0   =   ( c p 0 0   *   ( - s i n a   *   s i n b   *   i x   -   c o s a   *   i y   +   s i n a   *   c o s b   *   i z )   +   c p 0 1   *   ( i x   *   c o s b   +   s i n b   *   i z ) ) ; 
 
                                 d o u b l e   X 1   =   ( c p 0 1   *   ( s i n a   *   i x   *   s i n b   +   c o s a   *   i y   -   s i n a   *   i z   *   c o s b )   +   c p 0 0   *   ( c o s b   *   i x   +   s i n b   *   i z ) ) ; 
 
                                 d o u b l e   X 2   =   c p 0 0   *   i _ t r a n s . x   +   c p 0 1   *   ( i _ t r a n s . y )   +   c p 0 2   *   ( - c o s a   *   s i n b )   *   i x   +   c p 0 2   *   ( s i n a )   *   i y   +   c p 0 2   *   ( ( c o s b   *   c o s a )   *   i z   +   i _ t r a n s . z ) ; 
 
                                 d o u b l e   Y 0   =   c p 1 1   *   ( i x   *   c o s b   +   s i n b   *   i z ) ; 
 
                                 d o u b l e   Y 1   =   c p 1 1   *   ( s i n a   *   i x   *   s i n b   +   c o s a   *   i y   -   s i n a   *   i z   *   c o s b ) ; 
 
                                 d o u b l e   Y 2   =   ( c p 1 1   *   i _ t r a n s . y   +   c p 1 2   *   ( - c o s a   *   s i n b )   *   i x   +   c p 1 2   *   ( ( s i n a )   *   i y   +   ( c o s b   *   c o s a )   *   i z   +   i _ t r a n s . z ) ) ; 
 
                                 d o u b l e   H 0   =   0 ; 
 
                                 d o u b l e   H 1   =   0 ; 
 
                                 d o u b l e   H 2   =   ( ( - c o s a   *   s i n b )   *   i x   +   ( s i n a )   *   i y   +   ( c o s b   *   c o s a )   *   i z   +   i _ t r a n s . z ) ; 
 
 
 
                                 d o u b l e   V X   =   i _ v e r t e x 2 d [ i ] . x ; 
 
                                 d o u b l e   V Y   =   i _ v e r t e x 2 d [ i ] . y ; 
 
 
 
                                 d o u b l e   a ,   b ,   c ,   d ,   e ,   f ; 
 
                                 a   =   ( V X   *   H 0   -   X 0 ) ; 
 
                                 b   =   ( V X   *   H 1   -   X 1 ) ; 
 
                                 c   =   ( V X   *   H 2   -   X 2 ) ; 
 
                                 d   =   ( V Y   *   H 0   -   Y 0 ) ; 
 
                                 e   =   ( V Y   *   H 1   -   Y 1 ) ; 
 
                                 f   =   ( V Y   *   H 2   -   Y 2 ) ; 
 
 
 
                                 L   + =   d   *   e   +   a   *   b ; 
 
                                 N   + =   d   *   d   +   a   *   a ; 
 
                                 J   + =   d   *   f   +   a   *   c ; 
 
                                 M   + =   e   *   e   +   b   *   b ; 
 
                                 K   + =   e   *   f   +   b   *   c ; 
 
                                 O   + =   f   *   f   +   c   *   c ; 
 
 
 
                         } 
 
                         L   * =   2 ; 
 
                         J   * =   2 ; 
 
                         K   * =   2 ; 
 
 
 
                         r e t u r n   g e t M i n i m u m E r r o r A n g l e F r o m P a r a m ( L ,   J ,   K ,   M ,   N ,   O ,   i _ h i n t _ a n g l e ) ; 
 
                 } 
 
                 p r i v a t e   N y A R D o u b l e P o i n t 3 d   _ _ a n g   =   n e w   N y A R D o u b l e P o i n t 3 d ( ) ; 
 
                 / * * 
 
                   *   S0n0peo00VLR0 giSW0~0Y00
 
                   *   i _ v e r t e x 3 d n000000$P00i o _ r o t h0i _ t r a n s g0^j	Yc_k0\q_	YcW0_0!kCQ^jh00i _ v e r t e x 2 d L0 g0яO0j000F0k00i o _ r o t 0teW0~0Y00
 
                   *   i o _ r o t , i _ t r a n s n0$Po00B00z^n0|^g0Bl000f0D00_L0B00~0Y00
 
                   *   @ p a r a m   i o _ r o t 
 
                   *   teY00VLR
 
                   *   @ p a r a m   i _ t r a n s 
 
                   *   s^LyRϑ
 
                   *   @ p a r a m   i _ v e r t e x 3 d 
 
                   *   	N!kCQ00000^j
 
                   *   @ p a r a m   i _ v e r t e x 2 d 
 
                   *   t`^j|n0p^j
 
                   *   @ p a r a m   i _ n u m b e r _ o f _ v e r t e x 
 
                   *   ppe
 
                   *   @ 
 
                   * / 
 
                 p u b l i c   v o i d   m o d i f y M a t r i x ( N y A R D o u b l e M a t r i x 3 3   i o _ r o t ,   N y A R D o u b l e P o i n t 3 d   i _ 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 ,   i n t   i _ n u m b e r _ o f _ v e r t e x ) 
 
                 { 
 
                         N y A R D o u b l e P o i n t 3 d   a n g   =   t h i s . _ _ a n g ; 
 
                         / /   Z X Y |n0s i n / c o s $P0bQ
 
                         i o _ r o t . g e t Z X Y A n g l e ( a n g ) ; 
 
                         m o d i f y M a t r i x ( a n g ,   i _ t r a n s ,   i _ v e r t e x 3 d ,   i _ v e r t e x 2 d ,   i _ n u m b e r _ o f _ v e r t e x ,   a n g ) ; 
 
                         i o _ r o t . s e t Z X Y A n g l e ( a n g . x ,   a n g . y ,   a n g . z ) ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo00V҉0 giSW0~0Y00
 
                   *   i _ v e r t e x 3 d n000000$P00i _ a n g l e h0i _ t r a n s g0^j	Yc_k0\q_	YcW0_0!kCQ^jh00i _ v e r t e x 2 d L0 g0яO0j00$P00o _ a n g l e x0ԏW0~0Y00
 
                   *   i o _ r o t , i _ t r a n s n0$Po00B00z^n0|^g0Bl000f0D00_L0B00~0Y00
 
                   *   @ p a r a m   i _ a n g l e 
 
                   *   V҉
 
                   *   @ p a r a m   i _ t r a n s 
 
                   *   s^LyRϑ
 
                   *   @ p a r a m   i _ v e r t e x 3 d 
 
                   *   	N!kCQ00000^j
 
                   *   @ p a r a m   i _ v e r t e x 2 d 
 
                   *   t`^j|n0p^j
 
                   *   @ p a r a m   i _ n u m b e r _ o f _ v e r t e x 
 
                   *   ppe
 
                   *   @ p a r a m   o _ a n g l e 
 
                   *   teW0_0V҉0SQ0S0MR
 
                   *   @ 
 
                   * / 
 
                 p u b l i c   v o i d   m o d i f y M a t r i x ( N y A R D o u b l e P o i n t 3 d   i _ a n g l e ,   N y A R D o u b l e P o i n t 3 d   i _ 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 ,   i n t   i _ n u m b e r _ o f _ v e r t e x ,   N y A R D o u b l e P o i n t 3 d   o _ a n g l e ) 
 
                 { 
 
 
 
                         / /   Z X Y |n0s i n / c o s $P0bQ
 
                         d o u b l e   s i n x   =   M a t h . S i n ( i _ a n g l e . x ) ; 
 
                         d o u b l e   c o s x   =   M a t h . C o s ( i _ a n g l e . x ) ; 
 
                         d o u b l e   s i n y   =   M a t h . S i n ( i _ a n g l e . y ) ; 
 
                         d o u b l e   c o s y   =   M a t h . C o s ( i _ a n g l e . y ) ; 
 
                         d o u b l e   s i n z   =   M a t h . S i n ( i _ a n g l e . z ) ; 
 
                         d o u b l e   c o s z   =   M a t h . C o s ( i _ a n g l e . z ) ; 
 
                         o _ a n g l e . x   =   i _ a n g l e . x   +   o p t i m i z e P a r a m X ( s i n y ,   c o s y ,   s i n z ,   c o s z ,   i _ t r a n s ,   i _ v e r t e x 3 d ,   i _ v e r t e x 2 d ,   i _ n u m b e r _ o f _ v e r t e x ,   i _ a n g l e . x ) ; 
 
                         o _ a n g l e . y   =   i _ a n g l e . y   +   o p t i m i z e P a r a m Y ( s i n x ,   c o s x ,   s i n z ,   c o s z ,   i _ t r a n s ,   i _ v e r t e x 3 d ,   i _ v e r t e x 2 d ,   i _ n u m b e r _ o f _ v e r t e x ,   i _ a n g l e . y ) ; 
 
                         o _ a n g l e . z   =   i _ a n g l e . z   +   o p t i m i z e P a r a m Z ( s i n x ,   c o s x ,   s i n y ,   c o s y ,   i _ t r a n s ,   i _ v e r t e x 3 d ,   i _ v e r t e x 2 d ,   i _ n u m b e r _ o f _ v e r t e x ,   i _ a n g l e . z ) ; 
 
                         r e t u r n ; 
 
                 } 
 
 
 
                 p r i v a t e   d o u b l e [ ]   _ _ s i n _ t a b l e   =   n e w   d o u b l e [ 4 ] ; 
 
                 / * * 
 
                   *   000000L0 g\k0j00p0_00
 
                   * / 
 
                 p r i v a t e   d o u b l e   g e t M i n i m u m E r r o r A n g l e F r o m P a r a m ( d o u b l e   i L ,   d o u b l e   i J ,   d o u b l e   i K ,   d o u b l e   i M ,   d o u b l e   i N ,   d o u b l e   i O ,   d o u b l e   i _ h i n t _ a n g l e ) 
 
                 { 
 
                         / / i L L00 n0Bfo0]OckW0j0D00
 
                         i f   ( i L   = =   0 ) 
 
                         { 
 
                                 r e t u r n   0 ; 
 
                         } 
 
 
 
                         d o u b l e [ ]   s i n _ t a b l e   =   t h i s . _ _ s i n _ t a b l e ; 
 
 
 
                         d o u b l e   M   =   ( i N   -   i M )   /   i L ; 
 
                         d o u b l e   J   =   i J   /   i L ; 
 
                         d o u b l e   K   =   - i K   /   i L ; 
 
 
 
                         / /   00000K00s i n 00000\Ob
 
                         / /   ( -   4 * M ^ 2 - 4 ) * x ^ 4   +   ( 4 * K -   4 * J * M ) * x ^ 3   +   ( 4 * M ^ 2   - ( K ^ 2 -   4 ) -   J ^ 2 ) * x ^ 2   + ( 4 * J * M -   2 * K ) * x   +   J ^ 2 - 1   =   0 
 
                         i n t   n u m b e r _ o f _ s i n   =   N y A R E q u a t i o n S o l v e r . s o l v e 4 E q u a t i o n ( - 4   *   M   *   M   -   4 ,   4   *   K   -   4   *   J   *   M ,   4   *   M   *   M   -   ( K   *   K   -   4 )   -   J   *   J ,   4   *   J   *   M   -   2   *   K ,   J   *   J   -   1 ,   s i n _ t a b l e ) ; 
 
 
 
 
 
                         / /    g\$PP0_f0J0O00
 
                         d o u b l e   m i n _ a n g _ 0   =   D o u b l e . M a x V a l u e ; 
 
                         d o u b l e   m i n _ a n g _ 1   =   D o u b l e . M a x V a l u e ; 
 
                         d o u b l e   m i n _ e r r _ 0   =   D o u b l e . M a x V a l u e ; 
 
                         d o u b l e   m i n _ e r r _ 1   =   D o u b l e . M a x V a l u e ; 
 
                         f o r   ( i n t   i   =   0 ;   i   <   n u m b e r _ o f _ s i n ;   i + + ) 
 
                         { 
 
                                 / /   + - c o s _ v [ i ] L0pP܈
 
                                 d o u b l e   s i n _ r t   =   s i n _ t a b l e [ i ] ; 
 
                                 d o u b l e   c o s _ r t   =   M a t h . S q r t ( 1   -   ( s i n _ r t   *   s i n _ r t ) ) ; 
 
                                 / /   c o s 0O_0_R_g00 k0яD0eL0ck
 
                                 / /   0   =   2 * c o s ( x ) * s i n ( x ) * M   -   s i n ( x ) ^ 2   +   c o s ( x ) ^ 2   +   s i n ( x ) * K   +   c o s ( x ) * J 
 
                                 d o u b l e   a 1   =   2   *   c o s _ r t   *   s i n _ r t   *   M   +   s i n _ r t   *   ( K   -   s i n _ r t )   +   c o s _ r t   *   ( c o s _ r t   +   J ) ; 
 
                                 d o u b l e   a 2   =   2   *   ( - c o s _ r t )   *   s i n _ r t   *   M   +   s i n _ r t   *   ( K   -   s i n _ r t )   +   ( - c o s _ r t )   *   ( ( - c o s _ r t )   +   J ) ; 
 
                                 / /   v}[$Pk0j0J0W0f00wn0c o s $P0_f0J0O00
 
                                 a 1   =   a 1   <   0   ?   - a 1   :   a 1 ; 
 
                                 a 2   =   a 2   <   0   ?   - a 2   :   a 2 ; 
 
                                 c o s _ r t   =   ( a 1   <   a 2 )   ?   c o s _ r t   :   - c o s _ r t ; 
 
                                 d o u b l e   a n g   =   M a t h . A t a n 2 ( s i n _ r t ,   c o s _ r t ) ; 
 
                                 / /   000$P0{
 
                                 d o u b l e   e r r   =   i N   *   s i n _ r t   *   s i n _ r t   +   ( i L   *   c o s _ r t   +   i J )   *   s i n _ r t   +   i M   *   c o s _ r t   *   c o s _ r t   +   i K   *   c o s _ r t   +   i O ; 
 
                                 / /    g\n0P0rs_Y000
 
                                 i f   ( m i n _ e r r _ 0   >   e r r ) 
 
                                 { 
 
                                         m i n _ e r r _ 1   =   m i n _ e r r _ 0 ; 
 
                                         m i n _ a n g _ 1   =   m i n _ a n g _ 0 ; 
 
                                         m i n _ e r r _ 0   =   e r r ; 
 
                                         m i n _ a n g _ 0   =   a n g ; 
 
                                 } 
 
                                 e l s e   i f   ( m i n _ e r r _ 1   >   e r r ) 
 
                                 { 
 
                                         m i n _ e r r _ 1   =   e r r ; 
 
                                         m i n _ a n g _ 1   =   a n g ; 
 
                                 } 
 
                         } 
 
                         / /   [ 0 ] 0000
 
                         d o u b l e   g a p _ 0 ; 
 
                         g a p _ 0   =   m i n _ a n g _ 0   -   i _ h i n t _ a n g l e ; 
 
                         i f   ( g a p _ 0   >   M a t h . P I ) 
 
                         { 
 
                                 g a p _ 0   =   ( m i n _ a n g _ 0   -   M a t h . P I   *   2 )   -   i _ h i n t _ a n g l e ; 
 
                         } 
 
                         e l s e   i f   ( g a p _ 0   <   - M a t h . P I ) 
 
                         { 
 
                                 g a p _ 0   =   ( m i n _ a n g _ 0   +   M a t h . P I   *   2 )   -   i _ h i n t _ a n g l e ; 
 
                         } 
 
                         / /   [ 1 ] 0000
 
                         d o u b l e   g a p _ 1 ; 
 
                         g a p _ 1   =   m i n _ a n g _ 1   -   i _ h i n t _ a n g l e ; 
 
                         i f   ( g a p _ 1   >   M a t h . P I ) 
 
                         { 
 
                                 g a p _ 1   =   ( m i n _ a n g _ 1   -   M a t h . P I   *   2 )   -   i _ h i n t _ a n g l e ; 
 
                         } 
 
                         e l s e   i f   ( g a p _ 1   <   - M a t h . P I ) 
 
                         { 
 
                                 g a p _ 1   =   ( m i n _ a n g _ 1   +   M a t h . P I   *   2 )   -   i _ h i n t _ a n g l e ; 
 
                         } 
 
                         r e t u r n   M a t h . A b s ( g a p _ 1 )   <   M a t h . A b s ( g a p _ 0 )   ?   g a p _ 1   :   g a p _ 0 ; 
 
                 } 
 
         } 
 
 } 