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