1 /* Panorama_Tools - Generate, Edit and Convert Panoramic Images
2 Copyright (C) 1998,1999 - Helmut Dersch der@fh-furtwangen.de
4 This program is free software; you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation; either version 2, or (at your option)
9 This program is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
14 You should have received a copy of the GNU General Public License
15 along with this program; if not, write to the Free Software
16 Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. */
18 /*------------------------------------------------------------*/
24 #include "utils_math.h"
27 // Lookup Tables for Trig-functions and interpolator
37 void matrix_matrix_mult ( double m1[3][3],double m2[3][3],double result[3][3]);
38 void PV_transForm( TrformStr *TrPtr, double dist_r, double dist_e, int mt[3][3]);
39 int PV_atan2(int y, int x);
40 int PV_sqrt( int x1, int x2 );
44 * Extract image from pano in TrPtr->src using parameters in prefs (ignore
45 * image parameters in TrPtr)
47 void PV_ExtractStill( TrformStr *TrPtr )
49 /* field of view in rad */
57 a = DEG_TO_RAD( TrPtr->dest->hfov ); // field of view in rad
58 b = DEG_TO_RAD( TrPtr->src->hfov );
60 /* Set up the transformation matrix `mt' using Euler angles (somehow..) */
61 SetMatrix (DEG_TO_RAD (TrPtr->dest->pitch), /* alpha */
62 DEG_TO_RAD (TrPtr->dest->yaw), /* beta */
68 p[0] = (double)TrPtr->dest->width/ (2.0 * tan( a / 2.0 ) );
69 p[1] = (double) TrPtr->src->width / b;
73 mi[i][k] = 256 * mt[i][k];
78 PV_transForm( TrPtr, p[0], p[1], mi);
82 typedef enum interpolator_e
88 static int copy_pixel (Image *dest, Image *src,
89 int x_dest, int y_dest,
90 double x_src_fp, double y_src_fp,
91 interpolator_t interp)
94 uint32_t *src_data = (uint32_t *) (*src->data);
95 uint32_t *dest_data = (uint32_t *) (*dest->data);
99 if (interp == NNEIGHBOUR)
101 int x_src = (int) (x_src_fp + 0.5) % src->width;
102 int y_src = (int) (y_src_fp + 0.5) % src->height;
104 if ((x_src < 0) || (x_src >= src->width)
105 || (y_src < 0) || (y_src >= src->height))
106 pixel_value = 0x000000FF;
108 pixel_value = src_data[(y_src * src->width) + x_src];
111 dest_data[(y_dest * dest->width) + x_dest] = pixel_value;
114 } /* int copy_pixel */
116 // Main transformation function. Destination image is calculated using transformation
117 // Function "func". Either all colors (color = 0) or one of rgb (color =1,2,3) are
118 // determined. If successful, TrPtr->success = 1. Memory for destination image
119 // must have been allocated and locked!
121 void PV_transForm( TrformStr *TrPtr, double dist_r, double dist_e, int mt[3][3])
123 int x_dest, y_dest; // Loop through destination image
124 unsigned char *dest,*src;// Source and destination image data
126 // Variables used to convert screen coordinates to cartesian coordinates
129 int dest_width_left = TrPtr->dest->width / 2 ;
130 int dest_height_top = TrPtr->dest->height / 2 ;
131 int src_width_left = TrPtr->src->width / 2 ;
132 int src_height_top = TrPtr->src->height / 2 ;
135 int x_min, x_max, y_min, y_max;
139 dr1 = mt[2][0] * dist_r;
140 dr2 = mt[2][1] * dist_r;
141 dr3 = mt[2][2] * dist_r;
143 dest = *TrPtr->dest->data;
144 src = *TrPtr->src->data; // is locked
146 x_min = -dest_width_left; x_max = TrPtr->dest->width - dest_width_left;
147 y_min = -dest_height_top; y_max = TrPtr->dest->height - dest_height_top;
149 for(y_dest = y_min; y_dest < y_max; y_dest++)
151 for(x_dest = x_min; x_dest < x_max; x_dest++)
156 v[0] = mt[0][0] * x_dest + mt[1][0] * y_dest + dr1;
157 v[1] = mt[0][1] * x_dest + mt[1][1] * y_dest + dr2;
158 v[2] = mt[0][2] * x_dest + mt[1][2] * y_dest + dr3;
160 v[0] = v[0] >> 8; v[2] = v[2] >> 8;
163 x_src_fp = dist_e * atan2 (v[0], v[2]);
164 y_src_fp = dist_e * atan2 (v[1], sqrt (v[2] * v[2] + v[0] * v[0]));
166 copy_pixel (TrPtr->dest, TrPtr->src,
167 dest_width_left + x_dest, dest_height_top + y_dest,
168 src_width_left + x_src_fp, src_height_top + y_src_fp,
177 void matrix_inv_mult( double m[3][3], double vector[3] )
180 register double v0 = vector[0];
181 register double v1 = vector[1];
182 register double v2 = vector[2];
186 vector[i] = m[0][i] * v0 + m[1][i] * v1 + m[2][i] * v2;
191 // Set matrix elements based on Euler angles a, b, c
193 void SetMatrix( double a, double b, double c , double m[3][3], int cl )
195 double mx[3][3], my[3][3], mz[3][3], dummy[3][3];
198 // Calculate Matrices;
200 mx[0][0] = 1.0 ; mx[0][1] = 0.0 ; mx[0][2] = 0.0;
201 mx[1][0] = 0.0 ; mx[1][1] = cos(a) ; mx[1][2] = sin(a);
202 mx[2][0] = 0.0 ; mx[2][1] =-mx[1][2] ; mx[2][2] = mx[1][1];
204 my[0][0] = cos(b); my[0][1] = 0.0 ; my[0][2] =-sin(b);
205 my[1][0] = 0.0 ; my[1][1] = 1.0 ; my[1][2] = 0.0;
206 my[2][0] = -my[0][2]; my[2][1] = 0.0 ; my[2][2] = my[0][0];
208 mz[0][0] = cos(c) ; mz[0][1] = sin(c) ; mz[0][2] = 0.0;
209 mz[1][0] =-mz[0][1] ; mz[1][1] = mz[0][0] ; mz[1][2] = 0.0;
210 mz[2][0] = 0.0 ; mz[2][1] = 0.0 ; mz[2][2] = 1.0;
212 /* Calculate `m = mz * mx * my' */
215 matrix_matrix_mult( mz, mx, dummy);
217 matrix_matrix_mult( mx, mz, dummy);
218 matrix_matrix_mult( dummy, my, m);
219 } /* void SetMatrix */
221 void matrix_matrix_mult( double m1[3][3],double m2[3][3],double result[3][3])
227 result[i][k] = m1[i][0] * m2[0][k] + m1[i][1] * m2[1][k] + m1[i][2] * m2[2][k];
228 } /* void matrix_matrix_mult */
231 static int orig_PV_atan2(int y, int x)
233 // return atan2(y,x) * 256*NATAN;
238 return atan_LU[(int)( NATAN * y / ( x + y ))];
242 return -atan_LU[ (int)(NATAN * (-y) / ( x - y ))];
249 return (int)(256*NATAN*PI / 2.0);
251 return -(int)(256*NATAN*PI / 2.0);
256 return atan_LU[(int)( NATAN * y / ( x + y ))] - (int)(PI*256*NATAN);
260 return -atan_LU[ (int)(NATAN * (-y) / ( x - y ))] + (int)(PI*256*NATAN);
263 } /* int orig_PV_atan2 */
266 int PV_atan2(int y, int x)
268 double ret = atan2 (y, x) * NATAN * 256.0;
275 double dz = 1.0 / (double)NATAN;
278 atan_LU = (int*) malloc( (NATAN+1) * sizeof( int ));
280 if( atan_LU == NULL )
283 for( i=0; i< NATAN; i++, z+=dz )
284 atan_LU[i] = atan( z / (1.0 - z ) ) * NATAN * 256;
286 atan_LU[NATAN] = PI/4.0 * NATAN * 256;
290 for(i = -10; i< 10; i++)
293 for(k=-10; k<10; k++)
295 printf("i = %d k = %d atan2(i,k) = %g LUatan(i,k) = %g diff = %g\n", i,k,atan2(i,k),
296 (double)PV_atan2(i,k) / (256*NATAN) , atan2(i,k) - (double)PV_atan2(i,k) / (256*NATAN));
307 double dz = 1.0 / (double)NSQRT;
310 sqrt_LU = (int*) malloc( (NSQRT+1) * sizeof( int ));
312 if( sqrt_LU == NULL )
315 for( i=0; i< NSQRT; i++, z+=dz )
316 sqrt_LU[i] = sqrt( 1.0 + z*z ) * 256 * NSQRT;
318 sqrt_LU[NSQRT] = sqrt(2.0) * 256 * NSQRT;
329 mweights[i] = (int*)malloc( 256 * sizeof(int) );
330 if( mweights[i] == NULL ) return -1;
336 mweights[i][k] = i*k;
344 int PV_sqrt( int x1, int x2 )
348 return x1 * sqrt_LU[ NSQRT * x2 / x1 ] / NSQRT;
352 if( x2 == 0 ) return 0;
353 return x2 * sqrt_LU[ NSQRT * x1 / x2 ] / NSQRT;
372 // Find last jpeg inside image; create and copy to file jpeg
373 int extractJPEG( fullPath *image, fullPath *jpeg )
381 if( myopen( image, read_bin, fnum ) )
385 count = 1; i=0; // Get file length
389 myread( fnum, count, &ch );
396 im = (UCHAR*)malloc( count );
399 PrintError("Not enough memory");
403 if( myopen( image, read_bin, fnum ) )
406 myread(fnum,count,im);
414 for(i=0; i<count; i++)
416 if( im[i] == ID_0 && im[i+1] == ID_1 && im[i+2] == ID_2 && im[i+3] == ID_3
417 && im[i+4] == ID_4 && im[i+5] == ID_5 && im[i+6] == ID_6 && im[i+7] == ID_7
418 && im[i+8] == ID_8 && im[i+9] == ID_9 && im[i+10] == ID_10)
422 if( idx == -1 ) // No jpeg found
428 count = count + ID_LENGTH - idx;
430 mycreate( jpeg, 'GKON','JPEG');
431 if( myopen( jpeg, write_bin, fnum ) )
433 mywrite( fnum, count, im+idx );
441 * vim: set tabstop=4 softtabstop=4 shiftwidth=4 :