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 "filter.h"
  20
  21
  22 // Lookup Tables for Trig-functions and interpolator
  23
  24 #define NATAN 2048
  25 #define NSQRT 2048
  26
  27 int *atan_LU;
  28 int *sqrt_LU;
  29 int *mweights[256];
  30
  31
  32 void    matrix_matrix_mult      ( double m1[3][3],double m2[3][3],double result[3][3]);
  33 void    PV_transForm( TrformStr *TrPtr, int dist_r, int dist_e, int mt[3][3]);
  34 int     PV_atan2(int y, int x);
  35 int     PV_sqrt( int x1, int x2 );
  36
  37
  38 // Bilinear interpolator
  39
  40 static void bil( unsigned char *dst, unsigned char **rgb,
  41                 int dx,int dy)
  42 {
  43         int yr, yg, yb,weight;
  44         int rd, gd, bd ;
  45         register unsigned char *r;
  46         int *w1, *w2;
  47
  48
  49         w1 = mweights[dx]; w2 = mweights[255 - dx];
  50
  51         r = rgb[0]  + 1;
  52
  53         rd = w2[*r++]; gd = w2[*r++];   bd = w2[*r++];
  54         //weight = 255 - dx; rd = weight * *r++; gd = weight * *r++;    bd = weight * *r++;
  55
  56         r++;
  57         rd += w1[*r++]; gd += w1[*r++]; bd += w1[*r];
  58         //rd += dx * *r++; gd += dx * *r++;     bd += dx * *r;
  59
  60
  61         r = rgb[1]  + 1;
  62
  63         yr = w2[*r++]; yg = w2[*r++];   yb = w2[*r++];
  64         //rd = weight * *r++; gd = weight * *r++;       bd = weight * *r++;
  65
  66         r++;
  67         yr += w1[*r++]; yg += w1[*r++]; yb += w1[*r];
  68         //rd += dx * *r++; gd += dx * *r++;     bd += dx * *r;
  69
  70         weight = 255 - dy;
  71         rd = rd * weight + yr * dy;
  72         gd = gd * weight + yg * dy;
  73         bd = bd * weight + yb * dy;
  74
  75         *dst++ = rd >> 16;
  76         *dst++ = gd >> 16;
  77         *dst   = bd >> 16;
  78 }
  79
  80
  81
  82 // Extract image from pano in TrPtr->src
  83 // using parameters in prefs (ignore image parameters
  84 // in TrPtr)
  85
  86 void PV_ExtractStill( TrformStr *TrPtr )
  87 {
  88         double          a,b;                                                    // field of view in rad
  89         double      p[2];
  90         double          mt[3][3];
  91         int             mi[3][3],i,k;
  92
  93         TrPtr->success = 1;
  94
  95         a =      DEG_TO_RAD( TrPtr->dest->hfov );       // field of view in rad
  96         b =      DEG_TO_RAD( TrPtr->src->hfov );
  97
  98         SetMatrix(      DEG_TO_RAD( TrPtr->dest->pitch ),
  99                                 DEG_TO_RAD( TrPtr->dest->yaw ),
 100                                 0.0 ,
 101                                 mt,
 102                                 1 );
 103
 104
 105         p[0] = (double)TrPtr->dest->width/ (2.0 * tan( a / 2.0 ) );
 106         p[1] = (double)TrPtr->src->width/ b;
 107
 108         for(i=0; i<3; i++){
 109                 for(k=0; k<3; k++){
 110                         mi[i][k] = 256 * mt[i][k];
 111                 }
 112         }
 113
 114
 115         PV_transForm( TrPtr,  (int)(p[0]+.5), (int)(p[1]+.5), mi);
 116         return;
 117 }
 118
 119
 120
 121
 122
 123 //    Main transformation function. Destination image is calculated using transformation
 124 //    Function "func". Either all colors (color = 0) or one of rgb (color =1,2,3) are
 125 //    determined. If successful, TrPtr->success = 1. Memory for destination image
 126 //    must have been allocated and locked!
 127
 128 void PV_transForm( TrformStr *TrPtr, int dist_r, int dist_e, int mt[3][3])
 129 {
 130         register int            x, y;                   // Loop through destination image
 131         unsigned char           *dest,*src,*sry, *dst;// Source and destination image data
 132         long                            cy;                             // rownum in destimage
 133         int                                     dx,dy;
 134
 135         int                             xc;             // Cartesian Coordinates of point ("target") in Destination image
 136         int                             xs, ys;
 137
 138         unsigned char           *rgb[2] ,
 139                                                 cdata[16];      // Image data handed to sampler
 140
 141
 142         int                                     mix       = TrPtr->src->width - 1; // maximum x-index src
 143         int                                     miy       = TrPtr->src->height - 1;// maximum y-index src
 144
 145         // Variables used to convert screen coordinates to cartesian coordinates
 146
 147
 148         int                             w2      =  TrPtr->dest->width  / 2 ;
 149         int                             h2      =  TrPtr->dest->height / 2 ;
 150         int                             sw2 =  TrPtr->src->width   / 2 ;
 151         int                             sh2 =  TrPtr->src->height  / 2 ;
 152
 153         int                                     BytesPerLine    = TrPtr->src->bytesPerLine;
 154         int                             v[3];
 155         int                                     x_min, x_max, y_min, y_max;
 156
 157         int                                     dr1, dr2, dr3;
 158
 159         dr1 = mt[2][0] * dist_r;
 160         dr2 = mt[2][1] * dist_r;
 161         dr3 = mt[2][2] * dist_r;
 162
 163
 164
 165
 166
 167         dest = *TrPtr->dest->data;
 168         src  = *TrPtr->src->data; // is locked
 169
 170         x_min = -w2; x_max = TrPtr->dest->width - w2;
 171         y_min = -h2; y_max = TrPtr->dest->height - h2;
 172
 173         cy = 0;
 174
 175         if( TrPtr->interpolator == _bilinear )
 176         {
 177                 for(y = y_min; y < y_max; y++, cy+=TrPtr->dest->bytesPerLine)
 178                 {
 179                         dst = dest + cy + 1;
 180                         for(x = x_min; x < x_max; x++, dst+=4)
 181                         {
 182                                 v[0] = mt[0][0] * x + mt[1][0] * y + dr1;
 183                                 v[1] = mt[0][1] * x + mt[1][1] * y + dr2;
 184                                 v[2] = mt[0][2] * x + mt[1][2] * y + dr3;
 185
 186                                 v[0] = v[0] >> 8; v[2] = v[2] >> 8;
 187
 188                                 xs = dist_e * PV_atan2( v[0], v[2] ) / NATAN ;
 189
 190                                 ys = dist_e * PV_atan2( v[1], PV_sqrt( abs(v[2]), abs(v[0]) ) ) / NATAN ;
 191
 192                                 dx = xs & 255; dy = ys & 255; // fraction
 193
 194
 195                                 xs = (xs >> 8) + sw2;
 196                                 ys = (ys >> 8) + sh2;
 197
 198
 199                                 if( ys >= 0 && ys < miy && xs >= 0 && xs < mix )  // all interpolation pixels inside image
 200                                                                                                                                           // (most pixels)
 201                                 {
 202                                         sry = src + ys * BytesPerLine + xs * 4;
 203                                         rgb[0] = sry;
 204                                         rgb[1] = sry + BytesPerLine;
 205                                 }
 206                                 else // edge pixels
 207                                 {
 208                                         xc = xs;
 209
 210                                         rgb[0] = cdata;
 211                                         if( ys < 0 )
 212                                                 sry = src;
 213                                         else if( ys > miy )
 214                                                 sry = src + miy * BytesPerLine;
 215                                         else
 216                                                 sry = src + ys  * BytesPerLine;
 217
 218                                         if( xs < 0 )  xs = mix;
 219                                         if( xs > mix) xs = 0;
 220                                         *(long*)rgb[0] = *(long*)(sry + xs*4);
 221
 222                                         xs = xc+1;
 223                                         if( xs < 0 )  xs = mix;
 224                                         if( xs > mix) xs = 0;
 225                                         *(long*)(rgb[0]+4) = *(long*)(sry + xs*4);
 226
 227
 228
 229                                         rgb[1] = cdata+8;
 230                                         ys+=1;
 231                                         if( ys < 0 )
 232                                                 sry = src;
 233                                         else if( ys > miy )
 234                                                 sry = src + miy * BytesPerLine;
 235                                         else
 236                                                 sry = src + ys  * BytesPerLine;
 237                                         xs = xc;
 238                                         if( xs < 0 )  xs = mix;
 239                                         if( xs > mix) xs = 0;
 240                                         *(long*)rgb[1] = *(long*)(sry + xs*4);
 241                                         xs = xc+1;
 242                                         if( xs < 0 )  xs = mix;
 243                                         if( xs > mix) xs = 0;
 244                                         *(long*)(rgb[1]+4) = *(long*)(sry + xs*4);
 245
 246
 247                                 }
 248                                 bil( dst, rgb, dx, dy );
 249                         }
 250                 }
 251         }
 252         else if(  TrPtr->interpolator   == _nn )
 253         {
 254                 for(y = y_min; y < y_max; y++, cy+=TrPtr->dest->bytesPerLine)
 255                 {
 256                         dst = dest + cy;
 257                         for(x = x_min; x < x_max; x++, dst+=4)
 258                         {
 259                                 v[0] = mt[0][0] * x + mt[1][0] * y + dr1;
 260                                 v[1] = mt[0][1] * x + mt[1][1] * y + dr2;
 261                                 v[2] = mt[0][2] * x + mt[1][2] * y + dr3;
 262
 263                                 v[0] = v[0] >> 8; v[2] = v[2] >> 8;
 264
 265                                 xs      = dist_e * PV_atan2( v[0], v[2] ) / NATAN ;
 266
 267                                 ys      = dist_e * PV_atan2( v[1], PV_sqrt( abs(v[2]), abs(v[0]) ) ) / NATAN ;
 268
 269                                 dx      = xs & 255; dy = ys & 255; // fraction
 270
 271
 272                                 xs = (xs >> 8) + sw2;
 273                                 ys = (ys >> 8) + sh2;
 274
 275                                 if( xs < 0 )    xs = 0;
 276                                 if( xs > mix )  xs = mix;
 277                                 if( ys < 0)     ys = 0;
 278                                 if( ys > miy )  ys = miy;
 279
 280                                 *(long*)dst = *(long*)(src + ys * BytesPerLine + xs * 4);
 281                         }
 282                 }
 283         }
 284
 285         TrPtr->success = 1;
 286         return;
 287 }
 288
 289
 290 void matrix_inv_mult( double m[3][3], double vector[3] )
 291 {
 292         register int i;
 293         register double v0 = vector[0];
 294         register double v1 = vector[1];
 295         register double v2 = vector[2];
 296
 297         for(i=0; i<3; i++)
 298         {
 299                 vector[i] = m[0][i] * v0 + m[1][i] * v1 + m[2][i] * v2;
 300         }
 301 }
 302
 303 // Set matrix elements based on Euler angles a, b, c
 304
 305 void SetMatrix( double a, double b, double c , double m[3][3], int cl )
 306 {
 307         double mx[3][3], my[3][3], mz[3][3], dummy[3][3];
 308
 309
 310         // Calculate Matrices;
 311
 312         mx[0][0] = 1.0 ;                                mx[0][1] = 0.0 ;                                mx[0][2] = 0.0;
 313         mx[1][0] = 0.0 ;                                mx[1][1] = cos(a) ;                     mx[1][2] = sin(a);
 314         mx[2][0] = 0.0 ;                                mx[2][1] =-mx[1][2] ;                   mx[2][2] = mx[1][1];
 315
 316         my[0][0] = cos(b);                              my[0][1] = 0.0 ;                                my[0][2] =-sin(b);
 317         my[1][0] = 0.0 ;                                my[1][1] = 1.0 ;                                my[1][2] = 0.0;
 318         my[2][0] = -my[0][2];                   my[2][1] = 0.0 ;                                my[2][2] = my[0][0];
 319
 320         mz[0][0] = cos(c) ;                     mz[0][1] = sin(c) ;                     mz[0][2] = 0.0;
 321         mz[1][0] =-mz[0][1] ;                   mz[1][1] = mz[0][0] ;                   mz[1][2] = 0.0;
 322         mz[2][0] = 0.0 ;                                mz[2][1] = 0.0 ;                                mz[2][2] = 1.0;
 323
 324         if( cl )
 325                 matrix_matrix_mult( mz, mx,     dummy);
 326         else
 327                 matrix_matrix_mult( mx, mz,     dummy);
 328         matrix_matrix_mult( dummy, my, m);
 329 }
 330
 331 void matrix_matrix_mult( double m1[3][3],double m2[3][3],double result[3][3])
 332 {
 333         register int i,k;
 334
 335         for(i=0;i<3;i++)
 336         {
 337                 for(k=0; k<3; k++)
 338                 {
 339                         result[i][k] = m1[i][0] * m2[0][k] + m1[i][1] * m2[1][k] + m1[i][2] * m2[2][k];
 340                 }
 341         }
 342 }
 343
 344
 345
 346 int PV_atan2(int y, int x)
 347 {
 348         // return atan2(y,x) * 256*NATAN;
 349         if( x > 0 )
 350         {
 351                 if( y > 0 )
 352                 {
 353                         return  atan_LU[(int)( NATAN * y / ( x + y ))];
 354                 }
 355                 else
 356                 {
 357                         return -atan_LU[ (int)(NATAN * (-y) / ( x - y ))];
 358                 }
 359         }
 360
 361         if( x == 0 )
 362         {
 363                 if( y > 0 )
 364                         return  (int)(256*NATAN*PI / 2.0);
 365                 else
 366                         return  -(int)(256*NATAN*PI / 2.0);
 367         }
 368
 369         if( y < 0 )
 370         {
 371                 return  atan_LU[(int)( NATAN * y / ( x + y ))] - (int)(PI*256*NATAN);
 372         }
 373         else
 374         {
 375                 return -atan_LU[ (int)(NATAN * (-y) / ( x - y ))] + (int)(PI*256*NATAN);
 376         }
 377
 378 }
 379
 380
 381
 382 int SetUpAtan()
 383 {
 384         int i;
 385         double dz = 1.0 / (double)NATAN;
 386         double z = 0.0;
 387
 388         atan_LU = (int*) malloc( (NATAN+1) * sizeof( int ));
 389
 390         if( atan_LU == NULL )
 391                 return -1;
 392
 393         for( i=0; i< NATAN; i++, z+=dz )
 394                 atan_LU[i] = atan( z / (1.0 - z ) ) * NATAN * 256;
 395
 396         atan_LU[NATAN] = PI/4.0 * NATAN * 256;
 397
 398         // Print a test
 399 #if 0
 400         for(i = -10; i< 10; i++)
 401         {
 402                 int k;
 403                 for(k=-10; k<10; k++)
 404                 {
 405                         printf("i =  %d  k = %d   atan2(i,k) = %g    LUatan(i,k) = %g diff = %g\n", i,k,atan2(i,k),
 406                                 (double)PV_atan2(i,k) / (256*NATAN) , atan2(i,k) - (double)PV_atan2(i,k) / (256*NATAN));
 407                 }
 408         }
 409         exit(0);
 410 #endif
 411         return 0;
 412 }
 413
 414 int SetUpSqrt()
 415 {
 416         int i;
 417         double dz = 1.0 / (double)NSQRT;
 418         double z = 0.0;
 419
 420         sqrt_LU = (int*) malloc( (NSQRT+1) * sizeof( int ));
 421
 422         if( sqrt_LU == NULL )
 423                 return -1;
 424
 425         for( i=0; i< NSQRT; i++, z+=dz )
 426                 sqrt_LU[i] = sqrt( 1.0 + z*z ) * 256 * NSQRT;
 427
 428         sqrt_LU[NSQRT] = sqrt(2.0) * 256 * NSQRT;
 429
 430         return 0;
 431 }
 432
 433 int SetUpMweights()
 434 {
 435         int i,k;
 436
 437         for(i=0; i<256; i++)
 438         {
 439                 mweights[i] = (int*)malloc( 256 * sizeof(int) );
 440                 if( mweights[i] == NULL ) return -1;
 441         }
 442         for(i=0; i<256; i++)
 443         {
 444                 for(k=0; k<256; k++)
 445                 {
 446                         mweights[i][k] = i*k;
 447                 }
 448         }
 449
 450         return 0;
 451 }
 452
 453
 454 int PV_sqrt( int x1, int x2 )
 455 {
 456         if( x1 > x2 )
 457         {
 458                 return  x1 * sqrt_LU[ NSQRT * x2 /  x1 ] / NSQRT;
 459         }
 460         else
 461         {
 462                 if( x2 == 0 ) return 0;
 463                 return x2 * sqrt_LU[ NSQRT * x1 /  x2 ] / NSQRT;
 464         }
 465 }
 466
 467
 468 #define ID_0 0xff
 469 #define ID_1 0xd8
 470 #define ID_2 0xff
 471 #define ID_3 0xe0
 472 #define ID_4 0x00
 473 #define ID_5 0x10
 474 #define ID_6 0x4a
 475 #define ID_7 0x46
 476 #define ID_8 0x49
 477 #define ID_9 0x46
 478 #define ID_10 0x00
 479
 480 #define ID_LENGTH 11
 481
 482 // Find last jpeg inside image; create and copy to file jpeg
 483 int extractJPEG( fullPath *image, fullPath *jpeg )
 484 {
 485         file_spec                               fnum;
 486         long                                    count;
 487         unsigned char*                  im;
 488         int                                     i, idx = -1;
 489         unsigned char                   ch;
 490
 491         if( myopen( image, read_bin, fnum ) )
 492                 return -1;
 493
 494
 495         count = 1; i=0; // Get file length
 496
 497         while( count == 1 )
 498         {
 499                 myread(  fnum, count, &ch );
 500                 if(count==1) i++;
 501         }
 502         myclose(fnum);
 503
 504         count = i;
 505
 506         im = (UCHAR*)malloc( count );
 507         if( im == NULL )
 508         {
 509                 PrintError("Not enough memory");
 510                 return -1;
 511         }
 512
 513         if( myopen( image, read_bin, fnum ) )
 514                 return -1;
 515
 516         myread(fnum,count,im);
 517         myclose(fnum);
 518
 519         if( i != count )
 520                 return -1;
 521
 522         count -= ID_LENGTH;
 523
 524         for(i=0; i<count; i++)
 525         {
 526                 if( im[i] == ID_0 && im[i+1] == ID_1 && im[i+2] == ID_2 && im[i+3] == ID_3
 527                                         && im[i+4] == ID_4 && im[i+5] == ID_5 && im[i+6] == ID_6 && im[i+7] == ID_7
 528                                         && im[i+8] == ID_8 && im[i+9] == ID_9 && im[i+10] == ID_10)
 529                                 idx = i;
 530         }
 531
 532         if( idx == -1 ) // No jpeg found
 533         {
 534                 free(im);
 535                 return -1;
 536         }
 537
 538         count = count + ID_LENGTH - idx;
 539
 540         mycreate( jpeg, 'GKON','JPEG');
 541         if( myopen( jpeg, write_bin, fnum ) )
 542                 return -1;
 543         mywrite( fnum, count, im+idx );
 544         free( im );
 545         myclose( fnum );
 546         return 0;
 547 }
 548
 549