/ *   
 
   *   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 . 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0A R T o o l K i t h0TX0{Kbg0&N200000Bl0~0Y00
 
           * / 
 
         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 _ A R T o o l K i t   :   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   r e a d o n l y   N y A R M a t   _ m a t _ a t   =   n e w   N y A R M a t ( 3 ,   8 ) ; / / 3 , N U M B E R _ O F _ V E R T E X * 2 
 
                 p r i v a t e   r e a d o n l y   N y A R M a t   _ m a t _ a   =   n e w   N y A R M a t ( 8 ,   3 ) ; / / N U M B E R _ O F _ V E R T E X , 3 
 
                 p r i v a t e   r e a d o n l y   N y A R M a t   _ m a t _ t   =   n e w   N y A R M a t ( 3 ,   3 ) ; / / N U M B E R _ O F _ V E R T E X , 3 
 
                 p r i v a t e   r e a d o n l y   N y A R M a t   _ m a t _ c   =   n e w   N y A R M a t ( 8 ,   1 ) ; / / N U M B E R _ O F _ V E R T E X   *   2 ,   1 
 
                 p r i v a t e   r e a d o n l y   N y A R M a t   _ m a t _ e   =   n e w   N y A R M a t ( 3 ,   1 ) ; 
 
                 p r i v a t e   r e a d o n l y   N y A R M a t   _ m a t _ f   =   n e w   N y A R M a t ( 3 ,   1 ) ; 
 
                 p r i v a t e   r e a d o n l y   d o u b l e [ ]   _ c x   =   n e w   d o u b l e [ 4 ] ; 
 
                 p r i v a t e   r e a d o n l y   d o u b l e [ ]   _ c y   =   n e w   d o u b l e [ 4 ] ; 
 
 
 
                 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 ; 
 
                 / * * 
 
                   *   0000000g0Y00
 
                   *   \q_	Yc000000n0Sgq$P0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 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 _ A R T o o l K i t ( 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   =   i _ p r o j e c t i o n _ m a t _ r e f ; 
 
                         / / a h0b ( a n0nLR) n0V[R0-[0
 
                         d o u b l e [ ] [ ]   m a t a   =   t h i s . _ m a t _ a . g e t A r r a y ( ) ; 
 
                         d o u b l e [ ] [ ]   m a t a t   =   t h i s . _ m a t _ a t . g e t A r r a y ( ) ; 
 
 
 
                         / / 	Yc(uLRn0c p a r a R0HQk0\Ob
 
                         f o r   ( i n t   i   =   0 ;   i   <   4 ;   i + + ) 
 
                         { 
 
                                 i n t   x 2   =   i   *   2 ; 
 
                                 m a t a [ x 2 ] [ 0 ]   =   m a t a t [ 0 ] [ x 2 ]   =   i _ p r o j e c t i o n _ m a t _ r e f . m 0 0 ; / /   m a t _ a - > m [ j * 6 + 0 ] = m a t _ b - > m [ n u m * 0 + j * 2 ]   = c p a r a [ 0 ] [ 0 ] ; 
 
                                 m a t a [ x 2 ] [ 1 ]   =   m a t a t [ 1 ] [ x 2 ]   =   i _ p r o j e c t i o n _ m a t _ r e f . m 0 1 ; / /   m a t _ a - > m [ j * 6 + 1 ] = m a t _ b - > m [ n u m * 2 + j * 2 ] = c p a r a [ 0 ] [ 1 ] ; 
 
                                 m a t a [ x 2   +   1 ] [ 0 ]   =   m a t a t [ 0 ] [ x 2   +   1 ]   =   0 . 0 ; / /   m a t _ a - > m [ j * 6 + 3 ]   = m a t _ b - > m [ n u m * 0 + j * 2 + 1 ] =   0 . 0 ; 
 
                                 m a t a [ x 2   +   1 ] [ 1 ]   =   m a t a t [ 1 ] [ x 2   +   1 ]   =   i _ p r o j e c t i o n _ m a t _ r e f . m 1 1 ; / /   m a t _ a - > m [ j * 6 + 4 ]   = m a t _ b - > m [ n u m * 2 + j * 2 + 1 ] =   c p a r a [ 1 ] [ 1 ] ; 
 
                         } 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   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g0B00_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 ) 
 
                 { 
 
                         D e b u g . A s s e r t ( i _ n u m b e r _ o f _ v e r t e x   = =   4 ) ; 
 
                         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   c p a r a 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   c p a r a 1 2   =   t h i s . _ p r o j e c t i o n _ m a t . m 1 2 ; 
 
                         N y A R M a t   m a t _ t   =   t h i s . _ m a t _ t ; 
 
                         d o u b l e [ ] [ ]   m a t a   =   t h i s . _ m a t _ a . g e t A r r a y ( ) ; 
 
                         d o u b l e [ ] [ ]   m a t a t   =   t h i s . _ m a t _ a t . g e t A r r a y ( ) ; 
 
                         f o r   ( i n t   i   =   0 ;   i   <   4 ;   i + + ) 
 
                         { 
 
                                 c x [ i ]   =   i _ r e f _ v e r t e x _ 2 d [ i ] . x ; 
 
                                 c y [ i ]   =   i _ r e f _ v e r t e x _ 2 d [ i ] . y ; 
 
                                 i n t   x 2   =   i   *   2 ; 
 
                                 m a t a [ x 2 ] [ 2 ]   =   m a t a t [ 2 ] [ x 2 ]   =   c p a r a 0 2   -   i _ r e f _ v e r t e x _ 2 d [ i ] . x ; / /   m a t _ a - > m [ j * 6 + 2 ] = m a t _ b - > m [ n u m * 4 + j * 2 ] = c p a r a [ 0 ] [ 2 ] - p o s 2 d [ j ] [ 0 ] ; 
 
                                 m a t a [ x 2   +   1 ] [ 2 ]   =   m a t a t [ 2 ] [ x 2   +   1 ]   =   c p a r a 1 2   -   i _ r e f _ v e r t e x _ 2 d [ i ] . y ; / /   m a t _ a - > m [ j * 6 + 5 ] = m a t _ b - > m [ n u m * 4 + j * 2 + 1 ] = c p a r a [ 1 ] [ 2 ] - p o s 2 d [ j ] [ 1 ] ; 
 
                         } 
 
                         / / T ( 3 x 3 LR) n0\Ob
 
                         m a t _ t . m u l ( t h i s . _ m a t _ a t ,   t h i s . _ m a t _ a ) ; 
 
                         m a t _ t . i n v e r s e ( ) ; 
 
                         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 ) 
 
                 { 
 
                         d o u b l e [ ] [ ]   m a t c   =   t h i s . _ m a t _ c . g e t A r r a y ( ) ; 
 
                         d o u b l e   c p a r a 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   c p a r a 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   c p a r a 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   c p a r a 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   c p a r a 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 [ ]   c x   =   t h i s . _ c x ; 
 
                         d o u b l e [ ]   c y   =   t h i s . _ c y ; 
 
 
 
                         / / 3 D ^j	0 NbˊBl
 
                         f o r   ( i n t   i   =   0 ;   i   <   4 ;   i + + ) 
 
                         { 
 
                                 i n t   x 2   =   i   +   i ; 
 
                                 N y A R D o u b l e P o i n t 3 d   p o i n t 3 d _ p t r   =   i _ v e r t e x 3 d [ i ] ; 
 
                                 / / 	Yc
 
                                 m a t c [ x 2 ] [ 0 ]   =   p o i n t 3 d _ p t r . z   *   c x [ i ]   -   c p a r a 0 0   *   p o i n t 3 d _ p t r . x   -   c p a r a 0 1   *   p o i n t 3 d _ p t r . y   -   c p a r a 0 2   *   p o i n t 3 d _ p t r . z ; / /   m a t _ c - > m [ j * 2 + 0 ]   =   w z * p o s 2 d [ j ] [ 0 ] - c p a r a [ 0 ] [ 0 ] * w x - c p a r a [ 0 ] [ 1 ] * w y - c p a r a [ 0 ] [ 2 ] * w z ; 
 
                                 m a t c [ x 2   +   1 ] [ 0 ]   =   p o i n t 3 d _ p t r . z   *   c y [ i ]   -   c p a r a 1 1   *   p o i n t 3 d _ p t r . y   -   c p a r a 1 2   *   p o i n t 3 d _ p t r . z ; / /   m a t _ c - > m [ j * 2 + 1 ] =   w z * p o s 2 d [ j ] [ 1 ] - c p a r a [ 1 ] [ 1 ] * w y - c p a r a [ 1 ] [ 2 ] * w z ; 
 
                         } 
 
                         t h i s . _ m a t _ e . m u l ( t h i s . _ m a t _ a t ,   t h i s . _ m a t _ c ) ; 
 
                         t h i s . _ m a t _ f . m u l ( t h i s . _ m a t _ t ,   t h i s . _ m a t _ e ) ; 
 
 
 
                         d o u b l e [ ] [ ]   m a t f   =   t h i s . _ m a t _ f . g e t A r r a y ( ) ; 
 
                         o _ t r a n s f e r . x   =   m a t f [ 0 ] [ 0 ] ; / /   t r a n s [ 0 ]   =   m a t _ f - > m [ 0 ] ; 
 
                         o _ t r a n s f e r . y   =   m a t f [ 1 ] [ 0 ] ; 
 
                         o _ t r a n s f e r . z   =   m a t f [ 2 ] [ 0 ] ; / /   t r a n s [ 2 ]   =   m a t _ f - > m [ 2 ] ; 
 
                         r e t u r n ; 
 
                 } 
 
         } 
 
 } 
 
 