/ *   
 
   *   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 > 
 
   *   
 
   * / 
 
 n a m e s p a c e   j p . n y a t l a . n y a r t o o l k i t . c s . c o r e 
 
 { 
 
 
 
 
 
 
 
 
 
 
 
         / * * 
 
           *   S0n0000o00A R T o o l K i t h0TX000000000puj00o{Kbg0QtW0f00&N200000Bl0~0Y00
 
           *   000000o00A R T o o l K i t   b5_s[0000000eQ  n00P 2 0 7 n00n0g0Y00
 
           *   < p > {Kb
 
           *   < p r e > 
 
           *   [ A ] * [ T ] = b 00[ A ] T * [ A ] * [ T ] = [ A ] T * [ b ] k0Y000
 
           *   s e t 2 d V e r t e x g0[ A ] T * [ A ] = [ M ] 0{W0f00A n03 Rvn0`1X`0Q0OX[W0f0J0O00
 
           *   g e t T r a n s p o r t V e c t o r g0[ M ] * [ T ] = [ A ] T * [ b ] 0#zez_g0D0f00[ T ] 0_00
 
           *   < / p r e > 
 
           *   < / p > 
 
           * / 
 
         p u b l i c   c l a s s   N y A R T r a n s p o r t V e c t o r S o l v e r   :   I N y A R T r a n s p o r t V e c t o r S o l v e r 
 
         { 
 
                 p r i v a t e   d o u b l e [ ]   _ c x ; 
 
                 p r i v a t e   d o u b l e [ ]   _ c y ; 
 
                 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 ; 
 
                 p r i v a t e   i n t   _ n m b e r _ o f _ v e r t e x ; 
 
                 / * * 
 
                   *   0000000g0Y00
 
                   *   \q_	Yc000000n0Sgq$Ph00S0qbF0pn0 g'Ype0c[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$Pg0Y00
 
                   *   @ p a r a m   i _ m a x _ v e r t e x 
 
                   *   S0qbF0pn0 g'Ype0
 
                   * / 
 
                 p u b l i c   N y A R T r a n s p o r t V e c t o r S o l v e r ( 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 ,   i n t   i _ m a x _ v e r t e x ) 
 
                 { 
 
                         t h i s . _ p r o j e c t i o n _ m a t   =   i _ p r o j e c t i o n _ m a t _ r e f ; 
 
                         t h i s . _ c x   =   n e w   d o u b l e [ i _ m a x _ v e r t e x ] ; 
 
                         t h i s . _ c y   =   n e w   d o u b l e [ i _ m a x _ v e r t e x ] ; 
 
                         r e t u r n ; 
 
                 } 
 
                 p r i v a t e   d o u b l e   _ a 0 0 ,   _ a 0 1 _ 1 0 ,   _ a 0 2 _ 2 0 ,   _ a 1 1 ,   _ a 1 2 _ 2 1 ,   _ a 2 2 ; 
 
                 / * * 
 
                   *   S0n0peo00\q_	Yc_n0!kCQp^j0000W0~0Y00
 
                   *   i _ n u m b e r _ o f _ v e r t e x o000000000g0c[W0_0 g'YpeNNg0B00_L0B00~0Y00
 
                   * / 
 
                 p u b l i c   v o i d   s e t 2 d V e r t e x ( N y A R D o u b l e P o i n t 2 d [ ]   i _ r e f _ 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 ) 
 
                 { 
 
                         / / 3 x 2 n h02 n * 3 n0LRK000 g\NWNl{Y00_00k03 x 3 000000\O00	 	 
 
                         / / LR[ A ] n03 Rvn000000
 
                         d o u b l e [ ]   c x   =   t h i s . _ c x ; 
 
                         d o u b l e [ ]   c y   =   t h i s . _ c y ; 
 
 
 
                         d o u b l e   m 2 2 ; 
 
                         d o u b l e   p 0 0   =   t h i s . _ p r o j e c t i o n _ m a t . m 0 0 ; 
 
                         d o u b l e   p 0 1   =   t h i s . _ p r o j e c t i o n _ m a t . m 0 1 ; 
 
                         d o u b l e   p 1 1   =   t h i s . _ p r o j e c t i o n _ m a t . m 1 1 ; 
 
                         d o u b l e   p 1 2   =   t h i s . _ p r o j e c t i o n _ m a t . m 1 2 ; 
 
                         d o u b l e   p 0 2   =   t h i s . _ p r o j e c t i o n _ m a t . m 0 2 ; 
 
                         d o u b l e   w 1 ,   w 2 ,   w 3 ,   w 4 ; 
 
 
 
                         t h i s . _ a 0 0   =   i _ n u m b e r _ o f _ v e r t e x   *   p 0 0   *   p 0 0 ; 
 
                         t h i s . _ a 0 1 _ 1 0   =   i _ n u m b e r _ o f _ v e r t e x   *   p 0 0   *   p 0 1 ; 
 
                         t h i s . _ a 1 1   =   i _ n u m b e r _ o f _ v e r t e x   *   ( p 0 1   *   p 0 1   +   p 1 1   *   p 1 1 ) ; 
 
 
 
                         / / [ A ] T * [ A ] n0{
 
                         m 2 2   =   0 ; 
 
                         w 1   =   w 2   =   0 ; 
 
                         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 + + ) 
 
                         { 
 
                                 / / ^j0OX[W0f0J0O00
 
                                 w 3   =   p 0 2   -   ( c x [ i ]   =   i _ r e f _ v e r t e x _ 2 d [ i ] . x ) ; 
 
                                 w 4   =   p 1 2   -   ( c y [ i ]   =   i _ r e f _ v e r t e x _ 2 d [ i ] . y ) ; 
 
                                 w 1   + =   w 3 ; 
 
                                 w 2   + =   w 4 ; 
 
                                 m 2 2   + =   w 3   *   w 3   +   w 4   *   w 4 ; 
 
                         } 
 
                         t h i s . _ a 0 2 _ 2 0   =   w 1   *   p 0 0 ; 
 
                         t h i s . _ a 1 2 _ 2 1   =   p 0 1   *   w 1   +   p 1 1   *   w 2 ; 
 
                         t h i s . _ a 2 2   =   m 2 2 ; 
 
 
 
                         t h i s . _ n m b e r _ o f _ v e r t e x   =   i _ n u m b e r _ o f _ v e r t e x ; 
 
                         r e t u r n ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   ;ub^jh03 !kCQ^jK000s^LyRϑ0{W0~0Y00
 
                   *   2 d ^j|o00vMRk0[LW0_0{ @ l i n k   # s e t 2 d V e r t e x } n00n00O(uW0~0Y00
 
                   * / 
 
                 p u b l i c   v o i d   s o l v e T r a n s p o r t V e c t o r ( 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 3 d   o _ t r a n s f e r ) 
 
                 { 
 
                         i n t   n u m b e r _ o f _ v e r t e x   =   t h i s . _ n m b e r _ o f _ v e r t e x ; 
 
                         d o u b l e   p 0 0   =   t h i s . _ p r o j e c t i o n _ m a t . m 0 0 ; 
 
                         d o u b l e   p 0 1   =   t h i s . _ p r o j e c t i o n _ m a t . m 0 1 ; 
 
                         d o u b l e   p 0 2   =   t h i s . _ p r o j e c t i o n _ m a t . m 0 2 ; 
 
                         d o u b l e   p 1 1   =   t h i s . _ p r o j e c t i o n _ m a t . m 1 1 ; 
 
                         d o u b l e   p 1 2   =   t h i s . _ p r o j e c t i o n _ m a t . m 1 2 ; 
 
                         / / LR[ A ] n03 Rvn000000
 
                         d o u b l e [ ]   c x   =   t h i s . _ c x ; 
 
                         d o u b l e [ ]   c y   =   t h i s . _ c y ; 
 
 
 
                         / / VLR0CQ^jn0pk0i_
 
                         / / [ A ] T * [ b ] 0{
 
                         d o u b l e   b 1   =   0 ,   b 2   =   0 ,   b 3   =   0 ; 
 
                         f o r   ( i n t   i   =   0 ;   i   <   n u m b e r _ o f _ v e r t e x ;   i + + ) 
 
                         { 
 
                                 d o u b l e   w 1   =   i _ v e r t e x 3 d [ i ] . z   *   c x [ i ]   -   p 0 0   *   i _ v e r t e x 3 d [ i ] . x   -   p 0 1   *   i _ v e r t e x 3 d [ i ] . y   -   p 0 2   *   i _ v e r t e x 3 d [ i ] . z ; 
 
                                 d o u b l e   w 2   =   i _ v e r t e x 3 d [ i ] . z   *   c y [ i ]   -   p 1 1   *   i _ v e r t e x 3 d [ i ] . y   -   p 1 2   *   i _ v e r t e x 3 d [ i ] . z ; 
 
                                 b 1   + =   w 1 ; 
 
                                 b 2   + =   w 2 ; 
 
                                 b 3   + =   c x [ i ]   *   w 1   +   c y [ i ]   *   w 2 ; 
 
                         } 
 
                         / / [ A ] T * [ b ] 0{
 
                         b 3   =   p 0 2   *   b 1   +   p 1 2   *   b 2   -   b 3 ; / / ju	YH0_00000
 
                         b 2   =   p 0 1   *   b 1   +   p 1 1   *   b 2 ; 
 
                         b 1   =   p 0 0   *   b 1 ; 
 
                         / / ( [ A ] T * [ A ] ) * [ T ] = [ A ] T * [ b ] 0ez_g0O00
 
                         / / a 0 1 h0a 1 0 00 h0N[W0f00oD00X00j0D0K0j0
 
                         d o u b l e   a 0 0   =   t h i s . _ a 0 0 ; 
 
                         d o u b l e   a 0 1   =   t h i s . _ a 0 1 _ 1 0 ; 
 
                         d o u b l e   a 0 2   =   t h i s . _ a 0 2 _ 2 0 ; 
 
                         d o u b l e   a 1 1   =   t h i s . _ a 1 1 ; 
 
                         d o u b l e   a 1 2   =   t h i s . _ a 1 2 _ 2 1 ; 
 
                         d o u b l e   a 2 2   =   t h i s . _ a 2 2 ; 
 
 
 
                         d o u b l e   t 1   =   a 2 2   *   b 2   -   a 1 2   *   b 3 ; 
 
                         d o u b l e   t 2   =   a 1 2   *   b 2   -   a 1 1   *   b 3 ; 
 
                         d o u b l e   t 3   =   a 0 1   *   b 3   -   a 0 2   *   b 2 ; 
 
                         d o u b l e   t 4   =   a 1 2   *   a 1 2   -   a 1 1   *   a 2 2 ; 
 
                         d o u b l e   t 5   =   a 0 2   *   a 1 2   -   a 0 1   *   a 2 2 ; 
 
                         d o u b l e   t 6   =   a 0 2   *   a 1 1   -   a 0 1   *   a 1 2 ; 
 
                         d o u b l e   d e t   =   a 0 0   *   t 4   -   a 0 1   *   t 5   +   a 0 2   *   t 6 ; 
 
                         o _ t r a n s f e r . x   =   ( a 0 1   *   t 1   -   a 0 2   *   t 2   +   b 1   *   t 4 )   /   d e t ; 
 
                         o _ t r a n s f e r . y   =   - ( a 0 0   *   t 1   +   a 0 2   *   t 3   +   b 1   *   t 5 )   /   d e t ; 
 
                         o _ t r a n s f e r . z   =   ( a 0 0   *   t 2   +   a 0 1   *   t 3   +   b 1   *   t 6 )   /   d e t ; 
 
 
 
 
 
                         r e t u r n ; 
 
                 } 
 
         } 
 
 } 
 
 