root/snippet/rotation_3d/rotation_3d.c

Revision 81, 7.9 KB (checked in by aqua, 2 years ago)

initial import
Line 
1#include <stdio.h>
2#include <stdlib.h>
3
4#include <string.h>
5#include <math.h>
6
7#ifndef M_PI
8#define M_PI 3.141592
9#endif
10
11#define INFILE  "lena512.raw"
12#define OUTFILE "lena512_%s.raw"
13
14#define MAX_FILENAME 256
15
16#define IMG_WIDTH  512
17#define IMG_HEIGHT 512
18
19#define WIDTH  (IMG_WIDTH*3/2)
20#define HEIGHT (IMG_HEIGHT*3/2)
21
22typedef unsigned char uchar;
23
24void load_identity( double axis[9] ){
25
26        axis[0] = 1.0;  axis[1] = 0.0;  axis[2] = 0.0;
27        axis[3] = 0.0;  axis[4] = 1.0;  axis[5] = 0.0;
28        axis[6] = 0.0;  axis[7] = 0.0;  axis[8] = 1.0;
29
30}
31
32void rotate_xy( double dst_axis[9], double src_axis[9], double degree ){
33       
34        double radian = degree/180.0*M_PI;
35
36        double c = cos(radian);
37        double s = sin(radian);
38
39        dst_axis[0] = src_axis[0] * c - src_axis[1] * s;
40        dst_axis[1] = src_axis[1] * c + src_axis[0] * s;
41        dst_axis[2] = src_axis[2];
42
43        dst_axis[3] = src_axis[3] * c - src_axis[4] * s;
44        dst_axis[4] = src_axis[4] * c + src_axis[3] * s;
45        dst_axis[5] = src_axis[5];
46
47        dst_axis[6] = src_axis[6] * c - src_axis[7] * s;
48        dst_axis[7] = src_axis[7] * c + src_axis[6] * s;
49        dst_axis[8] = src_axis[8];
50
51}
52
53void rotate_yz( double dst_axis[9], double src_axis[9], double degree ){
54       
55        double radian = degree/180.0*M_PI;
56
57        double c = cos(radian);
58        double s = sin(radian);
59
60        dst_axis[0] = src_axis[0];
61        dst_axis[1] = src_axis[1] * c - src_axis[2] * s;
62        dst_axis[2] = src_axis[2] * c + src_axis[1] * s;
63
64        dst_axis[3] = src_axis[3];
65        dst_axis[4] = src_axis[4] * c - src_axis[5] * s;
66        dst_axis[5] = src_axis[5] * c + src_axis[4] * s;
67
68        dst_axis[6] = src_axis[6];
69        dst_axis[7] = src_axis[7] * c - src_axis[8] * s;
70        dst_axis[8] = src_axis[8] * c + src_axis[7] * s;
71
72}
73
74void rotate_zx( double dst_axis[9], double src_axis[9], double degree ){
75       
76        double radian = degree/180.0*M_PI;
77
78        double c = cos(radian);
79        double s = sin(radian);
80
81        dst_axis[0] = src_axis[0] * c + src_axis[2] * s;
82        dst_axis[1] = src_axis[1];
83        dst_axis[2] = src_axis[2] * c - src_axis[0] * s;
84
85        dst_axis[3] = src_axis[3] * c + src_axis[5] * s;
86        dst_axis[4] = src_axis[4];
87        dst_axis[5] = src_axis[5] * c - src_axis[3] * s;
88
89        dst_axis[6] = src_axis[6] * c + src_axis[8] * s;
90        dst_axis[7] = src_axis[7];
91        dst_axis[8] = src_axis[8] * c - src_axis[6] * s;
92
93}
94
95int bound_check( int x, int y ){
96
97        if( x < 0 || x >= WIDTH || y < 0 || y >= HEIGHT )
98                return 0;
99
100        return 1;
101
102}
103
104void warping_i( uchar* dst, uchar* src, double axis[9] ){
105
106        double x, y;
107
108        int x_, y_;
109        int i, j;
110
111        double* tmp;
112        double* tmp2;
113
114        double  wx[2], wy[2];
115
116        tmp  = (double*)malloc(sizeof(double)*WIDTH*HEIGHT);
117        tmp2 = (double*)malloc(sizeof(double)*WIDTH*HEIGHT);
118
119        memset( tmp, 0, sizeof(double)*WIDTH*HEIGHT);
120        memset( tmp2, 0, sizeof(double)*WIDTH*HEIGHT);
121
122        for( j = 0 ; j < HEIGHT ; j++ ){
123                for( i = 0 ; i < WIDTH ; i++ ){
124
125                        x = axis[0]*(i-WIDTH/2) + axis[1]*(j-HEIGHT/2) + WIDTH/2;
126                        y = axis[3]*(i-WIDTH/2) + axis[4]*(j-HEIGHT/2) + HEIGHT/2;
127
128                        x_ = (int)floor(x);
129                        y_ = (int)floor(y);
130
131                        wx[1] = x - floor(x);
132                        wx[0] = 1.0 - wx[1];
133
134                        wy[1] = y - floor(y);
135                        wy[0] = 1.0 - wy[1];
136
137                        if( bound_check( x_, y_ ) ){
138                                tmp[y_*WIDTH+x_]     += src[j*WIDTH+i]*wx[0]*wy[0];
139                                tmp2[y_*WIDTH+x_]     += wx[0]*wy[0];
140                        }
141
142                        if( bound_check( x_+1, y_ ) ){
143                                tmp[y_*WIDTH+(x_+1)] += src[j*WIDTH+i]*wx[1]*wy[0];
144                                tmp2[y_*WIDTH+(x_+1)] += wx[1]*wy[0];
145                        }
146
147                        if( bound_check( x_, y_+1 ) ){
148                                tmp[(y_+1)*WIDTH+x_] += src[j*WIDTH+i]*wx[0]*wy[1];
149                                tmp2[(y_+1)*WIDTH+x_] += wx[0]*wy[1];
150                        }
151
152                        if( bound_check( x_+1, y_+1 ) ){
153                                tmp[(y_+1)*WIDTH+(x_+1)] += src[j*WIDTH+i]*wx[1]*wy[1];
154                                tmp2[(y_+1)*WIDTH+(x_+1)] += wx[1]*wy[1];
155                        }
156
157                }
158        }       
159        for( j = 0 ; j < HEIGHT ; j++ ){
160                for( i = 0 ; i < WIDTH ; i++ ){
161                        dst[j*WIDTH+i] = (uchar)floor(tmp[j*WIDTH+i]/tmp2[j*WIDTH+i] + 0.5);
162                }
163        }
164        free(tmp);
165        free(tmp2);
166
167}
168       
169void warping( uchar* dst, uchar* src, double axis[9] ){
170
171        double x, y;
172
173        int x_, y_;
174        int i, j;
175
176        memset( dst, 0, sizeof(uchar)*WIDTH*HEIGHT );
177        for( j = 0 ; j < HEIGHT ; j++ ){
178                for( i = 0 ; i < WIDTH ; i++ ){
179
180                        x = axis[0]*(i-WIDTH/2) + axis[1]*(j-HEIGHT/2);
181                        y = axis[3]*(i-WIDTH/2) + axis[4]*(j-HEIGHT/2);
182
183                        x_ = (int)floor(x) + WIDTH/2;
184                        y_ = (int)floor(y) + HEIGHT/2;
185
186                        if( bound_check( x_, y_ ) )
187                                dst[y_*WIDTH+x_] = src[j*WIDTH+i];
188
189                }
190
191        }
192
193}
194
195uchar* trim_image( uchar* src, int* width, int* height ){
196
197        int i, j, k, w, h;
198
199        int min_x = WIDTH;
200        int max_x = 0;
201
202        int min_y = HEIGHT;
203        int max_y = 0;
204
205        uchar* dst;
206        for( j = 0 ; j < HEIGHT ; j++ ){
207                for( i = 0 ; i < WIDTH ; i++ ){
208                        if( src[j*WIDTH+i] != 0 ){
209                                if( j < min_y ) min_y = j;
210                                if( j > max_y ) max_y = j;
211
212                                if( i < min_x ) min_x = i;
213                                if( i > max_x ) max_x = i;
214                        }
215                }
216        }
217
218        w = max_x - min_x;
219        h = max_y - min_y;
220
221        if( w < 0 || h < 0 )
222                return NULL;
223
224        *width  = w;
225        *height = h;
226
227        k = 0;
228        dst = (uchar*)malloc(sizeof(uchar)*w*h);
229        for( j = 0 ; j < h ; j++ )
230                for( i = 0 ; i < w ; i++ )
231                        dst[k++] = src[(j+min_y)*WIDTH+i+min_x];
232
233        return dst;
234}
235
236int main( int argc, char* argv[] ){
237
238        char filename[MAX_FILENAME];
239
240        double src_axis[9];
241        double dst_axis[9];
242
243        uchar* dst;
244        uchar* src;
245
246        uchar* im;
247
248        FILE* in;
249        FILE* out;
250
251        int x, y, i, j, w, h;
252
253        im  = (uchar*)malloc(sizeof(uchar)*IMG_WIDTH*IMG_HEIGHT);
254
255        src = (uchar*)malloc(sizeof(uchar)*WIDTH*HEIGHT);
256        dst = (uchar*)malloc(sizeof(uchar)*WIDTH*HEIGHT);
257
258        in = fopen( INFILE, "rb" );
259        fread( im, sizeof(uchar), IMG_WIDTH*IMG_HEIGHT, in );
260        fclose(in);
261
262        memset( src, 0, sizeof(uchar)*WIDTH*HEIGHT );
263        for( j = 0 ; j < IMG_HEIGHT ; j++ ){
264                for( i = 0 ; i < IMG_WIDTH ; i++ ){
265                        x = i + WIDTH/2  - IMG_WIDTH/2;
266                        y = j + HEIGHT/2 - IMG_HEIGHT/2;
267
268                        src[y*WIDTH+x] = im[j*IMG_WIDTH+i];
269                }
270        }
271        free(im);
272
273        load_identity( src_axis );
274        rotate_xy( dst_axis, src_axis, 30.0 );
275
276        warping( dst, src, dst_axis );
277        im = trim_image( dst, &w, &h );
278
279        snprintf( filename, MAX_FILENAME, OUTFILE, "fwarping" );
280        out = fopen( filename, "wb" );
281        fwrite(im, sizeof(uchar), w*h, out );
282        fclose(out);
283
284        fprintf( stderr, "%s: %dx%d\n", filename, w, h );
285        free(im);
286
287
288        warping_i( dst, src, dst_axis );
289        im = trim_image( dst, &w, &h );
290
291        snprintf( filename, MAX_FILENAME, OUTFILE, "fwarping_i" );
292        out = fopen( filename, "wb" );
293        fwrite(im, sizeof(uchar), w*h, out );
294        fclose(out);
295
296        fprintf( stderr, "%s: %dx%d\n", filename, w, h );
297        free(im);
298
299
300
301
302        load_identity( src_axis );
303        rotate_xy( dst_axis, src_axis, 30.0 );
304
305        warping_i( dst, src, dst_axis );
306        im = trim_image( dst, &w, &h );
307
308        snprintf( filename, MAX_FILENAME, OUTFILE, "z30" );
309        out = fopen( filename, "wb" );
310        fwrite(im, sizeof(uchar), w*h, out );
311        fclose(out);
312
313        fprintf( stderr, "%s: %dx%d\n", filename, w, h );
314        free(im);
315
316
317        load_identity( src_axis );
318        rotate_zx( dst_axis, src_axis, 30.0 );
319
320        warping_i( dst, src, dst_axis );
321        im = trim_image( dst, &w, &h );
322
323        snprintf( filename, MAX_FILENAME, OUTFILE, "y30" );
324        out = fopen( filename, "wb" );
325        fwrite(im, sizeof(uchar), w*h, out );
326        fclose(out);
327
328        fprintf( stderr, "%s: %dx%d\n", filename, w, h );
329        free(im);
330
331
332        load_identity( src_axis );
333        rotate_zx( dst_axis, src_axis, 60.0 );
334        rotate_yz( src_axis, dst_axis, 30.0 );
335
336        warping_i( dst, src, src_axis );
337        im = trim_image( dst, &w, &h );
338
339        snprintf( filename, MAX_FILENAME, OUTFILE, "x30_y60" );
340        out = fopen( filename, "wb" );
341        fwrite(im, sizeof(uchar), w*h, out );
342        fclose(out);
343
344        fprintf( stderr, "%s: %dx%d\n", filename, w, h );
345        free(im);
346
347
348        load_identity( src_axis );
349        rotate_yz( dst_axis, src_axis, 30.0 );
350        rotate_zx( src_axis, dst_axis, 30.0 );
351        rotate_xy( dst_axis, src_axis, 30.0 );
352
353        warping_i( dst, src, dst_axis );
354        im = trim_image( dst, &w, &h );
355
356        snprintf( filename, MAX_FILENAME, OUTFILE, "x30_y30_z30" );
357        out = fopen( filename, "wb" );
358        fwrite(im, sizeof(uchar), w*h, out );
359        fclose(out);
360
361        fprintf( stderr, "%s: %dx%d\n", filename, w, h );
362        free(im);
363
364
365        return 0;
366}
367
Note: See TracBrowser for help on using the browser.