/ *   
 
   *   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 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0N y A R T o o l k i t n0{_g00N!kCQwb_K00!kCQMOnYR0c[W0~0Y00
 
           * / 
 
         p u b l i c   c l a s s   N y A R T r a n s M a t   :   I N y A R T r a n s M a t 
 
         { 
 
                 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   _ r e f _ p r o j e c t i o n _ m a t ; 
 
                 p r o t e c t e d   N y A R R o t M a t r i x   _ r o t m a t r i x ; 
 
                 p r o t e c t e d   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   _ t r a n s s o l v e r ; 
 
                 p r o t e c t e d   N y A R P a r t i a l D i f f e r e n t i a t i o n O p t i m i z e   _ m a t _ o p t i m i z e ; 
 
 
 
 
 
                 p r i v a t e   I N y A R C a m e r a D i s t o r t i o n F a c t o r   _ r e f _ d i s t _ f a c t o r ; 
 
 
 
                 / * * 
 
                   *   0000000g0Y00
 
                   *   >mu000g0Rg00000000000c[W0_0D04XTo0S0a000OD0~0Y00
 
                   * / 
 
                 p r o t e c t e d   N y A R T r a n s M a t ( ) 
 
                 { 
 
                         / / _ c a l c u l a t o r , _ r o t m a t r i x , _ m a t _ o p t i m i z e 00000000n0B}N_k0
 
                         / / \ObW0f0rR0S_f0f0O0`0U0D00
 
                         r e t u r n ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   S0n0peo000000000K00|Ts0QW0f0O0`0U0D00
 
                   *   @ p a r a m   i _ d i s t f a c t o r 
 
                   *   jk0n0wckk0OF00000000
 
                   *   @ p a r a m   i _ p r o j m a t 
 
                   *   @ 
 
                   * / 
 
                 p r i v a t e   v o i d   i n i t I n s t a n c e ( I N y A R C a m e r a D i s t o r t i o n F a c t o r   i _ d i s t f a c t o 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 m a t ) 
 
                 { 
 
                         t h i s . _ t r a n s s o l v e r   =   n e w   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 _ p r o j m a t ,   4 ) ; 
 
                         / / Nc'`L0͑j0Bfo00N y A R R o t M a t r i x _ A R T o o l K i t 0OF0S0h00
 
                         / / tH\o0N y A R R o t M a t r i x _ N y A R T o o l K i t 0N y A R R o t M a t r i x _ A R T o o l K i t 0TX0`0Q0i00\W0`0Q0$PL0Z0000
 
                         t h i s . _ r o t m a t r i x   =   n e w   N y A R R o t M a t r i x ( i _ p r o j m a t ) ; 
 
                         t h i s . _ m a t _ o p t i m i z e   =   n e w   N y A R P a r t i a l D i f f e r e n t i a t i o n O p t i m i z e ( i _ p r o j m a t ) ; 
 
                         t h i s . _ r e f _ d i s t _ f a c t o r   =   i _ d i s t f a c t o r ; 
 
                         t h i s . _ r e f _ p r o j e c t i o n _ m a t   =   i _ p r o j m a t ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   0000000g0Y00
 
                   *   ^j{k0_j0000000n0Sgq$P0CQk000000000ubW0~0Y00
 
                   *   @ p a r a m   i _ r e f _ d i s t f a c t o r 
 
                   *   =jWjk0wck000000n0Sgq$Pg0Y00jk0wckL0
Nj0Bfo00n u l l 0c[W0~0Y00
 
                   *   @ p a r a m   i _ r e f _ p r o j m a t 
 
                   *   \q_	Yc000000n0Sgq$Pg0Y00
 
                   *   @ 
 
                   * / 
 
                 p u b l i c   N y A R T r a n s M a t ( I N y A R C a m e r a D i s t o r t i o n F a c t o r   i _ r e f _ d i s t f a c t o 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 _ r e f _ p r o j m a t ) 
 
                 { 
 
                         i n i t I n s t a n c e ( i _ r e f _ d i s t f a c t o r ,   i _ r e f _ p r o j m a t ) ; 
 
                         r e t u r n ; 
 
                 } 
 
                 / * * 
 
                   *   0000000g0Y00
 
                   *   ^j{k0_j000000000n0Sgq$P0CQk000000000ubW0~0Y00
 
                   *   @ p a r a m   i _ p a r a m 
 
                   *   A R T o o l K i t b__n000000000g0Y00
 
                   *   000000o00S0n0-NK00=jWjk0wck0000000\q_	Yc0000000SgqW0~0Y00
 
                   *   @ 
 
                   * / 
 
                 p u b l i c   N y A R T r a n s M a t ( N y A R P a r a m   i _ p a r a m ) 
 
                 { 
 
                         i n i t I n s t a n c e ( i _ p a r a m . g e t D i s t o r t i o n F a c t o r ( ) ,   i _ p a r a m . g e t 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 ( ) ) ; 
 
                         r e t u r n ; 
 
                 } 
 
 
 
                 p r i v a t e   r e a d o n l y   N y A R D o u b l e P o i n t 2 d [ ]   _ _ t r a n s M a t _ v e r t e x _ 2 d   =   N y A R D o u b l e P o i n t 2 d . c r e a t e A r r a y ( 4 ) ; 
 
                 p r i v a t e   r e a d o n l y   N y A R D o u b l e P o i n t 3 d [ ]   _ _ t r a n s M a t _ 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 . c r e a t e A r r a y ( 4 ) ; 
 
                 p r i v a t e   r e a d o n l y   N y A R D o u b l e P o i n t 3 d   _ _ t r a n s M a t _ t r a n s   =   n e w   N y A R D o u b l e P o i n t 3 d ( ) ; 
 
 
 
                 / * * 
 
                   *   p`1X0CQk00000$P0{W0~0Y00
 
                   *   @ p a r a m   i _ v e r t e x 
 
                   * / 
 
                 p r i v a t e   d o u b l e   m a k e E r r T h r e s h o l d ( N y A R D o u b l e P o i n t 2 d [ ]   i _ v e r t e x ) 
 
                 { 
 
                         d o u b l e   a ,   b ,   l 1 ,   l 2 ; 
 
                         a   =   i _ v e r t e x [ 0 ] . x   -   i _ v e r t e x [ 2 ] . x ; 
 
                         b   =   i _ v e r t e x [ 0 ] . y   -   i _ v e r t e x [ 2 ] . y ; 
 
                         l 1   =   a   *   a   +   b   *   b ; 
 
                         a   =   i _ v e r t e x [ 1 ] . x   -   i _ v e r t e x [ 3 ] . x ; 
 
                         b   =   i _ v e r t e x [ 1 ] . y   -   i _ v e r t e x [ 3 ] . y ; 
 
                         l 2   =   a   *   a   +   b   *   b ; 
 
                         r e t u r n   ( M a t h . S q r t ( l 1   >   l 2   ?   l 1   :   l 2 ) )   /   2 0 0 ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   S0n0peo00t`^j|n0V҉|0CQk00MOnYR	YcLR0Bl0~0Y00
 
                   *   A R T o o l K i t n0a r G e t T r a n s M a t k0rS_W0~0Y00
 
                   *   @ s e e   I N y A R T r a n s M a t # t r a n s M a t C o n t i n u e 
 
                   * / 
 
 	         p u b l i c   b o o l   t r a n s M a t ( N y A R S q u a r e   i _ s q u a r e , N y A R R e c t O f f s e t   i _ o f f s e t ,   N y A R D o u b l e M a t r i x 4 4   o _ r e s u l t , N y A R T r a n s M a t R e s u l t P a r a m   o _ p a r a m ) 
 
                 { 
 
                         N y A R D o u b l e P o i n t 3 d   t r a n s   =   t h i s . _ _ t r a n s M a t _ t r a n s ; 
 
                         d o u b l e   e r r _ t h r e s h o l d   =   m a k e E r r T h r e s h o l d ( i _ s q u a r e . s q v e r t e x ) ; 
 
 
 
                         N y A R D o u b l e P o i n t 2 d [ ]   v e r t e x _ 2 d ; 
 
                         i f   ( t h i s . _ r e f _ d i s t _ f a c t o r   ! =   n u l l ) 
 
                         { 
 
                                 / / jk0_CQ_
 
                                 v e r t e x _ 2 d   =   t h i s . _ _ t r a n s M a t _ v e r t e x _ 2 d ; 
 
                                 t h i s . _ r e f _ d i s t _ f a c t o r . i d e a l 2 O b s e r v B a t c h ( i _ s q u a r e . s q v e r t e x ,   v e r t e x _ 2 d ,   4 ) ; 
 
                         } 
 
                         e l s e 
 
                         { 
 
                                 / / jk0_CQo0
N
 
                                 v e r t e x _ 2 d   =   i _ s q u a r e . s q v e r t e x ; 
 
                         } 
 
                         / / s^LyRϑ{_jk002 D ^j|0000
 
                         t h i s . _ t r a n s s o l v e r . s e t 2 d V e r t e x ( v e r t e x _ 2 d ,   4 ) ; 
 
 
 
                         / / VLR0{
 
                         i f   ( ! t h i s . _ r o t m a t r i x . i n i t R o t B y S q u a r e ( i _ s q u a r e . l i n e ,   i _ s q u a r e . s q v e r t e x ) ) 
 
                         { 
 
                                 r e t u r n   f a l s e ; 
 
                         } 
 
 
 
                         / / V⎌_n03 D ^j|K000s^LyRϑ0{
 
                         N y A R D o u b l e P o i n t 3 d [ ]   v e r t e x _ 3 d   =   t h i s . _ _ t r a n s M a t _ v e r t e x _ 3 d ; 
 
                         t h i s . _ r o t m a t r i x . g e t P o i n t 3 d B a t c h ( i _ o f f s e t . v e r t e x ,   v e r t e x _ 3 d ,   4 ) ; 
 
                         t h i s . _ t r a n s s o l v e r . s o l v e T r a n s p o r t V e c t o r ( v e r t e x _ 3 d ,   t r a n s ) ; 
 
 
 
                         / / {P}gn0 giS( s^LyRϑh0VLRn0 giS) 
 
                         d o u b l e   e r r   =   t h i s . o p t i m i z e ( t h i s . _ r o t m a t r i x ,   t r a n s ,   t h i s . _ t r a n s s o l v e r ,   i _ o f f s e t . v e r t e x ,   v e r t e x _ 2 d ,   e r r _ t h r e s h o l d ,   o _ r e s u l t ) ; 
 
                         / / _j00{000000ԏtS
 
                         i f   ( o _ p a r a m   ! =   n u l l ) 
 
                         { 
 
                                 o _ p a r a m . l a s t _ e r r o r   =   e r r ; 
 
                         }   
 
                         r e t u r n   t r u e ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   S0n0peo00t`^j|n0V҉|0CQk00MOnYR	YcLR0Bl0~0Y00
 
                   *   {k0NSn0e\tk0OF0pL00{ @ l i n k   # t r a n s M a t } h0puj00~0Y00
 
                   *   @ s e e   I N y A R T r a n s M a t # t r a n s M a t C o n t i n u e 
 
                   * / 
 
   	         p u b l i c   b o o l   t r a n s M a t C o n t i n u e ( N y A R S q u a r e   i _ s q u a r e , N y A R R e c t O f f s e t   i _ o f f s e t ,   N y A R D o u b l e M a t r i x 4 4   i _ p r e v _ r e s u l t , d o u b l e   i _ p r e v _ e r r , N y A R D o u b l e M a t r i x 4 4   o _ r e s u l t , N y A R T r a n s M a t R e s u l t P a r a m   o _ p a r a m ) 
 
                 { 
 
                         N y A R D o u b l e P o i n t 3 d   t r a n s   =   t h i s . _ _ t r a n s M a t _ t r a n s ; 
 
 
 
                         / /  giS{n0$P0zl[
 
                         d o u b l e   e r r _ t h r e s h o l d   =   m a k e E r r T h r e s h o l d ( i _ s q u a r e . s q v e r t e x ) ; 
 
 
 
 
 
                         / / s^LyRϑ{_jk002 D ^j|0000
 
                         N y A R D o u b l e P o i n t 2 d [ ]   v e r t e x _ 2 d ; 
 
                         i f   ( t h i s . _ r e f _ d i s t _ f a c t o r   ! =   n u l l ) 
 
                         { 
 
                                 v e r t e x _ 2 d   =   t h i s . _ _ t r a n s M a t _ v e r t e x _ 2 d ; 
 
                                 t h i s . _ r e f _ d i s t _ f a c t o r . i d e a l 2 O b s e r v B a t c h ( i _ s q u a r e . s q v e r t e x ,   v e r t e x _ 2 d ,   4 ) ; 
 
                         } 
 
                         e l s e 
 
                         { 
 
                                 v e r t e x _ 2 d   =   i _ s q u a r e . s q v e r t e x ; 
 
                         } 
 
                         t h i s . _ t r a n s s o l v e r . s e t 2 d V e r t e x ( v e r t e x _ 2 d ,   4 ) ; 
 
 
 
                         / / VLR0{
 
                         N y A R R o t M a t r i x   r o t   =   t h i s . _ r o t m a t r i x ; 
 
                         r o t . i n i t R o t B y P r e v R e s u l t ( i _ p r e v _ r e s u l t ) ; 
 
 
 
                         / / V⎌_n03 D ^j|K000s^LyRϑ0{
 
                         N y A R D o u b l e P o i n t 3 d [ ]   v e r t e x _ 3 d   =   t h i s . _ _ t r a n s M a t _ v e r t e x _ 3 d ; 
 
                         r o t . g e t P o i n t 3 d B a t c h ( i _ o f f s e t . v e r t e x ,   v e r t e x _ 3 d ,   4 ) ; 
 
                         t h i s . _ t r a n s s o l v e r . s o l v e T r a n s p o r t V e c t o r ( v e r t e x _ 3 d ,   t r a n s ) ; 
 
 
 
                         / / s(Wn00000000{
 
                         d o u b l e   m i n _ e r r   =   e r r R a t e ( r o t ,   t r a n s ,   i _ o f f s e t . v e r t e x ,   v e r t e x _ 2 d ,   4 ,   v e r t e x _ 3 d ) ; 
 
                         
 
                         / / 000000n0$R[
 
 	 	         i f ( m i n _ e r r < i _ p r e v _ e r r + e r r _ t h r e s h o l d ) { 
 
 	 	 	         / / s a v e   i n i t i a l   r e s u l t 
 
 	 	 	         o _ r e s u l t . s e t V a l u e ( r o t , t r a n s ) ; 
 
                                 / / 	 	 	 S y s t e m . o u t . p r i n t l n ( " T R : o k " ) ; 
 
                                 / /  giSW0f0000
 
                                 f o r   ( i n t   i   =   0 ;   i   <   5 ;   i + + ) 
 
                                 { 
 
                                         / / 	YcLRn0 giS
 
                                         t h i s . _ m a t _ o p t i m i z e . m o d i f y M a t r i x ( r o t ,   t r a n s ,   i _ o f f s e t . v e r t e x ,   v e r t e x _ 2 d ,   4 ) ; 
 
                                         d o u b l e   e r r   =   e r r R a t e ( r o t ,   t r a n s ,   i _ o f f s e t . v e r t e x ,   v e r t e x _ 2 d ,   4 ,   v e r t e x _ 3 d ) ; 
 
                                         / / S y s t e m . o u t . p r i n t l n ( " E : " + e r r ) ; 
 
                                         i f   ( m i n _ e r r   -   e r r   <   e r r _ t h r e s h o l d   /   2 ) 
 
                                         { 
 
                                                 / / S y s t e m . o u t . p r i n t l n ( " B R E A K " ) ; 
 
                                                 b r e a k ; 
 
                                         } 
 
                                         t h i s . _ t r a n s s o l v e r . s o l v e T r a n s p o r t V e c t o r ( v e r t e x _ 3 d ,   t r a n s ) ; 
 
                                         o _ r e s u l t . s e t V a l u e ( r o t ,   t r a n s ) ; 
 
                                         m i n _ e r r   =   e r r ; 
 
                                 } 
 
                                 / / }}{bR
 
                                 i f   ( o _ p a r a m   ! =   n u l l ) 
 
                                 { 
 
                                         o _ p a r a m . l a s t _ e r r o r   =   m i n _ e r r ; 
 
                                 } 
 
                                 r e t u r n   t r u e ; 
 
                         } 
 
                         / / }}{1YWe
 
                         r e t u r n   f a l s e ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   
 
                   *   @ p a r a m   i w _ r o t m a t 
 
                   *   @ p a r a m   i w _ t r a n s v e c 
 
                   *   @ p a r a m   i _ s o l v e r 
 
                   *   @ p a r a m   i _ o f f s e t _ 3 d 
 
                   *   @ p a r a m   i _ 2 d _ v e r t e x 
 
                   *   @ p a r a m   i _ e r r _ t h r e s h o l d 
 
                   *   @ p a r a m   o _ r e s u l t 
 
                   *   @ r e t u r n 
 
                   *   @ 
 
                   * / 
 
 	         p r i v a t e   d o u b l e   o p t i m i z e ( N y A R R o t M a t r i x   i w _ r o t m a t , N y A R D o u b l e P o i n t 3 d   i w _ t r a n s v e c , 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   i _ s o l v e r , N y A R D o u b l e P o i n t 3 d [ ]   i _ o f f s e t _ 3 d , N y A R D o u b l e P o i n t 2 d [ ]   i _ 2 d _ v e r t e x , d o u b l e   i _ e r r _ t h r e s h o l d , N y A R D o u b l e M a t r i x 4 4   o _ r e s u l t ) 
 
                 { 
 
                         / / S y s t e m . o u t . p r i n t l n ( " S T A R T " ) ; 
 
                         N y A R D o u b l e P o i n t 3 d [ ]   v e r t e x _ 3 d   =   t h i s . _ _ t r a n s M a t _ v e r t e x _ 3 d ; 
 
                         / / Rgn0000$P0{
 
                         d o u b l e   m i n _ e r r   =   e r r R a t e ( i w _ r o t m a t ,   i w _ t r a n s v e c ,   i _ o f f s e t _ 3 d ,   i _ 2 d _ v e r t e x ,   4 ,   v e r t e x _ 3 d ) ; 
 
                         o _ r e s u l t . s e t V a l u e ( i w _ r o t m a t ,   i w _ t r a n s v e c ) ; 
 
 
 
                         f o r   ( i n t   i   =   0 ;   i   <   5 ;   i + + ) 
 
                         { 
 
                                 / / 	YcLRn0 giS
 
                                 t h i s . _ m a t _ o p t i m i z e . m o d i f y M a t r i x ( i w _ r o t m a t ,   i w _ t r a n s v e c ,   i _ o f f s e t _ 3 d ,   i _ 2 d _ v e r t e x ,   4 ) ; 
 
                                 d o u b l e   e r r   =   e r r R a t e ( i w _ r o t m a t ,   i w _ t r a n s v e c ,   i _ o f f s e t _ 3 d ,   i _ 2 d _ v e r t e x ,   4 ,   v e r t e x _ 3 d ) ; 
 
                                 / / S y s t e m . o u t . p r i n t l n ( " E : " + e r r ) ; 
 
                                 i f   ( m i n _ e r r   -   e r r   <   i _ e r r _ t h r e s h o l d ) 
 
                                 { 
 
                                         / / S y s t e m . o u t . p r i n t l n ( " B R E A K " ) ; 
 
                                         b r e a k ; 
 
                                 } 
 
                                 i _ s o l v e r . s o l v e T r a n s p o r t V e c t o r ( v e r t e x _ 3 d ,   i w _ t r a n s v e c ) ; 
 
                                 o _ r e s u l t . s e t V a l u e ( i w _ r o t m a t ,   i w _ t r a n s v e c ) ; 
 
                                 m i n _ e r r   =   e r r ; 
 
                         } 
 
                         / / S y s t e m . o u t . p r i n t l n ( " E N D " ) ; 
 
                         r e t u r n   m i n _ e r r ; 
 
                 } 
 
 
 
                 / * * 
 
                   *   S0n0peo00YRLRn00000000{W0~0Y00
 
                   *   000000o00VLR0s^LyRϑ0000000[^jK00{W0~0Y00
 
                   *   8^0000L0OF0S0h0o0B00~0[000
 
                   *   @ p a r a m   i _ r o t 
 
                   *   VLR
 
                   *   @ p a r a m   i _ t r a n s 
 
                   *   s^LyRϑ
 
                   *   @ p a r a m   i _ v e r t e x 3 d 
 
                   *   00000MOn
 
                   *   @ p a r a m   i _ v e r t e x 2 d 
 
                   *   t`^j
 
                   *   @ p a r a m   i _ n u m b e r _ o f _ v e r t e x 
 
                   *   UOY00ppe
 
                   *   @ p a r a m   o _ r o t _ v e r t e x 
 
                   *   {Nzg0_00_00Tpn0	N!kCQ^j
 
                   *   @ r e t u r n 
 
                   *   000000( ( t`^jh0{^jn0ݍ[ n ] ^ 2 ) ) 
 
                   *   @ 
 
                   * / 
 
                 p u b l i c   d o u b l e   e r r R a t e ( N y A R D o u b l e M a t r i x 3 3   i _ r o t ,   N y A R D o u b l e P o i n t 3 d   i _ t r a n s ,   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 2 d [ ]   i _ 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 ,   N y A R D o u b l e P o i n t 3 d [ ]   o _ r o t _ v e r t e x ) 
 
                 { 
 
                         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 p   =   t h i s . _ r e f _ p r o j e c t i o n _ m a t ; 
 
                         d o u b l e   c p 0 0   =   c p . m 0 0 ; 
 
                         d o u b l e   c p 0 1   =   c p . m 0 1 ; 
 
                         d o u b l e   c p 0 2   =   c p . m 0 2 ; 
 
                         d o u b l e   c p 1 1   =   c p . m 1 1 ; 
 
                         d o u b l e   c p 1 2   =   c p . m 1 2 ; 
 
 
 
                         d o u b l e   e r r   =   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 + + ) 
 
                         { 
 
                                 d o u b l e   x 3 d ,   y 3 d ,   z 3 d ; 
 
                                 o _ r o t _ v e r t e x [ i ] . x   =   x 3 d   =   i _ r o t . m 0 0   *   i _ v e r t e x 3 d [ i ] . x   +   i _ r o t . m 0 1   *   i _ v e r t e x 3 d [ i ] . y   +   i _ r o t . m 0 2   *   i _ v e r t e x 3 d [ i ] . z ; 
 
                                 o _ r o t _ v e r t e x [ i ] . y   =   y 3 d   =   i _ r o t . m 1 0   *   i _ v e r t e x 3 d [ i ] . x   +   i _ r o t . m 1 1   *   i _ v e r t e x 3 d [ i ] . y   +   i _ r o t . m 1 2   *   i _ v e r t e x 3 d [ i ] . z ; 
 
                                 o _ r o t _ v e r t e x [ i ] . z   =   z 3 d   =   i _ r o t . m 2 0   *   i _ v e r t e x 3 d [ i ] . x   +   i _ r o t . m 2 1   *   i _ v e r t e x 3 d [ i ] . y   +   i _ r o t . m 2 2   *   i _ v e r t e x 3 d [ i ] . z ; 
 
                                 x 3 d   + =   i _ t r a n s . x ; 
 
                                 y 3 d   + =   i _ t r a n s . y ; 
 
                                 z 3 d   + =   i _ t r a n s . z ; 
 
 
 
                                 / / \q_	Yc
 
                                 d o u b l e   x 2 d   =   x 3 d   *   c p 0 0   +   y 3 d   *   c p 0 1   +   z 3 d   *   c p 0 2 ; 
 
                                 d o u b l e   y 2 d   =   y 3 d   *   c p 1 1   +   z 3 d   *   c p 1 2 ; 
 
                                 d o u b l e   h 2 d   =   z 3 d ; 
 
 
 
                                 / / 000000{
 
                                 d o u b l e   t 1   =   i _ v e r t e x 2 d [ i ] . x   -   x 2 d   /   h 2 d ; 
 
                                 d o u b l e   t 2   =   i _ v e r t e x 2 d [ i ] . y   -   y 2 d   /   h 2 d ; 
 
                                 e r r   + =   t 1   *   t 1   +   t 2   *   t 2 ; 
 
 
 
                         } 
 
                         r e t u r n   e r r   /   i _ n u m b e r _ o f _ v e r t e x ; 
 
                 } 
 
         } 
 
 } 