/ *   
 
   *   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0A R T o o l K i t b__n0	YcLR0<h
}W0~0Y00
 
           *   	Ycpeh00\q_	YcLRn0ubpe0cOW0~0Y00
 
           *   S0n0000o0{ @ l i n k   N y A R P a r a m } k0@b	gU000S0h00MRck0W0f0J000XSrn0Nio0naU00f0D0~0[000
 
           *   < p > 000000  - 
 
           *   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 / P a p e r s / A R T 0 2 - T u t o r i a l . p d f n07 0000Sgq0
 
           *   < / p > 
 
           * / 
 
         s e a l e d   p u b l i c   c l a s s   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   :   N y A R D o u b l e M a t r i x 4 4 
 
         { 
 
                 / * * 
 
                   *   0000000g0Y00zzn0LR0ubW0~0Y00
 
                   * / 
 
                 p u b l i c   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 ( ) 
 
                 { 
 
                         t h i s . m 3 0   =   t h i s . m 3 1   =   t h i s . m 3 2   =   0 ; 
 
                         t h i s . m 3 3   =   1 ; 
 
                 } 
 
                 / * 
 
                   *   s t a t i c   d o u b l e   d o t (   d o u b l e   a 1 ,   d o u b l e   a 2 ,   d o u b l e   a 3 , d o u b l e   b 1 ,   d o u b l e   b 2 , d o u b l e   b 3   ) 
 
                   * / 
 
                 p r i v a t e   s t a t i c   d o u b l e   d o t ( d o u b l e   a 1 ,   d o u b l e   a 2 ,   d o u b l e   a 3 ,   d o u b l e   b 1 ,   d o u b l e   b 2 ,   d o u b l e   b 3 ) 
 
                 { 
 
                         r e t u r n   ( a 1   *   b 1   +   a 2   *   b 2   +   a 3   *   b 3 ) ; 
 
                 } 
 
 
 
                 / *   s t a t i c   d o u b l e   n o r m (   d o u b l e   a ,   d o u b l e   b ,   d o u b l e   c   )   * / 
 
                 p r i v a t e   s t a t i c   d o u b l e   n o r m ( d o u b l e   a ,   d o u b l e   b ,   d o u b l e   c ) 
 
                 { 
 
                         r e t u r n   M a t h . S q r t ( a   *   a   +   b   *   b   +   c   *   c ) ; 
 
                 } 
 
                 / * * 
 
                   *   S0n0peo00A R T o o l K i t n0a r P a r a m D e c o m p M a t h0TX0g0Y00
 
                   *   R\Oo00O00K00~0[00& 0
 
                   *   @ p a r a m   o _ c p a r a 
 
                   *   s0}
Nf03 x 4 n0000000c[Y00S0h00
 
                   *   @ p a r a m   o _ t r a n s 
 
                   *   s0}
Nf03 x 4 n0000000c[Y00S0h00
 
                   * / 
 
                 p u b l i c   v o i d   d e c o m p M a t ( N y A R M a t   o _ c p a r a ,   N y A R M a t   o _ t r a n s ) 
 
                 { 
 
                         d o u b l e   r e m 1 ,   r e m 2 ,   r e m 3 ; 
 
                         d o u b l e   c 0 0 ,   c 0 1 ,   c 0 2 ,   c 0 3 ,   c 1 0 ,   c 1 1 ,   c 1 2 ,   c 1 3 ,   c 2 0 ,   c 2 1 ,   c 2 2 ,   c 2 3 ; 
 
                         i f   ( t h i s . m 2 3   > =   0 ) 
 
                         { / /   i f (   s o u r c e [ 2 ] [ 3 ]   > =   0   )   { 
 
                                 c 0 0   =   t h i s . m 0 0 ; 
 
                                 c 0 1   =   t h i s . m 0 1 ; 
 
                                 c 0 2   =   t h i s . m 0 2 ; 
 
                                 c 0 3   =   t h i s . m 0 3 ; 
 
                                 c 1 0   =   t h i s . m 1 0 ; 
 
                                 c 1 1   =   t h i s . m 1 1 ; 
 
                                 c 1 2   =   t h i s . m 1 2 ; 
 
                                 c 1 3   =   t h i s . m 1 3 ; 
 
                                 c 2 0   =   t h i s . m 2 0 ; 
 
                                 c 2 1   =   t h i s . m 2 1 ; 
 
                                 c 2 2   =   t h i s . m 2 2 ; 
 
                                 c 2 3   =   t h i s . m 2 3 ; 
 
                         } 
 
                         e l s e 
 
                         { 
 
                                 / /   < O p t i m i z e > 
 
                                 / /   f o r ( i n t   r   =   0 ;   r   <   3 ;   r + +   ) { 
 
                                 / /   f o r ( i n t   c   =   0 ;   c   <   4 ;   c + +   ) { 
 
                                 / /   C p a r a [ r ] [ c ] = - s o u r c e [ r ] [ c ] ; / / C p a r a [ r ] [ c ]   =   - ( s o u r c e [ r ] [ c ] ) ; 
 
                                 / /   } 
 
                                 / /   } 
 
                                 c 0 0   =   - t h i s . m 0 0 ; 
 
                                 c 0 1   =   - t h i s . m 0 1 ; 
 
                                 c 0 2   =   - t h i s . m 0 2 ; 
 
                                 c 0 3   =   - t h i s . m 0 3 ; 
 
                                 c 1 0   =   - t h i s . m 1 0 ; 
 
                                 c 1 1   =   - t h i s . m 1 1 ; 
 
                                 c 1 2   =   - t h i s . m 1 2 ; 
 
                                 c 1 3   =   - t h i s . m 1 3 ; 
 
                                 c 2 0   =   - t h i s . m 2 0 ; 
 
                                 c 2 1   =   - t h i s . m 2 1 ; 
 
                                 c 2 2   =   - t h i s . m 2 2 ; 
 
                                 c 2 3   =   - t h i s . m 2 3 ; 
 
                         } 
 
 
 
                         d o u b l e [ ] [ ]   c p a r a   =   o _ c p a r a . g e t A r r a y ( ) ; 
 
                         d o u b l e [ ] [ ]   t r a n s   =   o _ t r a n s . g e t A r r a y ( ) ; 
 
                         f o r   ( i n t   r   =   0 ;   r   <   3 ;   r + + ) 
 
                         { 
 
                                 f o r   ( i n t   c   =   0 ;   c   <   4 ;   c + + ) 
 
                                 { 
 
                                         c p a r a [ r ] [ c ]   =   0 . 0 ; / /   c p a r a [ r ] [ c ]   =   0 . 0 ; 
 
                                 } 
 
                         } 
 
                         c p a r a [ 2 ] [ 2 ]   =   n o r m ( c 2 0 ,   c 2 1 ,   c 2 2 ) ; / /   c p a r a [ 2 ] [ 2 ]   = n o r m (   C p a r a [ 2 ] [ 0 ] , C p a r a [ 2 ] [ 1 ] , C p a r a [ 2 ] [ 2 ] ) ; 
 
                         t r a n s [ 2 ] [ 0 ]   =   c 2 0   /   c p a r a [ 2 ] [ 2 ] ; / /   t r a n s [ 2 ] [ 0 ]   =   C p a r a [ 2 ] [ 0 ]   / c p a r a [ 2 ] [ 2 ] ; 
 
                         t r a n s [ 2 ] [ 1 ]   =   c 2 1   /   c p a r a [ 2 ] [ 2 ] ; / /   t r a n s [ 2 ] [ 1 ]   =   C p a r a [ 2 ] [ 1 ]   /   c p a r a [ 2 ] [ 2 ] ; 
 
                         t r a n s [ 2 ] [ 2 ]   =   c 2 2   /   c p a r a [ 2 ] [ 2 ] ; / /   t r a n s [ 2 ] [ 2 ]   = C p a r a [ 2 ] [ 2 ]   / c p a r a [ 2 ] [ 2 ] ; 
 
                         t r a n s [ 2 ] [ 3 ]   =   c 2 3   /   c p a r a [ 2 ] [ 2 ] ; / /   t r a n s [ 2 ] [ 3 ]   = C p a r a [ 2 ] [ 3 ]   / c p a r a [ 2 ] [ 2 ] ; 
 
 
 
                         c p a r a [ 1 ] [ 2 ]   =   d o t ( t r a n s [ 2 ] [ 0 ] ,   t r a n s [ 2 ] [ 1 ] ,   t r a n s [ 2 ] [ 2 ] ,   c 1 0 ,   c 1 1 ,   c 1 2 ) ; / /   c p a r a [ 1 ] [ 2 ] = d o t ( t r a n s [ 2 ] [ 0 ] , t r a n s [ 2 ] [ 1 ] , t r a n s [ 2 ] [ 2 ] , C p a r a [ 1 ] [ 0 ] , C p a r a [ 1 ] [ 1 ] , C p a r a [ 1 ] [ 2 ] ) ; 
 
                         r e m 1   =   c 1 0   -   c p a r a [ 1 ] [ 2 ]   *   t r a n s [ 2 ] [ 0 ] ; / /   r e m 1   = C p a r a [ 1 ] [ 0 ]   - c p a r a [ 1 ] [ 2 ]   * t r a n s [ 2 ] [ 0 ] ; 
 
                         r e m 2   =   c 1 1   -   c p a r a [ 1 ] [ 2 ]   *   t r a n s [ 2 ] [ 1 ] ; / /   r e m 2   = C p a r a [ 1 ] [ 1 ]   - c p a r a [ 1 ] [ 2 ]   * t r a n s [ 2 ] [ 1 ] ; 
 
                         r e m 3   =   c 1 2   -   c p a r a [ 1 ] [ 2 ]   *   t r a n s [ 2 ] [ 2 ] ; / /   r e m 3   = C p a r a [ 1 ] [ 2 ]   - c p a r a [ 1 ] [ 2 ]   * t r a n s [ 2 ] [ 2 ] ; 
 
                         c p a r a [ 1 ] [ 1 ]   =   n o r m ( r e m 1 ,   r e m 2 ,   r e m 3 ) ; / /   c p a r a [ 1 ] [ 1 ]   =   n o r m (   r e m 1 , / /   r e m 2 ,   r e m 3   ) ; 
 
                         t r a n s [ 1 ] [ 0 ]   =   r e m 1   /   c p a r a [ 1 ] [ 1 ] ; / /   t r a n s [ 1 ] [ 0 ]   =   r e m 1   /   c p a r a [ 1 ] [ 1 ] ; 
 
                         t r a n s [ 1 ] [ 1 ]   =   r e m 2   /   c p a r a [ 1 ] [ 1 ] ; / /   t r a n s [ 1 ] [ 1 ]   =   r e m 2   /   c p a r a [ 1 ] [ 1 ] ; 
 
                         t r a n s [ 1 ] [ 2 ]   =   r e m 3   /   c p a r a [ 1 ] [ 1 ] ; / /   t r a n s [ 1 ] [ 2 ]   =   r e m 3   /   c p a r a [ 1 ] [ 1 ] ; 
 
 
 
                         c p a r a [ 0 ] [ 2 ]   =   d o t ( t r a n s [ 2 ] [ 0 ] ,   t r a n s [ 2 ] [ 1 ] ,   t r a n s [ 2 ] [ 2 ] ,   c 0 0 ,   c 0 1 ,   c 0 2 ) ; / /   c p a r a [ 0 ] [ 2 ]   = d o t ( t r a n s [ 2 ] [ 0 ] ,   t r a n s [ 2 ] [ 1 ] , t r a n s [ 2 ] [ 2 ] , C p a r a [ 0 ] [ 0 ] , C p a r a [ 0 ] [ 1 ] , C p a r a [ 0 ] [ 2 ] ) ; 
 
                         c p a r a [ 0 ] [ 1 ]   =   d o t ( t r a n s [ 1 ] [ 0 ] ,   t r a n s [ 1 ] [ 1 ] ,   t r a n s [ 1 ] [ 2 ] ,   c 0 0 ,   c 0 1 ,   c 0 2 ) ; / /   c p a r a [ 0 ] [ 1 ] = d o t ( t r a n s [ 1 ] [ 0 ] , t r a n s [ 1 ] [ 1 ] , t r a n s [ 1 ] [ 2 ] , C p a r a [ 0 ] [ 0 ] , C p a r a [ 0 ] [ 1 ] , C p a r a [ 0 ] [ 2 ] ) ; 
 
                         r e m 1   =   c 0 0   -   c p a r a [ 0 ] [ 1 ]   *   t r a n s [ 1 ] [ 0 ]   -   c p a r a [ 0 ] [ 2 ]   *   t r a n s [ 2 ] [ 0 ] ; / /   r e m 1   =   C p a r a [ 0 ] [ 0 ]   -   c p a r a [ 0 ] [ 1 ] * t r a n s [ 1 ] [ 0 ] -   c p a r a [ 0 ] [ 2 ] * t r a n s [ 2 ] [ 0 ] ; 
 
                         r e m 2   =   c 0 1   -   c p a r a [ 0 ] [ 1 ]   *   t r a n s [ 1 ] [ 1 ]   -   c p a r a [ 0 ] [ 2 ]   *   t r a n s [ 2 ] [ 1 ] ; / /   r e m 2   =   C p a r a [ 0 ] [ 1 ]   -   c p a r a [ 0 ] [ 1 ] * t r a n s [ 1 ] [ 1 ] -   c p a r a [ 0 ] [ 2 ] * t r a n s [ 2 ] [ 1 ] ; 
 
                         r e m 3   =   c 0 2   -   c p a r a [ 0 ] [ 1 ]   *   t r a n s [ 1 ] [ 2 ]   -   c p a r a [ 0 ] [ 2 ]   *   t r a n s [ 2 ] [ 2 ] ; / /   r e m 3   =   C p a r a [ 0 ] [ 2 ]   -   c p a r a [ 0 ] [ 1 ] * t r a n s [ 1 ] [ 2 ]   -   c p a r a [ 0 ] [ 2 ] * t r a n s [ 2 ] [ 2 ] ; 
 
                         c p a r a [ 0 ] [ 0 ]   =   n o r m ( r e m 1 ,   r e m 2 ,   r e m 3 ) ; / /   c p a r a [ 0 ] [ 0 ]   =   n o r m (   r e m 1 , r e m 2 ,   r e m 3   ) ; 
 
                         t r a n s [ 0 ] [ 0 ]   =   r e m 1   /   c p a r a [ 0 ] [ 0 ] ; / /   t r a n s [ 0 ] [ 0 ]   =   r e m 1   /   c p a r a [ 0 ] [ 0 ] ; 
 
                         t r a n s [ 0 ] [ 1 ]   =   r e m 2   /   c p a r a [ 0 ] [ 0 ] ; / /   t r a n s [ 0 ] [ 1 ]   =   r e m 2   /   c p a r a [ 0 ] [ 0 ] ; 
 
                         t r a n s [ 0 ] [ 2 ]   =   r e m 3   /   c p a r a [ 0 ] [ 0 ] ; / /   t r a n s [ 0 ] [ 2 ]   =   r e m 3   /   c p a r a [ 0 ] [ 0 ] ; 
 
 
 
                         t r a n s [ 1 ] [ 3 ]   =   ( c 1 3   -   c p a r a [ 1 ] [ 2 ]   *   t r a n s [ 2 ] [ 3 ] )   /   c p a r a [ 1 ] [ 1 ] ; / /   t r a n s [ 1 ] [ 3 ]   =   ( C p a r a [ 1 ] [ 3 ]   - c p a r a [ 1 ] [ 2 ] * t r a n s [ 2 ] [ 3 ] )   /   c p a r a [ 1 ] [ 1 ] ; 
 
                         t r a n s [ 0 ] [ 3 ]   =   ( c 0 3   -   c p a r a [ 0 ] [ 1 ]   *   t r a n s [ 1 ] [ 3 ]   -   c p a r a [ 0 ] [ 2 ]   *   t r a n s [ 2 ] [ 3 ] )   /   c p a r a [ 0 ] [ 0 ] ; / /   t r a n s [ 0 ] [ 3 ]   =   ( C p a r a [ 0 ] [ 3 ]   - c p a r a [ 0 ] [ 1 ] * t r a n s [ 1 ] [ 3 ] - c p a r a [ 0 ] [ 2 ] * t r a n s [ 2 ] [ 3 ] )   /   c p a r a [ 0 ] [ 0 ] ; 
 
 
 
                         f o r   ( i n t   r   =   0 ;   r   <   3 ;   r + + ) 
 
                         { 
 
                                 f o r   ( i n t   c   =   0 ;   c   <   3 ;   c + + ) 
 
                                 { 
 
                                         c p a r a [ r ] [ c ]   / =   c p a r a [ 2 ] [ 2 ] ; / /   c p a r a [ r ] [ c ]   / =   c p a r a [ 2 ] [ 2 ] ; 
 
                                 } 
 
                         } 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   LRk00000$P0Mz{W0~0Y00
 
                   *   @ p a r a m   i _ s c a l e 
 
                   *   0000$P
 
                   * / 
 
                 p u b l i c   v o i d   c h a n g e S c a l e ( d o u b l e   i _ x _ s c a l e ,   d o u b l e   i _ y _ s c a l e ) 
 
                 { 
 
                         t h i s . m 0 0   =   t h i s . m 0 0   *   i _ x _ s c a l e ; 
 
                         t h i s . m 1 0   =   t h i s . m 1 0   *   i _ y _ s c a l e ; 
 
                         t h i s . m 0 1   =   t h i s . m 0 1   *   i _ x _ s c a l e ; 
 
                         t h i s . m 1 1   =   t h i s . m 1 1   *   i _ y _ s c a l e ; 
 
                         t h i s . m 0 2   =   t h i s . m 0 2   *   i _ x _ s c a l e ; 
 
                         t h i s . m 1 2   =   t h i s . m 1 2   *   i _ y _ s c a l e ; 
 
                         t h i s . m 0 3   =   t h i s . m 0 3   *   i _ x _ s c a l e ; 
 
                         t h i s . m 1 3   =   t h i s . m 1 3   *   i _ y _ s c a l e ; 
 
                         / / f o r   ( i n t   i   =   0 ;   i   <   4 ;   i + + )   { 
 
                         / / 	 a r r a y 3 4 [ 0   *   4   +   i ]   =   a r r a y 3 4 [ 0   *   4   +   i ]   *   s c a l e ; / /   n e w p a r a m - > m a t [ 0 ] [ i ] = s o u r c e - > m a t [ 0 ] [ i ] *   s c a l e ; 
 
                         / / 	 a r r a y 3 4 [ 1   *   4   +   i ]   =   a r r a y 3 4 [ 1   *   4   +   i ]   *   s c a l e ; / /   n e w p a r a m - > m a t [ 1 ] [ i ] = s o u r c e - > m a t [ 1 ] [ i ] *   s c a l e ; 
 
                         / / 	 a r r a y 3 4 [ 2   *   4   +   i ]   =   a r r a y 3 4 [ 2   *   4   +   i ] ; / /   n e w p a r a m - > m a t [ 2 ] [ i ]   =   s o u r c e - > m a t [ 2 ] [ i ] ; 
 
                         / / } 
 
                         r e t u r n ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   ^j$P0\q_	YcW0~0Y00
 
                   *   @ p a r a m   i _ 3 d v e r t e x 
 
                   *   	YcCQn0^j$P
 
                   *   @ p a r a m   o _ 2 d 
 
                   *   	Yc_n0^j$P0SQ0S0000000
 
                   * / 
 
                 p u b l i c   v o i d   p r o j e c t ( N y A R D o u b l e P o i n t 3 d   i _ 3 d v e r t e x ,   N y A R D o u b l e P o i n t 2 d   o _ 2 d ) 
 
                 { 
 
                         d o u b l e   w   =   1   /   ( i _ 3 d v e r t e x . z   *   t h i s . m 2 2 ) ; 
 
                         o _ 2 d . x   =   ( i _ 3 d v e r t e x . x   *   t h i s . m 0 0   +   i _ 3 d v e r t e x . y   *   t h i s . m 0 1   +   i _ 3 d v e r t e x . z   *   t h i s . m 0 2 )   *   w ; 
 
                         o _ 2 d . y   =   ( i _ 3 d v e r t e x . y   *   t h i s . m 1 1   +   i _ 3 d v e r t e x . z   *   t h i s . m 1 2 )   *   w ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   ^j$P0\q_	YcW0~0Y00
 
                   *   @ p a r a m   i _ x 
 
                   *   	YcCQn0^j$P
 
                   *   @ p a r a m   i _ y 
 
                   *   	YcCQn0^j$P
 
                   *   @ p a r a m   i _ z 
 
                   *   	YcCQn0^j$P
 
                   *   @ p a r a m   o _ 2 d 
 
                   *   	Yc_n0^j$P0SQ0S0000000
 
                   * / 
 
                 p u b l i c   v o i d   p r o j e c t ( d o u b l e   i _ x ,   d o u b l e   i _ y ,   d o u b l e   i _ z ,   N y A R D o u b l e P o i n t 2 d   o _ 2 d ) 
 
                 { 
 
                         d o u b l e   w   =   1   /   ( i _ z   *   t h i s . m 2 2 ) ; 
 
                         o _ 2 d . x   =   ( i _ x   *   t h i s . m 0 0   +   i _ y   *   t h i s . m 0 1   +   i _ z   *   t h i s . m 0 2 )   *   w ; 
 
                         o _ 2 d . y   =   ( i _ y   *   t h i s . m 1 1   +   i _ z   *   t h i s . m 1 2 )   *   w ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   ^j$P0\q_	YcW0~0Y00
 
                   *   @ p a r a m   i _ 3 d v e r t e x 
 
                   *   	YcCQn0^j$P
 
                   *   @ p a r a m   o _ 2 d 
 
                   *   	Yc_n0^j$P0SQ0S0000000
 
                   * / 
 
                 p u b l i c   v o i d   p r o j e c t ( N y A R D o u b l e P o i n t 3 d   i _ 3 d v e r t e x ,   N y A R I n t P o i n t 2 d   o _ 2 d ) 
 
                 { 
 
                         d o u b l e   w   =   1   /   ( i _ 3 d v e r t e x . z   *   t h i s . m 2 2 ) ; 
 
                         o _ 2 d . x   =   ( i n t ) ( ( i _ 3 d v e r t e x . x   *   t h i s . m 0 0   +   i _ 3 d v e r t e x . y   *   t h i s . m 0 1   +   i _ 3 d v e r t e x . z   *   t h i s . m 0 2 )   *   w ) ; 
 
                         o _ 2 d . y   =   ( i n t ) ( ( i _ 3 d v e r t e x . y   *   t h i s . m 1 1   +   i _ 3 d v e r t e x . z   *   t h i s . m 1 2 )   *   w ) ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   ^j$P0\q_	YcW0~0Y00
 
                   *   @ p a r a m   i _ x 
 
                   *   	YcCQn0^j$P
 
                   *   @ p a r a m   i _ y 
 
                   *   	YcCQn0^j$P
 
                   *   @ p a r a m   i _ z 
 
                   *   	YcCQn0^j$P
 
                   *   @ p a r a m   o _ 2 d 
 
                   *   	Yc_n0^j$P0SQ0S0000000
 
                   * / 
 
                 p u b l i c   v o i d   p r o j e c t ( d o u b l e   i _ x ,   d o u b l e   i _ y ,   d o u b l e   i _ z ,   N y A R I n t P o i n t 2 d   o _ 2 d ) 
 
                 { 
 
                         d o u b l e   w   =   1   /   ( i _ z   *   t h i s . m 2 2 ) ; 
 
                         o _ 2 d . x   =   ( i n t ) ( ( i _ x   *   t h i s . m 0 0   +   i _ y   *   t h i s . m 0 1   +   i _ z   *   t h i s . m 0 2 )   *   w ) ; 
 
                         o _ 2 d . y   =   ( i n t ) ( ( i _ y   *   t h i s . m 1 1   +   i _ z   *   t h i s . m 1 2 )   *   w ) ; 
 
                         r e t u r n ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   SKb|n0S0\O0~0Y00
 
                   *   S0n0So00A R T o o l K i t n0a r g l C a m e r a V i e w R H n0\O0Sh0TX0g0Y00
 
                   *   @ p a r a m   i _ s c r e e n _ w i d t h 
 
                   *   000000000c[W0~0Y00
 
                   *   @ p a r a m   i _ s c r e e n _ h e i g h t 
 
                   *   000000000c[W0~0Y00
 
                   *   @ p a r a m   i _ d i s t _ m i n 
 
                   *   n e a r   p o i n t 0c[W0~0Y0( m m XSMO) 
 
                   *   @ p a r a m   i _ d i s t _ m a x 
 
                   *   f a r   p o i n t 0c[W0~0Y0( m m XSMO) 
 
                   *   @ p a r a m   o _ f r u s t u m 
 
                   *   Sn0<h
}HQ0000000c[W0~0Y00
 
                   * / 
 
                 p u b l i c   v o i d   m a k e C a m e r a F r u s t u m R H ( d o u b l e   i _ s c r e e n _ w i d t h ,   d o u b l e   i _ s c r e e n _ h e i g h t ,   d o u b l e   i _ d i s t _ m i n ,   d o u b l e   i _ d i s t _ m a x ,   N y A R D o u b l e M a t r i x 4 4   o _ f r u s t u m ) 
 
                 { 
 
                         N y A R M a t   t r a n s _ m a t   =   n e w   N y A R M a t ( 3 ,   4 ) ; 
 
                         N y A R M a t   i c p a r a _ m a t   =   n e w   N y A R M a t ( 3 ,   4 ) ; 
 
                         d o u b l e [ ] [ ]   p   =   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 ( 3 ,   3 ) ; 
 
                         i n t   i ; 
 
 
 
                         t h i s . d e c o m p M a t ( i c p a r a _ m a t ,   t r a n s _ m a t ) ; 
 
 
 
                         d o u b l e [ ] [ ]   i c p a r a   =   i c p a r a _ m a t . g e t A r r a y ( ) ; 
 
                         d o u b l e [ ] [ ]   t r a n s   =   t r a n s _ m a t . g e t A r r a y ( ) ; 
 
                         f o r   ( i   =   0 ;   i   <   4 ;   i + + ) 
 
                         { 
 
                                 i c p a r a [ 1 ] [ i ]   =   ( i _ s c r e e n _ h e i g h t   -   1 )   *   ( i c p a r a [ 2 ] [ i ] )   -   i c p a r a [ 1 ] [ i ] ; 
 
                         } 
 
                         p [ 0 ] [ 0 ]   =   i c p a r a [ 0 ] [ 0 ]   /   i c p a r a [ 2 ] [ 2 ] ; 
 
                         p [ 0 ] [ 1 ]   =   i c p a r a [ 0 ] [ 1 ]   /   i c p a r a [ 2 ] [ 2 ] ; 
 
                         p [ 0 ] [ 2 ]   =   i c p a r a [ 0 ] [ 2 ]   /   i c p a r a [ 2 ] [ 2 ] ; 
 
 
 
                         p [ 1 ] [ 0 ]   =   i c p a r a [ 1 ] [ 0 ]   /   i c p a r a [ 2 ] [ 2 ] ; 
 
                         p [ 1 ] [ 1 ]   =   i c p a r a [ 1 ] [ 1 ]   /   i c p a r a [ 2 ] [ 2 ] ; 
 
                         p [ 1 ] [ 2 ]   =   i c p a r a [ 1 ] [ 2 ]   /   i c p a r a [ 2 ] [ 2 ] ; 
 
 
 
                         p [ 2 ] [ 0 ]   =   i c p a r a [ 2 ] [ 0 ]   /   i c p a r a [ 2 ] [ 2 ] ; 
 
                         p [ 2 ] [ 1 ]   =   i c p a r a [ 2 ] [ 1 ]   /   i c p a r a [ 2 ] [ 2 ] ; 
 
                         p [ 2 ] [ 2 ]   =   i c p a r a [ 2 ] [ 2 ]   /   i c p a r a [ 2 ] [ 2 ] ; 
 
 
 
                         d o u b l e   q 0 0 ,   q 0 1 ,   q 0 2 ,   q 0 3 ,   q 1 0 ,   q 1 1 ,   q 1 2 ,   q 1 3 ,   q 2 0 ,   q 2 1 ,   q 2 2 ,   q 2 3 ,   q 3 0 ,   q 3 1 ,   q 3 2 ,   q 3 3 ; 
 
 
 
                         / / Sx0n0	Yc
 
                         q 0 0   =   ( 2 . 0   *   p [ 0 ] [ 0 ]   /   ( i _ s c r e e n _ w i d t h   -   1 ) ) ; 
 
                         q 0 1   =   ( 2 . 0   *   p [ 0 ] [ 1 ]   /   ( i _ s c r e e n _ w i d t h   -   1 ) ) ; 
 
                         q 0 2   =   - ( ( 2 . 0   *   p [ 0 ] [ 2 ]   /   ( i _ s c r e e n _ w i d t h   -   1 ) )   -   1 . 0 ) ; 
 
                         q 0 3   =   0 . 0 ; 
 
                         o _ f r u s t u m . m 0 0   =   q 0 0   *   t r a n s [ 0 ] [ 0 ]   +   q 0 1   *   t r a n s [ 1 ] [ 0 ]   +   q 0 2   *   t r a n s [ 2 ] [ 0 ] ; 
 
                         o _ f r u s t u m . m 0 1   =   q 0 0   *   t r a n s [ 0 ] [ 1 ]   +   q 0 1   *   t r a n s [ 1 ] [ 1 ]   +   q 0 2   *   t r a n s [ 2 ] [ 1 ] ; 
 
                         o _ f r u s t u m . m 0 2   =   q 0 0   *   t r a n s [ 0 ] [ 2 ]   +   q 0 1   *   t r a n s [ 1 ] [ 2 ]   +   q 0 2   *   t r a n s [ 2 ] [ 2 ] ; 
 
                         o _ f r u s t u m . m 0 3   =   q 0 0   *   t r a n s [ 0 ] [ 3 ]   +   q 0 1   *   t r a n s [ 1 ] [ 3 ]   +   q 0 2   *   t r a n s [ 2 ] [ 3 ]   +   q 0 3 ; 
 
 
 
                         q 1 0   =   0 . 0 ; 
 
                         q 1 1   =   - ( 2 . 0   *   p [ 1 ] [ 1 ]   /   ( i _ s c r e e n _ h e i g h t   -   1 ) ) ; 
 
                         q 1 2   =   - ( ( 2 . 0   *   p [ 1 ] [ 2 ]   /   ( i _ s c r e e n _ h e i g h t   -   1 ) )   -   1 . 0 ) ; 
 
                         q 1 3   =   0 . 0 ; 
 
                         o _ f r u s t u m . m 1 0   =   q 1 0   *   t r a n s [ 0 ] [ 0 ]   +   q 1 1   *   t r a n s [ 1 ] [ 0 ]   +   q 1 2   *   t r a n s [ 2 ] [ 0 ] ; 
 
                         o _ f r u s t u m . m 1 1   =   q 1 0   *   t r a n s [ 0 ] [ 1 ]   +   q 1 1   *   t r a n s [ 1 ] [ 1 ]   +   q 1 2   *   t r a n s [ 2 ] [ 1 ] ; 
 
                         o _ f r u s t u m . m 1 2   =   q 1 0   *   t r a n s [ 0 ] [ 2 ]   +   q 1 1   *   t r a n s [ 1 ] [ 2 ]   +   q 1 2   *   t r a n s [ 2 ] [ 2 ] ; 
 
                         o _ f r u s t u m . m 1 3   =   q 1 0   *   t r a n s [ 0 ] [ 3 ]   +   q 1 1   *   t r a n s [ 1 ] [ 3 ]   +   q 1 2   *   t r a n s [ 2 ] [ 3 ]   +   q 1 3 ; 
 
 
 
                         q 2 0   =   0 . 0 ; 
 
                         q 2 1   =   0 . 0 ; 
 
                         q 2 2   =   ( i _ d i s t _ m a x   +   i _ d i s t _ m i n )   /   ( i _ d i s t _ m i n   -   i _ d i s t _ m a x ) ; 
 
                         q 2 3   =   2 . 0   *   i _ d i s t _ m a x   *   i _ d i s t _ m i n   /   ( i _ d i s t _ m i n   -   i _ d i s t _ m a x ) ; 
 
                         o _ f r u s t u m . m 2 0   =   q 2 0   *   t r a n s [ 0 ] [ 0 ]   +   q 2 1   *   t r a n s [ 1 ] [ 0 ]   +   q 2 2   *   t r a n s [ 2 ] [ 0 ] ; 
 
                         o _ f r u s t u m . m 2 1   =   q 2 0   *   t r a n s [ 0 ] [ 1 ]   +   q 2 1   *   t r a n s [ 1 ] [ 1 ]   +   q 2 2   *   t r a n s [ 2 ] [ 1 ] ; 
 
                         o _ f r u s t u m . m 2 2   =   q 2 0   *   t r a n s [ 0 ] [ 2 ]   +   q 2 1   *   t r a n s [ 1 ] [ 2 ]   +   q 2 2   *   t r a n s [ 2 ] [ 2 ] ; 
 
                         o _ f r u s t u m . m 2 3   =   q 2 0   *   t r a n s [ 0 ] [ 3 ]   +   q 2 1   *   t r a n s [ 1 ] [ 3 ]   +   q 2 2   *   t r a n s [ 2 ] [ 3 ]   +   q 2 3 ; 
 
 
 
                         q 3 0   =   0 . 0 ; 
 
                         q 3 1   =   0 . 0 ; 
 
                         q 3 2   =   - 1 . 0 ; 
 
                         q 3 3   =   0 . 0 ; 
 
                         o _ f r u s t u m . m 3 0   =   q 3 0   *   t r a n s [ 0 ] [ 0 ]   +   q 3 1   *   t r a n s [ 1 ] [ 0 ]   +   q 3 2   *   t r a n s [ 2 ] [ 0 ] ; 
 
                         o _ f r u s t u m . m 3 1   =   q 3 0   *   t r a n s [ 0 ] [ 1 ]   +   q 3 1   *   t r a n s [ 1 ] [ 1 ]   +   q 3 2   *   t r a n s [ 2 ] [ 1 ] ; 
 
                         o _ f r u s t u m . m 3 2   =   q 3 0   *   t r a n s [ 0 ] [ 2 ]   +   q 3 1   *   t r a n s [ 1 ] [ 2 ]   +   q 3 2   *   t r a n s [ 2 ] [ 2 ] ; 
 
                         o _ f r u s t u m . m 3 3   =   q 3 0   *   t r a n s [ 0 ] [ 3 ]   +   q 3 1   *   t r a n s [ 1 ] [ 3 ]   +   q 3 2   *   t r a n s [ 2 ] [ 3 ]   +   q 3 3 ; 
 
                         r e t u r n ; 
 
                 } 
 
         } 
 
 } 