Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. */
/*------------------------------------------------------------*/
+#include <stdlib.h>
+#include <stdint.h>
+#include <assert.h>
+
#include "filter.h"
+#include "panolib.h"
#include "utils_math.h"
+#include "utils_image.h"
+#define DEG_TO_RAD(x) ((x) * 2.0 * M_PI / 360.0 )
// Lookup Tables for Trig-functions and interpolator
#define NATAN 2048
#define NSQRT 2048
-int *atan_LU;
-int *sqrt_LU;
-int *mweights[256];
-
-
-void matrix_matrix_mult ( double m1[3][3],double m2[3][3],double result[3][3]);
-void PV_transForm( TrformStr *TrPtr, int dist_r, int dist_e, int mt[3][3]);
-int PV_atan2(int y, int x);
-int PV_sqrt( int x1, int x2 );
-
-
-// Bilinear interpolator
-
-static void bil( unsigned char *dst, unsigned char **rgb,
- int dx,int dy)
-{
- int yr, yg, yb,weight;
- int rd, gd, bd ;
- register unsigned char *r;
- int *w1, *w2;
-
-
- w1 = mweights[dx]; w2 = mweights[255 - dx];
-
- r = rgb[0] + 1;
-
- rd = w2[*r++]; gd = w2[*r++]; bd = w2[*r++];
- //weight = 255 - dx; rd = weight * *r++; gd = weight * *r++; bd = weight * *r++;
-
- r++;
- rd += w1[*r++]; gd += w1[*r++]; bd += w1[*r];
- //rd += dx * *r++; gd += dx * *r++; bd += dx * *r;
-
-
- r = rgb[1] + 1;
-
- yr = w2[*r++]; yg = w2[*r++]; yb = w2[*r++];
- //rd = weight * *r++; gd = weight * *r++; bd = weight * *r++;
-
- r++;
- yr += w1[*r++]; yg += w1[*r++]; yb += w1[*r];
- //rd += dx * *r++; gd += dx * *r++; bd += dx * *r;
-
- weight = 255 - dy;
- rd = rd * weight + yr * dy;
- gd = gd * weight + yg * dy;
- bd = bd * weight + yb * dy;
-
- *dst++ = rd >> 16;
- *dst++ = gd >> 16;
- *dst = bd >> 16;
-}
-
-
-
-/*
- * Extract image from pano in TrPtr->src using parameters in prefs (ignore
- * image parameters in TrPtr)
- */
-void PV_ExtractStill( TrformStr *TrPtr )
-{
- /* field of view in rad */
- double a;
- double b;
-
- double p[2];
- double mt[3][3];
- int mi[3][3],i,k;
-
- a = DEG_TO_RAD( TrPtr->dest->hfov ); // field of view in rad
- b = DEG_TO_RAD( TrPtr->src->hfov );
-
- /* Set up the transformation matrix `mt' using Euler angles (somehow..) */
- SetMatrix (DEG_TO_RAD (TrPtr->dest->pitch), /* alpha */
- DEG_TO_RAD (TrPtr->dest->yaw), /* beta */
- 0.0, /* gamma */
- mt, /* output */
- 1);
-
-
- p[0] = (double)TrPtr->dest->width/ (2.0 * tan( a / 2.0 ) );
- p[1] = (double) TrPtr->src->width / b;
-
- for(i=0; i<3; i++){
- for(k=0; k<3; k++){
- mi[i][k] = 256 * mt[i][k];
- }
- }
-
-
- PV_transForm( TrPtr, (int)(p[0]+.5), (int)(p[1]+.5), mi);
- return;
-}
-
-
-
-
-
-// Main transformation function. Destination image is calculated using transformation
-// Function "func". Either all colors (color = 0) or one of rgb (color =1,2,3) are
-// determined. If successful, TrPtr->success = 1. Memory for destination image
-// must have been allocated and locked!
-
-void PV_transForm( TrformStr *TrPtr, int dist_r, int dist_e, int mt[3][3])
+static void matrix_matrix_mult( double m1[3][3],double m2[3][3],double result[3][3])
{
- int x_dest, y_dest; // Loop through destination image
- unsigned char *dest,*src,*sry, *dst;// Source and destination image data
- long cy; // rownum in destimage
- int dx,dy;
-
- int x_src, y_src;
-
- unsigned char *rgb[2] ,
- cdata[16]; // Image data handed to sampler
-
-
- int mix = TrPtr->src->width - 1; // maximum x-index src
- int miy = TrPtr->src->height - 1;// maximum y-index src
-
- // Variables used to convert screen coordinates to cartesian coordinates
-
-
- int w2 = TrPtr->dest->width / 2 ;
- int h2 = TrPtr->dest->height / 2 ;
- int sw2 = TrPtr->src->width / 2 ;
- int sh2 = TrPtr->src->height / 2 ;
-
- int BytesPerLine = TrPtr->src->bytesPerLine;
- int v[3];
- int x_min, x_max, y_min, y_max;
-
- int dr1, dr2, dr3;
-
- dr1 = mt[2][0] * dist_r;
- dr2 = mt[2][1] * dist_r;
- dr3 = mt[2][2] * dist_r;
-
- dest = *TrPtr->dest->data;
- src = *TrPtr->src->data; // is locked
-
- x_min = -w2; x_max = TrPtr->dest->width - w2;
- y_min = -h2; y_max = TrPtr->dest->height - h2;
-
- cy = 0;
-
- if( TrPtr->interpolator == _bilinear )
- {
- for(y_dest = y_min; y_dest < y_max; y_dest++, cy+=TrPtr->dest->bytesPerLine)
- {
- dst = dest + cy + 1;
- for(x_dest = x_min; x_dest < x_max; x_dest++, dst+=4)
- {
- v[0] = mt[0][0] * x_dest + mt[1][0] * y_dest + dr1;
- v[1] = mt[0][1] * x_dest + mt[1][1] * y_dest + dr2;
- v[2] = mt[0][2] * x_dest + mt[1][2] * y_dest + dr3;
-
- v[0] = v[0] >> 8; v[2] = v[2] >> 8;
-
- x_src = dist_e * PV_atan2( v[0], v[2] ) / NATAN ;
- y_src = dist_e * PV_atan2( v[1], utils_sqrt2 ( abs(v[2]), abs(v[0]) ) ) / NATAN ;
-
- dx = x_src & 255; dy = y_src & 255; // fraction
-
- x_src = (x_src >> 8) + sw2;
- y_src = (y_src >> 8) + sh2;
-
-
- if( y_src >= 0 && y_src < miy && x_src >= 0 && x_src < mix ) // all interpolation pixels inside image
- // (most pixels)
- {
- sry = src + y_src * BytesPerLine + x_src * 4;
- rgb[0] = sry;
- rgb[1] = sry + BytesPerLine;
- }
- else // edge pixels
- {
- int x_copy = x_src;
-
- rgb[0] = cdata;
- if( y_src < 0 )
- sry = src;
- else if( y_src > miy )
- sry = src + miy * BytesPerLine;
- else
- sry = src + y_src * BytesPerLine;
-
- if( x_src < 0 ) x_src = mix;
- if( x_src > mix) x_src = 0;
- *(long*)rgb[0] = *(long*)(sry + x_src*4);
-
- x_src = x_copy+1;
- if( x_src < 0 ) x_src = mix;
- if( x_src > mix) x_src = 0;
- *(long*)(rgb[0]+4) = *(long*)(sry + x_src*4);
-
-
-
- rgb[1] = cdata+8;
- y_src+=1;
- if( y_src < 0 )
- sry = src;
- else if( y_src > miy )
- sry = src + miy * BytesPerLine;
- else
- sry = src + y_src * BytesPerLine;
- x_src = x_copy;
- if( x_src < 0 ) x_src = mix;
- if( x_src > mix) x_src = 0;
- *(long*)rgb[1] = *(long*)(sry + x_src*4);
- x_src = x_copy+1;
- if( x_src < 0 ) x_src = mix;
- if( x_src > mix) x_src = 0;
- *(long*)(rgb[1]+4) = *(long*)(sry + x_src*4);
-
-
- }
- bil( dst, rgb, dx, dy );
- }
- }
- }
- else if( TrPtr->interpolator == _nn )
- {
- for(y_dest = y_min; y_dest < y_max; y_dest++, cy+=TrPtr->dest->bytesPerLine)
- {
- dst = dest + cy;
- for(x_dest = x_min; x_dest < x_max; x_dest++, dst+=4)
- {
- v[0] = mt[0][0] * x_dest + mt[1][0] * y_dest + dr1;
- v[1] = mt[0][1] * x_dest + mt[1][1] * y_dest + dr2;
- v[2] = mt[0][2] * x_dest + mt[1][2] * y_dest + dr3;
-
- v[0] = v[0] >> 8; v[2] = v[2] >> 8;
-
- x_src = dist_e * PV_atan2( v[0], v[2] ) / NATAN ;
-
- y_src = dist_e * PV_atan2( v[1], utils_sqrt2( abs(v[2]), abs(v[0]) ) ) / NATAN ;
-
- dx = x_src & 255; dy = y_src & 255; // fraction
-
-
- x_src = (x_src >> 8) + sw2;
- y_src = (y_src >> 8) + sh2;
-
- if( x_src < 0 ) x_src = 0;
- if( x_src > mix ) x_src = mix;
- if( y_src < 0) y_src = 0;
- if( y_src > miy ) y_src = miy;
-
- *(long*)dst = *(long*)(src + y_src * BytesPerLine + x_src * 4);
- }
- }
- }
-
- TrPtr->success = 1;
- return;
-}
-
-#if 0
-void matrix_inv_mult( double m[3][3], double vector[3] )
-{
- register int i;
- register double v0 = vector[0];
- register double v1 = vector[1];
- register double v2 = vector[2];
+ int i,k;
- for(i=0; i<3; i++)
- {
- vector[i] = m[0][i] * v0 + m[1][i] * v1 + m[2][i] * v2;
- }
-}
-#endif
-
-// Set matrix elements based on Euler angles a, b, c
+ 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];
+} /* void matrix_matrix_mult */
-void SetMatrix( double a, double b, double c , double m[3][3], int cl )
+/* Set matrix elements based on Euler angles a, b, c */
+static void set_transformation_matrix (double m[3][3],
+ double a, double b, double c)
{
double mx[3][3], my[3][3], mz[3][3], dummy[3][3];
/* Calculate `m = mz * mx * my' */
- if( cl )
- matrix_matrix_mult( mz, mx, dummy);
- else
- matrix_matrix_mult( mx, mz, dummy);
+ matrix_matrix_mult( mz, mx, dummy);
matrix_matrix_mult( dummy, my, m);
} /* void SetMatrix */
-void matrix_matrix_mult( double m1[3][3],double m2[3][3],double result[3][3])
+static int copy_pixel (ui_image_t *dest, const ui_image_t *src,
+ int x_dest, int y_dest,
+ double x_src_fp, double y_src_fp,
+ interpolator_t interp)
{
- register 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];
-} /* void matrix_matrix_mult */
+ uint32_t pixel_dest = (y_dest * dest->width) + x_dest;
+ assert (x_src_fp >= 0.0);
+ assert (y_src_fp >= 0.0);
-
-int PV_atan2(int y, int x)
-{
- // return atan2(y,x) * 256*NATAN;
- if( x > 0 )
+ if (interp == BILINEAR)
{
- if( y > 0 )
- {
- return atan_LU[(int)( NATAN * y / ( x + y ))];
- }
- else
- {
- return -atan_LU[ (int)(NATAN * (-y) / ( x - y ))];
- }
- }
+ int x_src_left;
+ int x_src_right;
+ int y_src_top;
+ int y_src_bottom;
- if( x == 0 )
- {
- if( y > 0 )
- return (int)(256*NATAN*PI / 2.0);
- else
- return -(int)(256*NATAN*PI / 2.0);
- }
-
- if( y < 0 )
- {
- return atan_LU[(int)( NATAN * y / ( x + y ))] - (int)(PI*256*NATAN);
- }
- else
- {
- return -atan_LU[ (int)(NATAN * (-y) / ( x - y ))] + (int)(PI*256*NATAN);
- }
-
-}
+ double x_right_frac;
+ double y_bottom_frac;
+ int i;
+ x_src_left = (int) x_src_fp;
+ x_src_right = x_src_left + 1;
+ y_src_top = (int) y_src_fp;
+ y_src_bottom = y_src_top + 1;
-int SetUpAtan()
-{
- int i;
- double dz = 1.0 / (double)NATAN;
- double z = 0.0;
-
- atan_LU = (int*) malloc( (NATAN+1) * sizeof( int ));
-
- if( atan_LU == NULL )
- return -1;
-
- for( i=0; i< NATAN; i++, z+=dz )
- atan_LU[i] = atan( z / (1.0 - z ) ) * NATAN * 256;
-
- atan_LU[NATAN] = PI/4.0 * NATAN * 256;
-
- // Print a test
-#if 0
- for(i = -10; i< 10; i++)
- {
- int k;
- for(k=-10; k<10; k++)
+ x_right_frac = x_src_fp - x_src_left;
+ y_bottom_frac = y_src_fp - y_src_top;
+
+ assert (x_src_right < src->width);
+ assert (y_src_bottom < src->height);
+
+ assert ((x_right_frac >= 0.0) && (x_right_frac <= 1.0));
+ assert ((y_bottom_frac >= 0.0) && (y_bottom_frac <= 1.0));
+
+ for (i = 0; i < 3; i++)
{
- printf("i = %d k = %d atan2(i,k) = %g LUatan(i,k) = %g diff = %g\n", i,k,atan2(i,k),
- (double)PV_atan2(i,k) / (256*NATAN) , atan2(i,k) - (double)PV_atan2(i,k) / (256*NATAN));
- }
- }
- exit(0);
-#endif
- return 0;
-}
+ uint8_t values[2][2];
+ double value_left;
+ double value_right;
+ double value_final;
-int SetUpSqrt()
-{
- int i;
- double dz = 1.0 / (double)NSQRT;
- double z = 0.0;
-
- sqrt_LU = (int*) malloc( (NSQRT+1) * sizeof( int ));
-
- if( sqrt_LU == NULL )
- return -1;
-
- for( i=0; i< NSQRT; i++, z+=dz )
- sqrt_LU[i] = sqrt( 1.0 + z*z ) * 256 * NSQRT;
-
- sqrt_LU[NSQRT] = sqrt(2.0) * 256 * NSQRT;
-
- return 0;
-}
+ values[0][0] = src->data[i][(y_src_top * src->width) + x_src_left];
+ values[0][1] = src->data[i][(y_src_top * src->width) + x_src_right];
+ values[1][0] = src->data[i][(y_src_bottom * src->width) + x_src_left];
+ values[1][1] = src->data[i][(y_src_bottom * src->width) + x_src_right];
-int SetUpMweights()
-{
- int i,k;
-
- for(i=0; i<256; i++)
- {
- mweights[i] = (int*)malloc( 256 * sizeof(int) );
- if( mweights[i] == NULL ) return -1;
+ value_left = (1.0 - y_bottom_frac) * values[0][0]
+ + y_bottom_frac * values[1][0];
+ value_right = (1.0 - y_bottom_frac) * values[0][1]
+ + y_bottom_frac * values[1][1];
+
+ value_final = (1.0 - x_right_frac) * value_left + x_right_frac * value_right;
+
+ assert ((value_final >= 0.0) && (value_final <= 255.0));
+
+ dest->data[i][pixel_dest] = (uint8_t) (value_final + 0.5);
+ }
}
- for(i=0; i<256; i++)
+ else /* if (interp == NNEIGHBOUR) */
{
- for(k=0; k<256; k++)
+ int x_src = (int) (x_src_fp + 0.5) % src->width;
+ int y_src = (int) (y_src_fp + 0.5) % src->height;
+
+ if ((x_src < 0) || (x_src >= src->width)
+ || (y_src < 0) || (y_src >= src->height))
+ {
+ dest->data[0][pixel_dest] = 0;
+ dest->data[1][pixel_dest] = 0;
+ dest->data[2][pixel_dest] = 0;
+ }
+ else
{
- mweights[i][k] = i*k;
+ uint32_t pixel_src = (y_src * src->width) + x_src;
+
+ dest->data[0][pixel_dest] = src->data[0][pixel_src];
+ dest->data[1][pixel_dest] = src->data[1][pixel_src];
+ dest->data[2][pixel_dest] = src->data[2][pixel_src];
}
}
-
- return 0;
-}
+ return (0);
+} /* int copy_pixel */
+
+// Main transformation function. Destination image is calculated using transformation
+// Function "func". Either all colors (color = 0) or one of rgb (color =1,2,3) are
+// determined. If successful, TrPtr->success = 1. Memory for destination image
+// must have been allocated and locked!
-int PV_sqrt( int x1, int x2 )
+int pl_extract_view (ui_image_t *view, const ui_image_t *pano,
+ double pitch, double yaw, double fov, interpolator_t interp)
{
- if( x1 > x2 )
- {
- return x1 * sqrt_LU[ NSQRT * x2 / x1 ] / NSQRT;
- }
- else
- {
- if( x2 == 0 ) return 0;
- return x2 * sqrt_LU[ NSQRT * x1 / x2 ] / NSQRT;
- }
-}
+ int x_dest, y_dest; // Loop through destination image
+ int dest_width_left = view->width / 2 ;
+ int dest_height_top = view->height / 2 ;
+ int src_width_left = pano->width / 2 ;
+ int src_height_top = pano->height / 2 ;
+
+ double v[3];
+ int x_min, x_max, y_min, y_max;
-#define ID_0 0xff
-#define ID_1 0xd8
-#define ID_2 0xff
-#define ID_3 0xe0
-#define ID_4 0x00
-#define ID_5 0x10
-#define ID_6 0x4a
-#define ID_7 0x46
-#define ID_8 0x49
-#define ID_9 0x46
-#define ID_10 0x00
+ /* The transformation matrix */
+ double tm[3][3];
-#define ID_LENGTH 11
+ double dist_r;
+ double dist_e;
-// Find last jpeg inside image; create and copy to file jpeg
-int extractJPEG( fullPath *image, fullPath *jpeg )
-{
- file_spec fnum;
- long count;
- unsigned char* im;
- int i, idx = -1;
- unsigned char ch;
-
- if( myopen( image, read_bin, fnum ) )
- return -1;
-
-
- count = 1; i=0; // Get file length
-
- while( count == 1 )
- {
- myread( fnum, count, &ch );
- if(count==1) i++;
- }
- myclose(fnum);
+ { /* What the fuck does this? -octo */
+ double a = DEG_TO_RAD (fov);
+ double b = 2.0 * M_PI; /* DEG_TO_RAD (360.0) */
- count = i;
-
- im = (UCHAR*)malloc( count );
- if( im == NULL )
- {
- PrintError("Not enough memory");
- return -1;
- }
-
- if( myopen( image, read_bin, fnum ) )
- return -1;
-
- myread(fnum,count,im);
- myclose(fnum);
-
- if( i != count )
- return -1;
-
- count -= ID_LENGTH;
-
- for(i=0; i<count; i++)
- {
- if( im[i] == ID_0 && im[i+1] == ID_1 && im[i+2] == ID_2 && im[i+3] == ID_3
- && im[i+4] == ID_4 && im[i+5] == ID_5 && im[i+6] == ID_6 && im[i+7] == ID_7
- && im[i+8] == ID_8 && im[i+9] == ID_9 && im[i+10] == ID_10)
- idx = i;
+ dist_r = ((double) view->width) / (2.0 * tan (a / 2.0));
+ dist_e = ((double) pano->width) / b;
}
- if( idx == -1 ) // No jpeg found
+ set_transformation_matrix (tm, DEG_TO_RAD (pitch), DEG_TO_RAD (yaw), 0.0);
+
+ x_min = -dest_width_left; x_max = view->width - dest_width_left;
+ y_min = -dest_height_top; y_max = view->height - dest_height_top;
+
+ for(y_dest = y_min; y_dest < y_max; y_dest++)
{
- free(im);
- return -1;
+ for(x_dest = x_min; x_dest < x_max; x_dest++)
+ {
+ double x_src_fp;
+ double y_src_fp;
+
+ v[0] = tm[0][0] * x_dest + tm[1][0] * y_dest + tm[2][0] * dist_r;
+ v[1] = tm[0][1] * x_dest + tm[1][1] * y_dest + tm[2][1] * dist_r;
+ v[2] = tm[0][2] * x_dest + tm[1][2] * y_dest + tm[2][2] * dist_r;
+
+ x_src_fp = dist_e * atan2 (v[0], v[2]);
+ y_src_fp = dist_e * atan2 (v[1], sqrt (v[2] * v[2] + v[0] * v[0]));
+
+ copy_pixel (view, pano,
+ dest_width_left + x_dest, dest_height_top + y_dest,
+ src_width_left + x_src_fp, src_height_top + y_src_fp,
+ interp); /* FIXME */
+ }
}
- count = count + ID_LENGTH - idx;
-
- mycreate( jpeg, 'GKON','JPEG');
- if( myopen( jpeg, write_bin, fnum ) )
- return -1;
- mywrite( fnum, count, im+idx );
- free( im );
- myclose( fnum );
- return 0;
+ return (0);
}
-
/*
* vim: set tabstop=4 softtabstop=4 shiftwidth=4 :