/ *   
 
   *   P R O J E C T :   N y A R T o o l k i t 
 
   *   - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 
 
   *   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   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{ @ l i n k   N y A R R o t M a t r i x } 000k000000( v}) K00VLR0{Y00_j0RW0~0Y00
 
           *   8^0000L0S0n00000OF0S0h0o0B00~0[000{ @ l i n k   N y A R R o t M a t r i x } 000K00OD0~0Y00
 
           * / 
 
         p u b l i c   c l a s s   N y A R R o t V e c t o r V 2   :   N y A R R o t V e c t o r 
 
         { 
 
                 / / p r i v a t e 000T	 
 
                 p r i v a t 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   _ p r o j e c t i o n _ m a t _ r e f ; 
 
                 p r i v a t e   N y A R D o u b l e M a t r i x 4 4   _ i n v _ c p a r a   =   n e w   N y A R D o u b l e M a t r i x 4 4 ( ) ; 
 
 
 
                 / * * 
 
                   *   0000000g0Y00
 
                   *   \q_	Yc000000n0Sgq$P0-[W0f000000000\ObW0~0Y00
 
                   *   @ p a r a m   i _ c m a t 
 
                   *   \q_	Yc0000000S0n0$Po0000000n0uX[-No0	YfW0j0D0g0O0`0U0D00
 
                   *   @ t h r o w s   N y A R E x c e p t i o n 
 
                   * / 
 
                 p u b l i c   N y A R R o t V e c t o r V 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 _ c m a t ) : b a s e ( ) 
 
                 { 
 
                         t h i s . _ i n v _ c p a r a . i n v e r s e ( i _ c m a t ) ; 
 
                         t h i s . _ p r o j e c t i o n _ m a t _ r e f   =   i _ c m a t ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   S0n0peo00v}k0vNY0000000{W0f00]0n0!kCQ00000000000k0<h
}W0~0Y00
 
                   *   YR	
 
                   *   @ p a r a m   i _ l i n e a r 1 
 
                   *   v}
 
                   *   @ p a r a m   i _ l i n e a r 2 
 
                   *   v}
 
                   * / 
 
                 p u b l i c   v o i d   e x t e r i o r P r o d u c t F r o m L i n e a r ( N y A R L i n e a r   i _ l i n e a r 1 ,   N y A R L i n e a r   i _ l i n e a r 2 ) 
 
                 { 
 
                         / / 1 Lv
 
                         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 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   w 1   =   i _ l i n e a r 1 . a   *   i _ l i n e a r 2 . b   -   i _ l i n e a r 2 . a   *   i _ l i n e a r 1 . b ; 
 
                         d o u b l e   w 2   =   i _ l i n e a r 1 . b   *   i _ l i n e a r 2 . c   -   i _ l i n e a r 2 . b   *   i _ l i n e a r 1 . c ; 
 
                         d o u b l e   w 3   =   i _ l i n e a r 1 . c   *   i _ l i n e a r 2 . a   -   i _ l i n e a r 2 . c   *   i _ l i n e a r 1 . a ; 
 
 
 
                         d o u b l e   m 0   =   w 1   *   ( c m a t . m 0 1   *   c m a t . m 1 2   -   c m a t . m 0 2   *   c m a t . m 1 1 )   +   w 2   *   c m a t . m 1 1   -   w 3   *   c m a t . m 0 1 ; / / w 1   *   ( c p a r a [ 0   *   4   +   1 ]   *   c p a r a [ 1   *   4   +   2 ]   -   c p a r a [ 0   *   4   +   2 ]   *   c p a r a [ 1   *   4   +   1 ] )   +   w 2   *   c p a r a [ 1   *   4   +   1 ]   -   w 3   *   c p a r a [ 0   *   4   +   1 ] ; 
 
                         d o u b l e   m 1   =   - w 1   *   c m a t . m 0 0   *   c m a t . m 1 2   +   w 3   *   c m a t . m 0 0 ; / / - w 1   *   c p a r a [ 0   *   4   +   0 ]   *   c p a r a [ 1   *   4   +   2 ]   +   w 3   *   c p a r a [ 0   *   4   +   0 ] ; 
 
                         d o u b l e   m 2   =   w 1   *   c m a t . m 0 0   *   c m a t . m 1 1 ; / / w 1   *   c p a r a [ 0   *   4   +   0 ]   *   c p a r a [ 1   *   4   +   1 ] ; 
 
                         d o u b l e   w   =   M a t h . S q r t ( m 0   *   m 0   +   m 1   *   m 1   +   m 2   *   m 2 ) ; 
 
                         t h i s . v 1   =   m 0   /   w ; 
 
                         t h i s . v 2   =   m 1   /   w ; 
 
                         t h i s . v 3   =   m 2   /   w ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo00A R T o o l K i t n0c h e c k _ d i r pek0vS_W0~0Y00
 
                   *   s0}o0
Nfg0Y00( 0000n0Y/ B}N^j0c[W0f000000n0eT0te) 
 
                   *   @ p a r a m   i _ s t a r t _ v e r t e x 
 
                   *   YMOn
 
                   *   @ p a r a m   i _ e n d _ v e r t e x 
 
                   *   B}NMOn
 
                   *   @ t h r o w s   N y A R E x c e p t i o n 
 
                   * / 
 
                 p u b l i c   b o o l   c h e c k V e c t o r B y V e r t e x ( N y A R D o u b l e P o i n t 2 d   i _ s t a r t _ v e r t e x ,   N y A R D o u b l e P o i n t 2 d   i _ e n d _ v e r t e x ) 
 
                 { 
 
                         d o u b l e   h ; 
 
                         N y A R D o u b l e M a t r i x 4 4   i n v _ c p a r a   =   t h i s . _ i n v _ c p a r a ; 
 
                         / / f i n a l   d o u b l e [ ]   w o r l d   =   _ _ c h e c k V e c t o r B y V e r t e x _ w o r l d ; / /   [ 2 ] [ 3 ] ; 
 
                         d o u b l e   w o r l d 0   =   i n v _ c p a r a . m 0 0   *   i _ s t a r t _ v e r t e x . x   *   1 0 . 0   +   i n v _ c p a r a . m 0 1   *   i _ s t a r t _ v e r t e x . y   *   1 0 . 0   +   i n v _ c p a r a . m 0 2   *   1 0 . 0 ; / /   m a t _ a - > m [ 0 ] * s t [ 0 ] * 1 0 . 0 + 
 
                         d o u b l e   w o r l d 1   =   i n v _ c p a r a . m 1 0   *   i _ s t a r t _ v e r t e x . x   *   1 0 . 0   +   i n v _ c p a r a . m 1 1   *   i _ s t a r t _ v e r t e x . y   *   1 0 . 0   +   i n v _ c p a r a . m 1 2   *   1 0 . 0 ; / /   m a t _ a - > m [ 3 ] * s t [ 0 ] * 1 0 . 0 + 
 
                         d o u b l e   w o r l d 2   =   i n v _ c p a r a . m 2 0   *   i _ s t a r t _ v e r t e x . x   *   1 0 . 0   +   i n v _ c p a r a . m 2 1   *   i _ s t a r t _ v e r t e x . y   *   1 0 . 0   +   i n v _ c p a r a . m 2 2   *   1 0 . 0 ; / /   m a t _ a - > m [ 6 ] * s t [ 0 ] * 1 0 . 0 + 
 
                         d o u b l e   w o r l d 3   =   w o r l d 0   +   t h i s . v 1 ; 
 
                         d o u b l e   w o r l d 4   =   w o r l d 1   +   t h i s . v 2 ; 
 
                         d o u b l e   w o r l d 5   =   w o r l d 2   +   t h i s . v 3 ; 
 
                         / /   < / 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   c m a t   =   t h i s . _ p r o j e c t i o n _ m a t _ r e f ; 
 
                         h   =   c m a t . m 2 0   *   w o r l d 0   +   c m a t . m 2 1   *   w o r l d 1   +   c m a t . m 2 2   *   w o r l d 2 ; 
 
                         i f   ( h   = =   0 . 0 ) 
 
                         { 
 
                                 r e t u r n   f a l s e ; 
 
                         } 
 
                         d o u b l e   c a m e r a 0   =   ( c m a t . m 0 0   *   w o r l d 0   +   c m a t . m 0 1   *   w o r l d 1   +   c m a t . m 0 2   *   w o r l d 2 )   /   h ; 
 
                         d o u b l e   c a m e r a 1   =   ( c m a t . m 1 0   *   w o r l d 0   +   c m a t . m 1 1   *   w o r l d 1   +   c m a t . m 1 2   *   w o r l d 2 )   /   h ; 
 
 
 
                         / / h   =   c p a r a [ 2   *   4   +   0 ]   *   w o r l d 3   +   c p a r a [ 2   *   4   +   1 ]   *   w o r l d 4   +   c p a r a [ 2   *   4   +   2 ]   *   w o r l d 5 ; 
 
                         h   =   c m a t . m 2 0   *   w o r l d 3   +   c m a t . m 2 1   *   w o r l d 4   +   c m a t . m 2 2   *   w o r l d 5 ; 
 
                         i f   ( h   = =   0 . 0 ) 
 
                         { 
 
                                 r e t u r n   f a l s e ; 
 
                         } 
 
                         d o u b l e   c a m e r a 2   =   ( c m a t . m 0 0   *   w o r l d 3   +   c m a t . m 0 1   *   w o r l d 4   +   c m a t . m 0 2   *   w o r l d 5 )   /   h ; 
 
                         d o u b l e   c a m e r a 3   =   ( c m a t . m 1 0   *   w o r l d 3   +   c m a t . m 1 1   *   w o r l d 4   +   c m a t . m 1 2   *   w o r l d 5 )   /   h ; 
 
 
 
                         d o u b l e   v   =   ( i _ e n d _ v e r t e x . x   -   i _ s t a r t _ v e r t e x . x )   *   ( c a m e r a 2   -   c a m e r a 0 )   +   ( i _ e n d _ v e r t e x . y   -   i _ s t a r t _ v e r t e x . y )   *   ( c a m e r a 3   -   c a m e r a 1 ) ; 
 
                         i f   ( v   <   0 ) 
 
                         { 
 
                                 t h i s . v 1   =   - t h i s . v 1 ; 
 
                                 t h i s . v 2   =   - t h i s . v 2 ; 
 
                                 t h i s . v 3   =   - t h i s . v 3 ; 
 
                         } 
 
                         r e t u r n   t r u e ; 
 
                 } 
 
         } 
 
 } 