/ *   
 
   *   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 1 2   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 ; 
 
 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0S	Y000n0LR{_j0cOW0~0Y00
 
           *   A R T o o l K i t n0A R M a t i SOh00]0n0Qtpek0vS_Y00_jL0B00~0Y00
 
           *   2 x 2 , 3 x 3 , 4 x 4 LRk0d0D0f0o00N y A T o o l K i t n0LR{000( { @ l i n k   N y A R D o u b l e M a t r i x 4 4 } , { @ l i n k   N y A R D o u b l e M a t r i x 3 3 } , { @ l i n k   N y A R D o u b l e M a t r i x 2 2 } , ) 
 
           *   L0B00~0Y0n0g00S0a000Oc0f0O0`0U0D00
 
           *   < p > m e m o : 
 
           *   S0n0000o00N_q}T0JRdY00S'`L0B00~0Y00
 
           *   < / p > 
 
           * / 
 
         p u b l i c   c l a s s   N y A R M a t 
 
         { 
 
                 / * * 
 
                   *   MR$P0<h
}Y0000000MR000h0LR000o0_Z0W00 NW0j0D0S0h0k0la0MRn00000LRn0'YM0U0h0W0f0O0j0D0S0h0
 
                   * / 
 
                 p r o t e c t e d   i n t e r n a l   d o u b l e [ ] [ ]   _ m ; 
 
                 / * * 
 
                   *   LR{k0OF0000000
 
                   * / 
 
                 p r i v a t e   i n t [ ]   _ _ m a t r i x S e l f I n v _ n o s ; 
 
                 / * * 
 
                   *   Rpe
 
                   * / 
 
                 p r o t e c t e d   i n t e r n a l   i n t   c l m ; 
 
                 / * * 
 
                   *   Lpe
 
                   * / 
 
                 p r o t e c t e d   i n t e r n a l   i n t   r o w ; 
 
                 / * * 
 
                   *   0000000000000
 
                   *   _jW0~0[000
 
                   *   @ 
 
                   * / 
 
                 p r o t e c t e d   N y A R M a t ( ) 
 
                 { 
 
                         t h r o w   n e w   N y A R E x c e p t i o n ( ) ; 
 
                 } 
 
                 / * * 
 
                   *   MRi _ m 0000W0f00000000ubW0~0Y00
 
                   *   
 
                   *   @ p a r a m   i _ r o w 
 
                   *   @ p a r a m   i _ c l m 
 
                   *   @ p a r a m   i _ m 
 
                   *   @ p a r a m   i _ i s _ a t t a c h e d _ b u f f e r 
 
                   *   i _ m 0000000L0{tY00K00:yW0~0Y00t r u e n04XT0i _ m n0@b	g)jo0000000k0y0~0Y00
 
                   * / 
 
                 p u b l i c   N y A R M a t ( i n t   i _ r o w ,   i n t   i _ c l m ,   d o u b l e [ ] [ ]   i _ m ,   b o o l   i _ i s _ a t t a c h e d _ b u f f e r ) 
 
                 { 
 
                         t h i s . c l m   =   i _ c l m ; 
 
                         t h i s . r o w   =   i _ r o w ; 
 
                         t h i s . _ m   =   i _ m ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   0000000g0Y00
 
                   *   LRn00000c[W0f00N y A R M a t 0000000\ObW0~0Y00
 
                   *   @ p a r a m   i _ r o w 
 
                   *   Lpeg0Y00
 
                   *   @ p a r a m   i _ c l m 
 
                   *   Rpeg0Y00
 
                   * / 
 
                 p u b l i c   N y A R M a t ( i n t   i _ r o w ,   i n t   i _ c l m ) 
 
                 { 
 
                         t h i s . _ m   =   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 ( i _ r o w ,   i _ c l m ) ; 
 
                         t h i s . c l m   =   i _ c l m ; 
 
                         t h i s . r o w   =   i _ r o w ; 
 
                         t h i s . _ _ m a t r i x S e l f I n v _ n o s   =   n e w   i n t [ i _ r o w ] ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   LRn0Rpe0ԏW0~0Y00
 
                   *   @ r e t u r n 
 
                   *   LRn0Rpe0
 
                   * / 
 
                 p u b l i c   i n t   g e t C l m ( ) 
 
                 { 
 
                         r e t u r n   c l m ; 
 
                 } 
 
                 / * * 
 
                   *   LRn0Lpe0ԏW0~0Y00
 
                   *   @ r e t u r n 
 
                   *   LRn0Rpe
 
                   * / 
 
                 p u b l i c   i n t   g e t R o w ( ) 
 
                 { 
 
                         r e t u r n   r o w ; 
 
                 } 
 
                 / * * 
 
                   *   LRn00000	YfW0~0Y00
 
                   *   pe0[LY00h00LRn0T$Po0
N[k0j00~0Y00
 
                   *   @ p a r a m   i _ r o w 
 
                   *   eW0D0Lpe
 
                   *   @ p a r a m   i _ c l m 
 
                   *   eW0D0Rpe0
 
                   * / 
 
                 p u b l i c   v o i d   r e a l l o c ( i n t   i _ r o w ,   i n t   i _ c l m ) 
 
                 { 
 
                         i f   ( i _ r o w   < =   t h i s . _ m . L e n g t h   & &   i _ c l m   < =   t h i s . _ m [ 0 ] . L e n g t h ) 
 
                         { 
 
                                 / /   ASRj0MRL0B00p0UO0W0j0D00
 
                         } 
 
                         e l s e 
 
                         { 
 
                                 / /   
NASRj00S0vY00
 
                                 t h i s . _ m   =   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 ( i _ r o w ,   i _ c l m ) ; 
 
                                 t h i s . _ _ m a t r i x S e l f I n v _ n o s   =   n e w   i n t [ i _ r o w ] ; 
 
                         } 
 
                         t h i s . c l m   =   i _ c l m ; 
 
                         t h i s . r o w   =   i _ r o w ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   LRTXn0cQ0{0[LW0~0Y00
 
                   *   i _ m a t _ a h0i _ m a t _ b n0Mz0{W0f00t h i s x0<h
}W0~0Y00
 
                   *   @ p a r a m   i _ m a t _ a 
 
                   *   {CQn0LRA 
 
                   *   @ p a r a m   i _ m a t _ b 
 
                   *   {CQn0LRB 
 
                   *   @ 
 
                   * / 
 
                 p u b l i c   v o i d   m u l ( N y A R M a t   i _ m a t _ a ,   N y A R M a t   i _ m a t _ b ) 
 
                 { 
 
                         D e b u g . A s s e r t ( i _ m a t _ a . c l m   = =   i _ m a t _ b . r o w   & &   t h i s . r o w   = =   i _ m a t _ a . r o w   & &   t h i s . c l m   = =   i _ m a t _ b . c l m ) ; 
 
 
 
                         d o u b l e   w ; 
 
                         i n t   r ,   c ,   i ; 
 
                         d o u b l e [ ] [ ]   a m   =   i _ m a t _ a . _ m ,   b m   =   i _ m a t _ b . _ m ,   d m   =   t h i s . _ m ; 
 
                         / /   F o r 	Yfybk
 
                         f o r   ( r   =   0 ;   r   <   t h i s . r o w ;   r + + ) 
 
                         { 
 
                                 f o r   ( c   =   0 ;   c   <   t h i s . c l m ;   c + + ) 
 
                                 { 
 
                                         w   =   0 . 0 ; 
 
                                         f o r   ( i   =   0 ;   i   <   i _ m a t _ a . c l m ;   i + + ) 
 
                                         { 
 
                                                 w   + =   a m [ r ] [ i ]   *   b m [ i ] [ c ] ; 
 
                                         } 
 
                                         d m [ r ] [ c ]   =   w ; 
 
                                 } 
 
                         } 
 
                         r e t u r n ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   LR0{W0f00t h i s x0<h
}W0~0Y00
 
                   *   @ r e t u r n 
 
                   *  0LRL0{g0M0_0K0n00w}P$P0ԏW0~0Y00t r u e j000LRL0X[(WW0~0Y00f a l s e j000LRo0B00~0[000
 
                   *   1YWeY00h00t h i s n0Q[o0
N[k0j00~0Y00
 
                   *   @ 
 
                   * / 
 
                 p u b l i c   b o o l   i n v e r s e ( ) 
 
                 { 
 
                         d o u b l e [ ] [ ]   a p   =   t h i s . _ m ; 
 
                         i n t   d i m e n   =   t h i s . r o w ; 
 
                         i n t   d i m e n _ 1   =   d i m e n   -   1 ; 
 
                         d o u b l e [ ]   a p _ n ,   a p _ i p ,   a p _ i ; / /   w a p ; 
 
                         i n t   j ,   i p ,   n w o r k ; 
 
                         i n t [ ]   n o s   =   _ _ m a t r i x S e l f I n v _ n o s ; / / 000	Ype
 
                         / /   d o u b l e   e p s l ; 
 
                         d o u b l e   p ,   p b u f ,   w o r k ; 
 
 
 
                         / *   c h e c k   s i z e   * / 
 
                         s w i t c h   ( d i m e n ) 
 
                         { 
 
                                 c a s e   0 : 
 
                                         t h r o w   n e w   N y A R E x c e p t i o n ( ) ; 
 
                                 c a s e   1 : 
 
                                         a p [ 0 ] [ 0 ]   =   1 . 0   /   a p [ 0 ] [ 0 ] ; / /   * a p   =   1 . 0   /   ( * a p ) ; 
 
                                         r e t u r n   t r u e ; / *   1   d i m e n s i o n   * / 
 
                         } 
 
                         f o r   ( i n t   n   =   0 ;   n   <   d i m e n ;   n + + ) 
 
                         { 
 
                                 n o s [ n ]   =   n ; 
 
                         } 
 
                         / / i p L0[~00j0D0g0{L0L0004XTL0B00n0g0?ceQ0  000Qg00 RgSW0f0D0D0K0L0$R0j0D00
 
                         i p   =   0 ; 
 
                         / /   F o r 	Yfybk
 
                         f o r   ( i n t   n   =   0 ;   n   <   d i m e n ;   n + + ) 
 
                         { 
 
                                 a p _ n   =   a p [ n ] ; / /   w c p   =   a p   +   n   *   r o w a ; 
 
                                 p   =   0 . 0 ; 
 
                                 f o r   ( i n t   i   =   n ;   i   <   d i m e n ;   i + + ) 
 
                                 { 
 
                                         i f   ( p   <   ( p b u f   =   M a t h . A b s ( a p [ i ] [ 0 ] ) ) ) 
 
                                         { 
 
                                                 p   =   p b u f ; 
 
                                                 i p   =   i ; 
 
                                         } 
 
                                 } 
 
                                 / /   i f   ( p   < =   m a t r i x S e l f I n v _ e p s l ) { 
 
                                 i f   ( p   = =   0 . 0 ) 
 
                                 { 
 
                                         r e t u r n   f a l s e ; 
 
                                         / /   t h r o w   n e w   N y A R E x c e p t i o n ( ) ; 
 
                                 } 
 
 
 
                                 n w o r k   =   n o s [ i p ] ; 
 
                                 n o s [ i p ]   =   n o s [ n ] ; 
 
                                 n o s [ n ]   =   n w o r k ; 
 
 
 
                                 a p _ i p   =   a p [ i p ] ; 
 
                                 f o r   ( j   =   0 ;   j   <   d i m e n ;   j + + ) 
 
                                 { / /   f o r ( j   =   0 ,   w a p   =   a p   +   i p   *   r o w a , 
 
                                         / /   w b p   =   w c p ;   j   <   d i m e n   ;   j + + )   { 
 
                                         w o r k   =   a p _ i p [ j ] ;   / /   w o r k   =   * w a p ; 
 
                                         a p _ i p [ j ]   =   a p _ n [ j ] ; 
 
                                         a p _ n [ j ]   =   w o r k ; 
 
                                 } 
 
 
 
                                 w o r k   =   a p _ n [ 0 ] ; 
 
                                 f o r   ( j   =   0 ;   j   <   d i m e n _ 1 ;   j + + ) 
 
                                 { 
 
                                         a p _ n [ j ]   =   a p _ n [ j   +   1 ]   /   w o r k ; / /   * w a p   =   * ( w a p   +   1 )   /   w o r k ; 
 
                                 } 
 
                                 a p _ n [ j ]   =   1 . 0   /   w o r k ; / /   * w a p   =   1 . 0   /   w o r k ; 
 
                                 f o r   ( i n t   i   =   0 ;   i   <   d i m e n ;   i + + ) 
 
                                 { 
 
                                         i f   ( i   ! =   n ) 
 
                                         { 
 
                                                 a p _ i   =   a p [ i ] ; / /   w a p   =   a p   +   i   *   r o w a ; 
 
                                                 w o r k   =   a p _ i [ 0 ] ; 
 
                                                 f o r   ( j   =   0 ;   j   <   d i m e n _ 1 ;   j + + ) 
 
                                                 { / /   f o r ( j   =   1 ,   w b p   =   w c p , w o r k   =   * w a p ; j   <   d i m e n   ; j + + ,   w a p + + ,   w b p + + ) 
 
                                                         a p _ i [ j ]   =   a p _ i [ j   +   1 ]   -   w o r k   *   a p _ n [ j ] ; / /   w a p   =   * ( w a p   + 1 )   -   w o r k   * ( * w b p ) ; 
 
                                                 } 
 
                                                 a p _ i [ j ]   =   - w o r k   *   a p _ n [ j ] ; / /   * w a p   =   - w o r k   *   ( * w b p ) ; 
 
                                         } 
 
                                 } 
 
                         } 
 
 
 
                         f o r   ( i n t   n   =   0 ;   n   <   d i m e n ;   n + + ) 
 
                         { 
 
                                 f o r   ( j   =   n ;   j   <   d i m e n ;   j + + ) 
 
                                 { 
 
                                         i f   ( n o s [ j ]   = =   n ) 
 
                                         { 
 
                                                 b r e a k ; 
 
                                         } 
 
                                 } 
 
                                 n o s [ j ]   =   n o s [ n ] ; 
 
                                 f o r   ( i n t   i   =   0 ;   i   <   d i m e n ;   i + + ) 
 
                                 { 
 
                                         a p _ i   =   a p [ i ] ; 
 
                                         w o r k   =   a p _ i [ j ] ; / /   w o r k   =   * w a p ; 
 
                                         a p _ i [ j ]   =   a p _ i [ n ] ; / /   * w a p   =   * w b p ; 
 
                                         a p _ i [ n ]   =   w o r k ; / /   * w b p   =   w o r k ; 
 
                                 } 
 
                         } 
 
                         r e t u r n   t r u e ; 
 
                 } 
 
                 / * * 
 
                   *   i _ c o p y _ f r o m n0Q[00t h i s x0000W0~0Y00  
 
                   *   @ p a r a m   i _ c o p y _ f r o m 
 
                   *   000CQn0LRg0Y00
 
                   *   S0n0LRn0000o00t h i s h0TX0g0j0Q00p0j00~0[000
 
                   * / 
 
                 p u b l i c   v o i d   s e t V a l u e ( N y A R M a t   i _ c o p y _ f r o m ) 
 
                 { 
 
                         / /   000x
 
                         i f   ( t h i s . r o w   ! =   i _ c o p y _ f r o m . r o w   | |   t h i s . c l m   ! =   i _ c o p y _ f r o m . c l m ) 
 
                         { 
 
                                 t h r o w   n e w   N y A R E x c e p t i o n ( ) ; 
 
                         } 
 
                         / /   $P000
 
                         f o r   ( i n t   r   =   t h i s . r o w   -   1 ;   r   > =   0 ;   r - - ) 
 
                         { 
 
                                 f o r   ( i n t   c   =   t h i s . c l m   -   1 ;   c   > =   0 ;   c - - ) 
 
                                 { 
 
                                         t h i s . _ m [ r ] [ c ]   =   i _ c o p y _ f r o m . _ m [ r ] [ c ] ; 
 
                                 } 
 
                         } 
 
                 } 
 
                 / * * 
 
                   *   LRn000000ԏW0~0Y00
 
                   *   ԏtS$Pn0	gRgk0laW0f0O0`0U0D00
 
                   *   S0n0$Pn0	gRBfo00!kk0S0n0S0n0000000n0pe0[LY00~0g0n0g0Y00
 
                   *   @ r e t u r n 
 
                   *   LRn00000
 
                   * / 
 
                 p u b l i c   d o u b l e [ ] [ ]   g e t A r r a y ( ) 
 
                 { 
 
                         r e t u r n   _ m ; 
 
                 } 
 
                 / * * 
 
                   *   LRn0 }00hQf00 k0W0~0Y00
 
                   * / 
 
                 p u b l i c   v o i d   l o a d Z e r o ( ) 
 
                 { 
 
                         / /   F o r 	YfO K 
 
                         f o r   ( i n t   i   =   t h i s . r o w   -   1 ;   i   > =   0 ;   i - - ) 
 
                         { 
 
                                 f o r   ( i n t   i 2   =   t h i s . c l m   -   1 ;   i 2   > =   0 ;   i 2 - - ) 
 
                                 { 
 
                                         t h i s . _ m [ i ] [ i 2 ]   =   0 . 0 ; 
 
                                 } 
 
                         } 
 
                 } 
 
                 / * * 
 
                   *   i _ s r c n0nLR0d e s t k0_~0Y00
 
                   *   S0n0peo0*g0000n0p0[LY00h0OYL0zvuW0~0Y00
 
                   *   @ p a r a m   d e s t 
 
                   *   QRHQn0000000
 
                   *   @ p a r a m   s o u r c e 
 
                   *   eQRCQn0000000
 
                   * / 
 
                 p u b l i c   v o i d   t r a n s p o s e ( N y A R M a t   i _ s r c ) 
 
                 { 
 
                         i f   ( t h i s . r o w   ! =   i _ s r c . c l m   | |   t h i s . c l m   ! =   i _ s r c . r o w ) 
 
                         { 
 
                                 t h r o w   n e w   N y A R E x c e p t i o n ( ) ; 
 
                         } 
 
                         f o r   ( i n t   r   =   0 ;   r   <   t h i s . r o w ;   r + + ) 
 
                         { 
 
                                 f o r   ( i n t   c   =   0 ;   c   <   t h i s . c l m ;   c + + ) 
 
                                 { 
 
                                         t h i s . _ m [ r ] [ c ]   =   i _ s r c . _ m [ c ] [ r ] ; 
 
                                 } 
 
                         } 
 
                 } 
 
 
 
                 / * * 
 
                   *   A R T o o l K i t n00a r M a t r i x U n i t peh0TI{j0peg0Y00u n i t 0XSMOLRk0RgSW0~0Y00
 
                   *   S0n0peo0*g0000n0p0[LY00h0OYL0zvuW0~0Y00
 
                   *   @ p a r a m   u n i t 
 
                   *   d\OY000000000
 
                   * / 
 
                 p u b l i c   s t a t i c   v o i d   m a t r i x U n i t ( N y A R M a t   u n i t ) 
 
                 { 
 
                         i f   ( u n i t . r o w   ! =   u n i t . c l m ) 
 
                         { 
 
                                 t h r o w   n e w   N y A R E x c e p t i o n ( ) ; 
 
                         } 
 
                         N y A R E x c e p t i o n . t r a p ( " *g0000n000" ) ; 
 
                         / /   F o r 	Yfybk
 
                         f o r   ( i n t   r   =   0 ;   r   <   u n i t . g e t R o w ( ) ;   r + + ) 
 
                         { 
 
                                 f o r   ( i n t   c   =   0 ;   c   <   u n i t . g e t C l m ( ) ;   c + + ) 
 
                                 { 
 
                                         i f   ( r   = =   c ) 
 
                                         { 
 
                                                 u n i t . _ m [ r ] [ c ]   =   1 . 0 ; 
 
                                         } 
 
                                         e l s e 
 
                                         { 
 
                                                 u n i t . _ m [ r ] [ c ]   =   0 . 0 ; 
 
                                         } 
 
                                 } 
 
                         } 
 
                 } 
 
 
 
                 / * *   @ d e p r e c a t e d   * / 
 
                 p u b l i c   v o i d   z e r o C r e a r ( )   {   t h i s . l o a d Z e r o ( ) ;   } 
 
                 / * *   @ d e p r e c a t e d   * / 
 
                 p u b l i c   v o i d   c o p y F r o m ( N y A R M a t   i _ c o p y _ f r o m )   {   t h i s . s e t V a l u e ( i _ c o p y _ f r o m ) ;   } 
 
                 / * *   @ d e p r e c a t e d   * / 
 
                 p u b l i c   b o o l   m a t r i x S e l f I n v ( )   {   r e t u r n   t h i s . i n v e r s e ( ) ;   } 
 
                 / * *   @ d e p r e c a t e d   * / 
 
                 p u b l i c   v o i d   m a t r i x M u l ( N y A R M a t   i _ m a t _ a ,   N y A R M a t   i _ m a t _ b )   {   t h i s . m u l ( i _ m a t _ a ,   i _ m a t _ b ) ;   } 
 
 
 
         } 
 
 } 