ptmath._java


// Math routines for PTViewer

 												// Lookup tables for atan, sqrt and byte-multiply
int atan_LU_HR[]		= null;
int atan_LU_LR[];
int atan_LU_L1[];
int atan_LU_L2[];
int atan_LU_L3[];
int atan_LU_L4[];
int atan_LU_L5[];
int sqrt_LU[];
int mweights[][];
int PV_atan0_HR, PV_pi_HR;

	static final int NATAN = 4096;  			// Size of Lookup table for atan2 routines 	= 2^12
	static final int NSQRT = 4096;  			// Size of Lookup table for sqrt routine 	= 2^12
	private double		mt[][];					// 3 x 3 Transformation Matrix
	private int 		mi[][];					// Integer version of matrix above				
   	



void math_init(){
	mt = new double[3][3];
	mi = new int[3][3];
	// Set up Multiplication table
	int i,k;
	mweights = new int[256][256];
	for(i=0; i<256; i++)
		for(k=0; k<256; k++)
			mweights[i][k] = i*k;
}

void math_dispose(){
	mweights 	= null;
	atan_LU_HR 	= null;
	atan_LU_L1 	= null;
	atan_LU_L2 	= null;
	atan_LU_L3 	= null;
	atan_LU_L4 	= null;
	atan_LU_L5 	= null;
	atan_LU_LR 	= null;
	sqrt_LU	 	= null;
	mt = null;
	mi = null;
}


final void math_setLookUp(int[][] pd){
	int i,k,t;
	double z, dz;
	if( pd == null ) return;
	
		
	int ph = pd.length; int pw = pd[0].length;

	if( atan_LU_HR == null ){ // First call, set up arrays
		atan_LU_HR = new int[NATAN+1];
		atan_LU_L1 = new int[NATAN+1];
		atan_LU_L2 = new int[NATAN+1];
		atan_LU_L3 = new int[NATAN+1];
		atan_LU_L4 = new int[NATAN+1];
		atan_LU_L5 = new int[NATAN+1];
		atan_LU_LR = new int[NATAN+1];

		sqrt_LU = new int[NSQRT+1];
    		
		// Set up table for sqrt just once
		
		dz = 1.0 / (double)NSQRT;
		z = 0.0;
	
		for( i=0; i< NSQRT; i++, z+=dz )
			sqrt_LU[i] = (int)(Math.sqrt( 1.0 + z*z ) *  NSQRT);
		
		sqrt_LU[NSQRT] = (int)(Math.sqrt(2.0) *  NSQRT);

	}
		
	dz = 1.0 / (double) NATAN;
	z = 0.0;
		
	double dist_e = (double)pw / (2.0 * Math.PI);
	int sh2 = ph/2;
	int sw2 = pw/2;
		
	PV_atan0_HR 	= pw * 64;
	PV_pi_HR 		= 256/2 * pw ;

	for( i=0; i< NATAN+1; i++, z+=dz ){
		if( i < NATAN ){
			atan_LU_HR[i] = (int) ( dist_e * Math.atan( z / (1.0 - z ) ) * 256.0 + 0.5) ;
			t = (int) ( dist_e * Math.atan( z / (1.0 - z ) ) + 0.5);
		}else{
			atan_LU_HR[i] = (int)( dist_e * Math.PI /2.0 * 256.0 + 0.5);
			t = (int)( dist_e * Math.PI /2.0 + 0.5); 
		}
		atan_LU_LR[i] = t;
		atan_LU_L1[i] = t + sh2;
		atan_LU_L2[i] = - t + sh2;
		atan_LU_L3[i] = t + sw2;
		atan_LU_L4[i] = - t + sw2;
		atan_LU_L5[i] = - t + pw;
		if( atan_LU_L1[i]  < 0 ) atan_LU_L1[i] = 0;
		if( atan_LU_L1[i]  >= ph ) atan_LU_L1[i] = ph -1;
		if( atan_LU_L2[i]  < 0 ) atan_LU_L2[i] = 0;
		if( atan_LU_L2[i]  >= ph ) atan_LU_L2[i] = ph -1;
		if( atan_LU_LR[i]  < 0 ) atan_LU_LR[i] += pw;
		if( atan_LU_LR[i]  >= pw ) atan_LU_LR[i] -= pw;
		if( atan_LU_L3[i]  < 0 ) atan_LU_L3[i] += pw;
		if( atan_LU_L3[i]  >= pw ) atan_LU_L3[i] -= pw;
		if( atan_LU_L4[i]  < 0 ) atan_LU_L4[i] += pw;
		if( atan_LU_L4[i]  >= pw ) atan_LU_L4[i] -= pw;
		if( atan_LU_L5[i]  < 0 ) atan_LU_L5[i] += pw;
		if( atan_LU_L5[i]  >= pw ) atan_LU_L5[i] -= pw;
	}
}	


	// Set matrix elements based on Euler angles a, b, (c- no rotation)

final void SetMatrix( double a, double b,  double m[][], int cl ){
		double mx[][], my[][];
	
		mx = new double[3][3];
		my = new double[3][3];
	

		// Calculate Matrices;

		mx[0][0] = 1.0 ; 				mx[0][1] = 0.0 ; 				mx[0][2] = 0.0;
		mx[1][0] = 0.0 ; 				mx[1][1] = Math.cos(a) ; 		mx[1][2] = Math.sin(a);
		mx[2][0] = 0.0 ;				mx[2][1] =-mx[1][2] ;			mx[2][2] = mx[1][1];
	
		my[0][0] = Math.cos(b); 		my[0][1] = 0.0 ; 				my[0][2] =-Math.sin(b);
		my[1][0] = 0.0 ; 				my[1][1] = 1.0 ; 				my[1][2] = 0.0;
		my[2][0] = -my[0][2];			my[2][1] = 0.0 ;				my[2][2] = my[0][0];
	
		if( cl == 1 )
			matrix_matrix_mult( mx, my, m);
		else
			matrix_matrix_mult( my, mx, m);
			
	}



final void matrix_matrix_mult( double m1[][],double m2[][],double result[][]){
	int i,k;
	
	for(i=0;i<3;i++){
		for(k=0; k<3; k++){
			result[i][k] = m1[i][0] * m2[0][k] + m1[i][1] * m2[1][k] + m1[i][2] * m2[2][k];
		}
	}
}





final int PV_atan2_HR(int y, int x){
		if( x > 0 ){
			if( y > 0 )
				return  atan_LU_HR[(int)( NATAN * y / ( x + y ))];
			else
				return -atan_LU_HR[ (int)(NATAN * (-y) / ( x - y ))];
		}

		if( x == 0 ){
			if( y > 0 )
				return  PV_atan0_HR;
			else
				return  -PV_atan0_HR;
		}
		
		if( y < 0 )
			return  atan_LU_HR[(int)( NATAN * y / ( x + y ))] - PV_pi_HR;
		else
			return -atan_LU_HR[ (int)(NATAN * (-y) / ( x - y ))] + PV_pi_HR;
	}



final int PV_sqrt( int x1, int x2 ){
	return x1 > x2 ? (x1 * sqrt_LU[ (x2 << 12)/  x1 ]) >> 12 : x2 == 0 ? 0 : (x2 * sqrt_LU[ (x1 << 12) /  x2 ]) >> 12;
}

	

	// Bilinear interpolator for 4 pixels
	
final int bil(int p00, int p01, int p10, int p11, int dx, int dy)	{	
	
		int w1[], w2[], rd, gd, bd, yr, yg, yb, weight;
		
		w1 = mweights[dx & 0xff]; w2 = mweights[(255 - dx) & 0xff];

		rd = w2[(p00 >> 16) & 0xff] + w1[(p01 >> 16) & 0xff];  
		gd = w2[(p00 >>  8) & 0xff] + w1[(p01 >> 8 ) & 0xff];	
		bd = w2[(p00      ) & 0xff] + w1[(p01      ) & 0xff];

		yr = w2[(p10 >> 16) & 0xff] + w1[(p11 >> 16) & 0xff];  
		yg = w2[(p10 >>  8) & 0xff] + w1[(p11 >>  8) & 0xff];
		yb = w2[(p10      ) & 0xff] + w1[(p11      ) & 0xff];

		weight = 255 - dy;
																				
		//rd = (rd * weight + yr * dy) >> 16;	
		rd = (rd * weight + yr * dy) & 0xff0000;	
		gd = (gd * weight + yg * dy) >> 16;		
		bd = (bd * weight + yb * dy) >> 16;	
		
		// return( (rd << 16) + (gd << 8) + bd + 0xff000000);
		return( rd  + (gd << 8) + bd + 0xff000000);
	}
	

final void math_extractview(int[][] pd, int[] v, byte[] hv, int vw, double fov, double pan, double tilt, boolean bilinear){
	math_set_int_matrix( fov, pan, tilt, vw);
	math_transform( pd, pd[0].length, pd.length, v, hv, vw, v.length/vw, bilinear );
}


final void math_set_int_matrix( double fov, double pan, double tilt, int vw){
	double		a,p,ta;	// field of view in rad
	int 		i,k;
		
	a  =  fov * 2.0 * Math.PI / 360.0;	// field of view in rad		
	p  = (double)vw / (2.0 * Math.tan( a / 2.0 ) );

	SetMatrix( 	tilt * 2.0 * Math.PI / 360.0, 
				pan   * 2.0 * Math.PI / 360.0, 
				mt, 
				1 );

	mt[0][0] /= p;
	mt[0][1] /= p;
	mt[0][2] /= p;
	mt[1][0] /= p;
	mt[1][1] /= p;
	mt[1][2] /= p;

	ta = (a > 0.3 ? 65536.0 * 2.0 / a : 65536.0 * 2.0 / 0.3 );
		
		
	for(i=0; i<3; i++){
		for(k=0; k<3; k++){
			mi[i][k] = (int)( ta  * mt[i][k] + 0.5);
		}
	}

}


final void math_transform( int[][] pd, int pw, int ph, int[] v, byte[] hv, int vw, int vh, boolean bilinear ){
	int 				x,y, cy,idx,idx2,idx_max,m0,m1,m2,xm=0,sw2m,pm;
	int 				xs, xs2, ys, cy_max;	
	

	int					mix	  = pw - 1;// maximum x-index src
	int					miy	  = ph - 1;// maximum y-index src

	// Variables used to convert screen coordinates to cartesian coordinates

	int 				w2 	=  (vw - 1 )  / 2 ;  
	int 				h2 	=  vh  / 2 ;
	int 				sw2 =  pw   / 2 ;
	int 				sh2 =  ph  / 2 ;
		
	int					pw_34 = (pw * 3) / 4;
	int					pw_14 = pw / 4;
	
	
	int 				v0,v1,v2, v0_12, v1_12, v2_12, v0t, v2t;
	int					x_min, x_max, y_min, y_max;
		
	int					mt_00 = mi[0][0], mt_02 = mi[0][2];
		

	x_min = -w2; x_max = vw - w2;
	y_min = -h2; y_max = vh - h2;
		

	cy = 0;
	cy_max = vw * vh;
		
	cy = 0;
		
		
	if( !bilinear ){ // nearest neighbor interpolation

		m0 = mi[1][0] * y_min + mi[2][0];
		m1 = mi[1][1] * y_min + mi[2][1];
		m2 = mi[1][2] * y_min + mi[2][2];
			
			
		for(cy = 0; cy < cy_max;	cy += vw,
									m0 += mi[1][0],
									m1 += mi[1][1],
									m2 += mi[1][2])
			{
				idx = cy;
				idx_max = cy + vw;
								
				v0 = mi[0][0] * x_min + m0;
				v1 = m1; // mi[0][1] = 0
				v2 = mi[0][2] * x_min + m2;
				
				if( v1 >= 0 )
				{
					v1_12 	= v1 << 12;
	
					for(idx = cy; idx < idx_max; idx++, 
												v0 += mt_00,
												v2 += mt_02){
						if( v[idx] != 0 ) continue;
						if( v2 > 0 ){
							if( v0 > 0 ){
								if(v0 > v2){
									v[idx] = pd[ atan_LU_L1[ v1_12 / (v1 + ((v0 * sqrt_LU[ (v2_12=v2<<12) /  v0 ]) >> 12))] ]
													  [ atan_LU_L3[ NATAN - v2_12 / ( v0 + v2 )] ] 
													  | 0xff000000;
								}
								else{
									v[idx] = pd[ atan_LU_L1[ v1_12 / (v1 + ((v2 * sqrt_LU[ (v0_12=v0<<12)/  v2 ]) >> 12))] ]
													  [ atan_LU_L3[ v0_12 / ( v0 + v2 )] ] 
													  | 0xff000000;
								}
							}
							else{
								v0t = -v0;
								if(v0t > v2){
									v[idx] = pd[ atan_LU_L1[ v1_12 / (v1 + ((v0t * sqrt_LU[ (v2_12=v2<<12) /  v0t ]) >> 12))] ]
													  [ atan_LU_L4[ NATAN - v2_12 / ( v0t + v2 )] ] 
													  | 0xff000000;
								}
								else{
									v[idx] = pd[ atan_LU_L1[ v1_12 / (v1 + ((v2 * sqrt_LU[ (v0_12=v0t<<12)/  v2 ]) >> 12))] ]
													  [ atan_LU_L4[ v0_12 / ( v0t + v2 )] ] 
													  | 0xff000000;
								}
							}
						}
						else if( v2 == 0 ){
							if( v0 > 0 ){
								v[idx] = pd[ atan_LU_L1[ v1_12 / (v1 + v0)]][pw_34] | 0xff000000;
							}
							else{
								v[idx] = pd[ atan_LU_L1[ v1_12 / (v1 - v0)]][pw_14] | 0xff000000;
							}
						}
						else{	// v2 < 0
							v2t = -v2;
							if( v0 < 0 ){
								v0t = -v0;
								if(v0t > v2t){
									v[idx] = pd[ atan_LU_L1[ v1_12 / (v1 + ((v0t * sqrt_LU[ (v2_12=v2t<<12) /  v0t ]) >> 12))] ]
													  [ atan_LU_LR[ NATAN - v2_12 / ( v0t + v2t )] ] 
													  | 0xff000000;
								}
								else{
									v[idx] = pd[ atan_LU_L1[ v1_12 / (v1 + ((v2t * sqrt_LU[ (v0_12=v0t<<12)/  v2t ]) >> 12))] ]
													  [ atan_LU_LR[ v0_12 / ( v0t + v2t )] ] 
													  | 0xff000000;
								}
							}
							else{ // v0 >= 0
								if(v0 > v2t){
									v[idx] = pd[ atan_LU_L1[ v1_12 / (v1 + ((v0 * sqrt_LU[ (v2_12=v2t<<12) /  v0 ]) >> 12))] ]
													  [ atan_LU_L5[ NATAN - v2_12 / ( v0 + v2t )] ] 
													  | 0xff000000;
								}
								else{
									v[idx] = pd[ atan_LU_L1[ v1_12 / (v1 + ((v2t * sqrt_LU[ (v0_12=v0<<12)/  v2t ]) >> 12))] ]
													  [ atan_LU_L5[ v0_12 / ( v0 + v2t )] ] 
													  | 0xff000000;
								}
							}
						}
					}
				}
				else if(v1 < 0){ 
					v1 = -v1;
					v1_12 	= v1 << 12;
	
					for(idx = cy; idx < idx_max; idx++, 
												v0 += mt_00,
												v2 += mt_02){
						if( v[idx] != 0 ) continue;
						if( v2 > 0 ){
							if( v0 > 0 ){
								if(v0 > v2){
									v[idx] = pd[ atan_LU_L2[ v1_12 / (v1 + ((v0 * sqrt_LU[ (v2_12=v2<<12) /  v0 ]) >> 12) )] ] 
													  [ atan_LU_L3[ NATAN - v2_12 / ( v0 + v2 )] ] 
													  | 0xff000000;
								}
								else{
									v[idx] = pd[ atan_LU_L2[ v1_12 / (v1 + ((v2 * sqrt_LU[ (v0_12=v0<<12)/  v2 ]) >> 12))] ]
													  [ atan_LU_L3[ v0_12 / ( v0 + v2 )] ]
													  | 0xff000000;
								}
							}
							else{
								v0t = -v0;
								if(v0t > v2){
									v[idx] = pd[ atan_LU_L2[ v1_12 / (v1 + ((v0t * sqrt_LU[ (v2_12=v2<<12) /  v0t ]) >> 12) )] ]
													  [ atan_LU_L4[ NATAN - v2_12 / ( v0t + v2 )] ]
													   | 0xff000000;
								}
								else{
									v[idx] = pd[ atan_LU_L2[ v1_12 / (v1 + ((v2 * sqrt_LU[ (v0_12=v0t<<12)/  v2 ]) >> 12))] ]
													  [ atan_LU_L4[ v0_12 / ( v0t + v2 )] ]
													  | 0xff000000;
								}
							}
						}
						else if( v2 == 0 ){
							if( v0 > 0 ){
								v[idx] = pd[ atan_LU_L2[ v1_12 / (v1 + v0)]][pw_34];
							}
							else{
								v[idx] = pd[ atan_LU_L2[ v1_12 / (v1 - v0)]][pw_14];
							}
						}
						else{	// v2 < 0
							v2t = -v2;
							if( v0 < 0 ){
								v0t = -v0;
								if(v0t > v2t){
									v[idx] = pd[ atan_LU_L2[ v1_12 / (v1 + ((v0t * sqrt_LU[ (v2_12=v2t<<12) /  v0t ]) >> 12) )] ]
													  [ atan_LU_LR[ NATAN - v2_12 / ( v0t + v2t )] ]
													  | 0xff000000;
								}
								else{
									v[idx] = pd[ atan_LU_L2[ v1_12 / (v1 + ((v2t * sqrt_LU[ (v0_12=v0t<<12)/  v2t ]) >> 12))] ]
													  [ atan_LU_LR[ v0_12 / ( v0t + v2t )] ] 
													  | 0xff000000;
								}
							}
							else{ // v0 >= 0
								if(v0 > v2t){
									v[idx] = pd[ atan_LU_L2[ v1_12 / (v1 + ((v0 * sqrt_LU[ (v2_12=v2t<<12) /  v0 ]) >> 12) )] ]
													  [ atan_LU_L5[ NATAN - v2_12 / ( v0 + v2t )] ] 
													  | 0xff000000;
								}
								else{
									v[idx] = pd[ atan_LU_L2[ v1_12 / (v1 + ((v2t * sqrt_LU[ (v0_12=v0<<12)/  v2t ]) >> 12))] ]
													  [ atan_LU_L5[ v0_12 / ( v0 + v2t )] ] 
													  | 0xff000000;
								}
							}
						}
					}
				}
				else{  // v1 = 0
					for(idx = cy; idx < idx_max; idx++, 
												v0 += mt_00,
												v2 += mt_02){
						if( v[idx] != 0 ) continue;
						if( v2 > 0 ){
							if( v0 > 0 ){
								xs = atan_LU_L3[ (v0<<12) / ( v0 + v2 )];
							}
							else{
								xs =  atan_LU_L4[ ((-v0)<<12) / ( v2 - v0 )];
							}
						}
						else if( v2 == 0 ){
							if( v0 > 0 ){
								xs =  pw_34;
							}
							else{
								xs =  pw_14;
							}
						}
						else{	// v2 < 0
							if( v0 < 0 ){
								xs =  atan_LU_LR[ ((-v0)<<12) / (-v0 - v2 )] ;
							}
							else{ // v0 >= 0
								xs =  atan_LU_L5[ (v0<<12) / ( v2 - v0 )];
							}
						}

						v[idx] = pd[ 0 ][  xs ] | 0xff000000;

					}
				}

			}		

			return;
		}
		
		// Bilinear interpolation
		{
			int p00, p01, p10, p11; // Four pixels to interpolate
			int dx, dy;  // fractions
			int idp;

			m0 = mi[1][0] * y_min + mi[2][0];
			m1 = mi[1][1] * y_min + mi[2][1];
			m2 = mi[1][2] * y_min + mi[2][2];
			
			for(y = y_min; y < y_max; 	y++,
										cy += vw,
										m0 += mi[1][0],
										m1 += mi[1][1],
										m2 += mi[1][2])
			{
				idx = cy;
				v0 = mi[0][0] * x_min + m0;
				v1 = m1; // mi[0][1] = 0
				v2 = mi[0][2] * x_min + m2;
				
				for(x = x_min; x < x_max; 	x++, 
											idx++,
											v0 += mi[0][0],
											v2 += mi[0][2])

				{
					if( v[idx] != 0 ) continue;
					xs 	=  PV_atan2_HR( v0, v2 ) ;
			
					ys 	=  PV_atan2_HR( v1, PV_sqrt( Math.abs(v2), Math.abs(v0) )) ;
					
					dx = xs & 0xff; dy = ys & 0xff; // fraction			
					xs = (xs >> 8) + sw2;
					ys = (ys >> 8) + sh2;

					if( ys >= 0 && ys < miy && xs >= 0 && xs < mix )  // all interpolation pixels inside image
					{
						p00 = pd[ys]  [xs]; 
						p01 = pd[ys]  [xs+1];
						p10 = pd[ys+1][xs];
						p11 = pd[ys+1][xs+1];
					}
					else // edge pixels
					{
					 	if(ys < 0)
							idp = 0;
						else if(  ys > miy )
							idp = miy;
						else
							idp = ys;
						
										
						if( xs < 0 )
							p00 = pd[idp][mix]; 
						else if(xs > mix )
							p00 = pd[idp][0]; 
						else
							p00 = pd[idp][xs]; 
							
						xs+=1;
						if( xs < 0 )
							p01 = pd[idp][mix]; 
						else if(xs > mix )
							p01 = pd[idp][0]; 
						else
							p01 = pd[idp][xs]; 
						xs-=1;
						 
						ys+=1;
						if(ys < 0)
							idp = 0;
						else if(  ys > miy )
							idp = miy;
						else
							idp = ys;
						
										
						if( xs < 0 )
							p10 = pd[idp][mix]; 
						else if(xs > mix )
							p10 = pd[idp][0]; 
						else
							p10 = pd[idp][xs]; 
							
						xs+=1;
						if( xs < 0 )
							p11 = pd[idp][mix]; 
						else if(xs > mix )
							p11 = pd[idp][0]; 
						else
							p11 = pd[idp][xs]; 
					}
					v[idx] = bil(p00, p01, p10, p11, dx, dy);
					hv[idx] = (byte) (p00 >> 24);
				
				}
			}
			return;
		}
	
	}


final double[] math_view2pano( int xv, int yv, int vw, int vh,
                            int pw, int ph,
                            double pan, double tilt, double fov){
	double		a,p,dr;							// field of view in rad
	int 		i,k;
	double		v0,v1,v2;

	double dist_e = (double)pw / (2.0 * Math.PI);	
	a  =  fov * 2.0 * Math.PI / 360.0;	// field of view in rad		
	p  = (double)vw / (2.0 * Math.tan( a / 2.0 ) );
	dr = (int)(p+.5);

	SetMatrix( 	tilt * 2.0 * Math.PI / 360.0, 
				pan   * 2.0 * Math.PI / 360.0, 
				mt, 
				1 );
		
	xv -= vw / 2;
	yv -= vh / 2;
		
	v0 = mt[0][0] * xv + mt[1][0] * yv + mt[2][0]*dr;
	v1 = mt[0][1] * xv + mt[1][1] * yv + mt[2][1]*dr;
	v2 = mt[0][2] * xv + mt[1][2] * yv + mt[2][2]*dr;

	double[] xp = new double[2];
		
	xp[0] = dist_e * Math.atan2( v0, v2 ) + pw / 2.0;
	xp[1] = dist_e * Math.atan2( v1, Math.sqrt( v2 * v2 + v0 * v0 )) + ph / 2.0;	
	
	return xp;	
}


// Calculate coordinates in panorama

final int[] math_int_view2pano( int xv, int yv, int vw, int vh,
                            int pw, int ph,
                            double pan, double tilt, double fov){
	double xp[] = math_view2pano(xv, yv, vw, vh, pw, ph, pan, tilt, fov);
	if(xp[0]<0) xp[0]=0; if(xp[0]>=pw) xp[0]=pw-1;
	if(xp[1]<0) xp[1]=0; if(xp[1]>=ph) xp[1]=ph-1;

	int[] cp = new int[2];
		
	cp[0] = (int)xp[0];
	cp[1] = (int)xp[1];
		
	return cp;	
}


// calculate vertical field of view
    		
final double math_fovy( double Hfov,  int vw, int vh){
   		return (360.0 / Math.PI) * Math.atan( (double)vh/(double)vw * Math.tan( Hfov/2.0 * Math.PI/180.0 ));
}
                            

Back