src/panolib.c

   1 /* Panorama_Tools       -       Generate, Edit and Convert Panoramic Images
   2    Copyright (C) 1998,1999 - Helmut Dersch  der@fh-furtwangen.de
   3
   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)
   7    any later version.
   8
   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.
  13
  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.  */
  17
  18 /*------------------------------------------------------------*/
  19 #include <stdlib.h>
  20 #include <stdint.h>
  21 #include <assert.h>
  22
  23 #include "filter.h"
  24 #include "utils_math.h"
  25
  26
  27 // Lookup Tables for Trig-functions and interpolator
  28
  29 #define NATAN 2048
  30 #define NSQRT 2048
  31
  32 int *atan_LU;
  33 int *sqrt_LU;
  34 int *mweights[256];
  35
  36
  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 );
  41
  42
  43 /*
  44  * Extract image from pano in TrPtr->src using parameters in prefs (ignore
  45  * image parameters in TrPtr)
  46  */
  47 void PV_ExtractStill( TrformStr *TrPtr )
  48 {
  49         /* field of view in rad */
  50         double a;
  51         double b;
  52
  53         double      p[2];
  54         double          mt[3][3];
  55         int             mi[3][3],i,k;
  56
  57         a =      DEG_TO_RAD( TrPtr->dest->hfov );       // field of view in rad
  58         b =      DEG_TO_RAD( TrPtr->src->hfov );
  59
  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 */
  63                         0.0, /* gamma */
  64                         mt, /* output */
  65                         1);
  66
  67
  68         p[0] = (double)TrPtr->dest->width/ (2.0 * tan( a / 2.0 ) );
  69         p[1] = (double) TrPtr->src->width / b;
  70
  71         for(i=0; i<3; i++){
  72                 for(k=0; k<3; k++){
  73                         mi[i][k] = 256 * mt[i][k];
  74                 }
  75         }
  76
  77
  78         PV_transForm( TrPtr, p[0], p[1], mi);
  79         return;
  80 }
  81
  82 typedef enum interpolator_e
  83 {
  84         NNEIGHBOUR,
  85         BILINEAR
  86 } interpolator_t;
  87
  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)
  92 {
  93         uint32_t pixel_value;
  94         uint32_t *src_data  = (uint32_t *) (*src->data);
  95         uint32_t *dest_data = (uint32_t *) (*dest->data);
  96
  97         interp = NNEIGHBOUR;
  98
  99         if (interp == NNEIGHBOUR)
 100         {
 101                 int x_src = (int) (x_src_fp + 0.5) % src->width;
 102                 int y_src = (int) (y_src_fp + 0.5) % src->height;
 103
 104                 if ((x_src < 0) || (x_src >= src->width)
 105                                 || (y_src < 0) || (y_src >= src->height))
 106                         pixel_value = 0x000000FF;
 107                 else
 108                         pixel_value = src_data[(y_src * src->width) + x_src];
 109         }
 110
 111         dest_data[(y_dest * dest->width) + x_dest] = pixel_value;
 112
 113         return (0);
 114 } /* int copy_pixel */
 115
 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!
 120
 121 void PV_transForm( TrformStr *TrPtr, double dist_r, double dist_e, int mt[3][3])
 122 {
 123         int x_dest, y_dest;                     // Loop through destination image
 124         unsigned char           *dest,*src;// Source and destination image data
 125
 126         // Variables used to convert screen coordinates to cartesian coordinates
 127
 128
 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 ;
 133
 134         int                             v[3];
 135         int                                     x_min, x_max, y_min, y_max;
 136
 137         int                                     dr1, dr2, dr3;
 138
 139         dr1 = mt[2][0] * dist_r;
 140         dr2 = mt[2][1] * dist_r;
 141         dr3 = mt[2][2] * dist_r;
 142
 143         dest = *TrPtr->dest->data;
 144         src  = *TrPtr->src->data; // is locked
 145
 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;
 148
 149         for(y_dest = y_min; y_dest < y_max; y_dest++)
 150         {
 151                 for(x_dest = x_min; x_dest < x_max; x_dest++)
 152                 {
 153                         double x_src_fp;
 154                         double y_src_fp;
 155
 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;
 159
 160                         v[0] = v[0] >> 8; v[2] = v[2] >> 8;
 161                         v[1] = v[1] >> 8;
 162
 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]));
 165
 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,
 169                                         NNEIGHBOUR);
 170                 }
 171         }
 172
 173         return;
 174 }
 175
 176 #if 0
 177 void matrix_inv_mult( double m[3][3], double vector[3] )
 178 {
 179         register int i;
 180         register double v0 = vector[0];
 181         register double v1 = vector[1];
 182         register double v2 = vector[2];
 183
 184         for(i=0; i<3; i++)
 185         {
 186                 vector[i] = m[0][i] * v0 + m[1][i] * v1 + m[2][i] * v2;
 187         }
 188 }
 189 #endif
 190
 191 // Set matrix elements based on Euler angles a, b, c
 192
 193 void SetMatrix( double a, double b, double c , double m[3][3], int cl )
 194 {
 195         double mx[3][3], my[3][3], mz[3][3], dummy[3][3];
 196
 197
 198         // Calculate Matrices;
 199
 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];
 203
 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];
 207
 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;
 211
 212         /* Calculate `m = mz * mx * my' */
 213
 214         if( cl )
 215                 matrix_matrix_mult( mz, mx,     dummy);
 216         else
 217                 matrix_matrix_mult( mx, mz,     dummy);
 218         matrix_matrix_mult( dummy, my, m);
 219 } /* void SetMatrix */
 220
 221 void matrix_matrix_mult( double m1[3][3],double m2[3][3],double result[3][3])
 222 {
 223         register int i,k;
 224
 225         for(i=0;i<3;i++)
 226                 for(k=0; k<3; k++)
 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 */
 229
 230 #if 0
 231 static int orig_PV_atan2(int y, int x)
 232 {
 233         // return atan2(y,x) * 256*NATAN;
 234         if( x > 0 )
 235         {
 236                 if( y > 0 )
 237                 {
 238                         return  atan_LU[(int)( NATAN * y / ( x + y ))];
 239                 }
 240                 else
 241                 {
 242                         return -atan_LU[ (int)(NATAN * (-y) / ( x - y ))];
 243                 }
 244         }
 245
 246         if( x == 0 )
 247         {
 248                 if( y > 0 )
 249                         return  (int)(256*NATAN*PI / 2.0);
 250                 else
 251                         return  -(int)(256*NATAN*PI / 2.0);
 252         }
 253
 254         if( y < 0 )
 255         {
 256                 return  atan_LU[(int)( NATAN * y / ( x + y ))] - (int)(PI*256*NATAN);
 257         }
 258         else
 259         {
 260                 return -atan_LU[ (int)(NATAN * (-y) / ( x - y ))] + (int)(PI*256*NATAN);
 261         }
 262
 263 } /* int orig_PV_atan2 */
 264 #endif
 265
 266 int PV_atan2(int y, int x)
 267 {
 268         double ret = atan2 (y, x) * NATAN * 256.0;
 269         return ((int) ret);
 270 } /* int PV_atan2 */
 271
 272 int SetUpAtan()
 273 {
 274         int i;
 275         double dz = 1.0 / (double)NATAN;
 276         double z = 0.0;
 277
 278         atan_LU = (int*) malloc( (NATAN+1) * sizeof( int ));
 279
 280         if( atan_LU == NULL )
 281                 return -1;
 282
 283         for( i=0; i< NATAN; i++, z+=dz )
 284                 atan_LU[i] = atan( z / (1.0 - z ) ) * NATAN * 256;
 285
 286         atan_LU[NATAN] = PI/4.0 * NATAN * 256;
 287
 288         // Print a test
 289 #if 0
 290         for(i = -10; i< 10; i++)
 291         {
 292                 int k;
 293                 for(k=-10; k<10; k++)
 294                 {
 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));
 297                 }
 298         }
 299         exit(0);
 300 #endif
 301         return 0;
 302 }
 303
 304 int SetUpSqrt()
 305 {
 306         int i;
 307         double dz = 1.0 / (double)NSQRT;
 308         double z = 0.0;
 309
 310         sqrt_LU = (int*) malloc( (NSQRT+1) * sizeof( int ));
 311
 312         if( sqrt_LU == NULL )
 313                 return -1;
 314
 315         for( i=0; i< NSQRT; i++, z+=dz )
 316                 sqrt_LU[i] = sqrt( 1.0 + z*z ) * 256 * NSQRT;
 317
 318         sqrt_LU[NSQRT] = sqrt(2.0) * 256 * NSQRT;
 319
 320         return 0;
 321 }
 322
 323 int SetUpMweights()
 324 {
 325         int i,k;
 326
 327         for(i=0; i<256; i++)
 328         {
 329                 mweights[i] = (int*)malloc( 256 * sizeof(int) );
 330                 if( mweights[i] == NULL ) return -1;
 331         }
 332         for(i=0; i<256; i++)
 333         {
 334                 for(k=0; k<256; k++)
 335                 {
 336                         mweights[i][k] = i*k;
 337                 }
 338         }
 339
 340         return 0;
 341 }
 342
 343
 344 int PV_sqrt( int x1, int x2 )
 345 {
 346         if( x1 > x2 )
 347         {
 348                 return  x1 * sqrt_LU[ NSQRT * x2 /  x1 ] / NSQRT;
 349         }
 350         else
 351         {
 352                 if( x2 == 0 ) return 0;
 353                 return x2 * sqrt_LU[ NSQRT * x1 /  x2 ] / NSQRT;
 354         }
 355 }
 356
 357
 358 #define ID_0 0xff
 359 #define ID_1 0xd8
 360 #define ID_2 0xff
 361 #define ID_3 0xe0
 362 #define ID_4 0x00
 363 #define ID_5 0x10
 364 #define ID_6 0x4a
 365 #define ID_7 0x46
 366 #define ID_8 0x49
 367 #define ID_9 0x46
 368 #define ID_10 0x00
 369
 370 #define ID_LENGTH 11
 371
 372 // Find last jpeg inside image; create and copy to file jpeg
 373 int extractJPEG( fullPath *image, fullPath *jpeg )
 374 {
 375         file_spec                               fnum;
 376         long                                    count;
 377         unsigned char*                  im;
 378         int                                     i, idx = -1;
 379         unsigned char                   ch;
 380
 381         if( myopen( image, read_bin, fnum ) )
 382                 return -1;
 383
 384
 385         count = 1; i=0; // Get file length
 386
 387         while( count == 1 )
 388         {
 389                 myread(  fnum, count, &ch );
 390                 if(count==1) i++;
 391         }
 392         myclose(fnum);
 393
 394         count = i;
 395
 396         im = (UCHAR*)malloc( count );
 397         if( im == NULL )
 398         {
 399                 PrintError("Not enough memory");
 400                 return -1;
 401         }
 402
 403         if( myopen( image, read_bin, fnum ) )
 404                 return -1;
 405
 406         myread(fnum,count,im);
 407         myclose(fnum);
 408
 409         if( i != count )
 410                 return -1;
 411
 412         count -= ID_LENGTH;
 413
 414         for(i=0; i<count; i++)
 415         {
 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)
 419                                 idx = i;
 420         }
 421
 422         if( idx == -1 ) // No jpeg found
 423         {
 424                 free(im);
 425                 return -1;
 426         }
 427
 428         count = count + ID_LENGTH - idx;
 429
 430         mycreate( jpeg, 'GKON','JPEG');
 431         if( myopen( jpeg, write_bin, fnum ) )
 432                 return -1;
 433         mywrite( fnum, count, im+idx );
 434         free( im );
 435         myclose( fnum );
 436         return 0;
 437 }
 438
 439
 440 /*
 441  * vim: set tabstop=4 softtabstop=4 shiftwidth=4 :
 442  */