1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 //  By downloading, copying, installing or using the software you agree to this license.
6 //  If you do not agree to this license, do not download, install,
7 //  copy or use the software.
8 //
9 //
10 //                        Intel License Agreement
11 //                For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 //   * Redistribution's of source code must retain the above copyright notice,
20 //     this list of conditions and the following disclaimer.
21 //
22 //   * Redistribution's in binary form must reproduce the above copyright notice,
23 //     this list of conditions and the following disclaimer in the documentation
24 //     and/or other materials provided with the distribution.
25 //
26 //   * The name of Intel Corporation may not be used to endorse or promote products
27 //     derived from this software without specific prior written permission.
28 //
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
39 //
40 //M*/
41 
42 #include "_cv.h"
43 
44 /**************************************************************************************\
45 *                                   line samplers                                      *
46 \**************************************************************************************/
47 
48 CV_IMPL int
cvSampleLine(const void * img,CvPoint pt1,CvPoint pt2,void * _buffer,int connectivity)49 cvSampleLine( const void* img, CvPoint pt1, CvPoint pt2,
50               void* _buffer, int connectivity )
51 {
52     int count = -1;
53 
54     CV_FUNCNAME( "cvSampleLine" );
55 
56     __BEGIN__;
57 
58     int i, coi = 0, pix_size;
59     CvMat stub, *mat = (CvMat*)img;
60     CvLineIterator iterator;
61     uchar* buffer = (uchar*)_buffer;
62 
63     CV_CALL( mat = cvGetMat( mat, &stub, &coi ));
64 
65     if( coi != 0 )
66         CV_ERROR( CV_BadCOI, "" );
67 
68     if( !buffer )
69         CV_ERROR( CV_StsNullPtr, "" );
70 
71     CV_CALL( count = cvInitLineIterator( mat, pt1, pt2, &iterator, connectivity ));
72 
73     pix_size = CV_ELEM_SIZE(mat->type);
74     for( i = 0; i < count; i++ )
75     {
76         CV_MEMCPY_AUTO( buffer, iterator.ptr, pix_size );
77         buffer += pix_size;
78         CV_NEXT_LINE_POINT( iterator );
79     }
80 
81     __END__;
82 
83     return count;
84 }
85 
86 
87 static const void*
icvAdjustRect(const void * srcptr,int src_step,int pix_size,CvSize src_size,CvSize win_size,CvPoint ip,CvRect * pRect)88 icvAdjustRect( const void* srcptr, int src_step, int pix_size,
89                CvSize src_size, CvSize win_size,
90                CvPoint ip, CvRect* pRect )
91 {
92     CvRect rect;
93     const char* src = (const char*)srcptr;
94 
95     if( ip.x >= 0 )
96     {
97         src += ip.x*pix_size;
98         rect.x = 0;
99     }
100     else
101     {
102         rect.x = -ip.x;
103         if( rect.x > win_size.width )
104             rect.x = win_size.width;
105     }
106 
107     if( ip.x + win_size.width < src_size.width )
108         rect.width = win_size.width;
109     else
110     {
111         rect.width = src_size.width - ip.x - 1;
112         if( rect.width < 0 )
113         {
114             src += rect.width*pix_size;
115             rect.width = 0;
116         }
117         assert( rect.width <= win_size.width );
118     }
119 
120     if( ip.y >= 0 )
121     {
122         src += ip.y * src_step;
123         rect.y = 0;
124     }
125     else
126         rect.y = -ip.y;
127 
128     if( ip.y + win_size.height < src_size.height )
129         rect.height = win_size.height;
130     else
131     {
132         rect.height = src_size.height - ip.y - 1;
133         if( rect.height < 0 )
134         {
135             src += rect.height*src_step;
136             rect.height = 0;
137         }
138     }
139 
140     *pRect = rect;
141     return src - rect.x*pix_size;
142 }
143 
144 
145 #define  ICV_DEF_GET_RECT_SUB_PIX_FUNC( flavor, srctype, dsttype, worktype, \
146                                         cast_macro, scale_macro, cast_macro2 )\
147 CvStatus CV_STDCALL icvGetRectSubPix_##flavor##_C1R                         \
148 ( const srctype* src, int src_step, CvSize src_size,                        \
149   dsttype* dst, int dst_step, CvSize win_size, CvPoint2D32f center )        \
150 {                                                                           \
151     CvPoint ip;                                                             \
152     worktype  a11, a12, a21, a22, b1, b2;                                   \
153     float a, b;                                                             \
154     int i, j;                                                               \
155                                                                             \
156     center.x -= (win_size.width-1)*0.5f;                                    \
157     center.y -= (win_size.height-1)*0.5f;                                   \
158                                                                             \
159     ip.x = cvFloor( center.x );                                             \
160     ip.y = cvFloor( center.y );                                             \
161                                                                             \
162     a = center.x - ip.x;                                                    \
163     b = center.y - ip.y;                                                    \
164     a11 = scale_macro((1.f-a)*(1.f-b));                                     \
165     a12 = scale_macro(a*(1.f-b));                                           \
166     a21 = scale_macro((1.f-a)*b);                                           \
167     a22 = scale_macro(a*b);                                                 \
168     b1 = scale_macro(1.f - b);                                              \
169     b2 = scale_macro(b);                                                    \
170                                                                             \
171     src_step /= sizeof(src[0]);                                             \
172     dst_step /= sizeof(dst[0]);                                             \
173                                                                             \
174     if( 0 <= ip.x && ip.x + win_size.width < src_size.width &&              \
175         0 <= ip.y && ip.y + win_size.height < src_size.height )             \
176     {                                                                       \
177         /* extracted rectangle is totally inside the image */               \
178         src += ip.y * src_step + ip.x;                                      \
179                                                                             \
180         if( icvCopySubpix_##flavor##_C1R_p &&                               \
181             icvCopySubpix_##flavor##_C1R_p( src, src_step*sizeof(src[0]),   \
182                                             dst, dst_step*sizeof(dst[0]),   \
183                                             win_size, a, b ) >= 0 )         \
184             return CV_OK;                                                   \
185                                                                             \
186         for( i = 0; i < win_size.height; i++, src += src_step,              \
187                                               dst += dst_step )             \
188         {                                                                   \
189             for( j = 0; j <= win_size.width - 2; j += 2 )                   \
190             {                                                               \
191                 worktype s0 = cast_macro(src[j])*a11 +                      \
192                               cast_macro(src[j+1])*a12 +                    \
193                               cast_macro(src[j+src_step])*a21 +             \
194                               cast_macro(src[j+src_step+1])*a22;            \
195                 worktype s1 = cast_macro(src[j+1])*a11 +                    \
196                               cast_macro(src[j+2])*a12 +                    \
197                               cast_macro(src[j+src_step+1])*a21 +           \
198                               cast_macro(src[j+src_step+2])*a22;            \
199                                                                             \
200                 dst[j] = (dsttype)cast_macro2(s0);                          \
201                 dst[j+1] = (dsttype)cast_macro2(s1);                        \
202             }                                                               \
203                                                                             \
204             for( ; j < win_size.width; j++ )                                \
205             {                                                               \
206                 worktype s0 = cast_macro(src[j])*a11 +                      \
207                               cast_macro(src[j+1])*a12 +                    \
208                               cast_macro(src[j+src_step])*a21 +             \
209                               cast_macro(src[j+src_step+1])*a22;            \
210                                                                             \
211                 dst[j] = (dsttype)cast_macro2(s0);                          \
212             }                                                               \
213         }                                                                   \
214     }                                                                       \
215     else                                                                    \
216     {                                                                       \
217         CvRect r;                                                           \
218                                                                             \
219         src = (const srctype*)icvAdjustRect( src, src_step*sizeof(*src),    \
220                                sizeof(*src), src_size, win_size,ip, &r);    \
221                                                                             \
222         for( i = 0; i < win_size.height; i++, dst += dst_step )             \
223         {                                                                   \
224             const srctype *src2 = src + src_step;                           \
225                                                                             \
226             if( i < r.y || i >= r.height )                                  \
227                 src2 -= src_step;                                           \
228                                                                             \
229             for( j = 0; j < r.x; j++ )                                      \
230             {                                                               \
231                 worktype s0 = cast_macro(src[r.x])*b1 +                     \
232                               cast_macro(src2[r.x])*b2;                     \
233                                                                             \
234                 dst[j] = (dsttype)cast_macro2(s0);                          \
235             }                                                               \
236                                                                             \
237             for( ; j < r.width; j++ )                                       \
238             {                                                               \
239                 worktype s0 = cast_macro(src[j])*a11 +                      \
240                               cast_macro(src[j+1])*a12 +                    \
241                               cast_macro(src2[j])*a21 +                     \
242                               cast_macro(src2[j+1])*a22;                    \
243                                                                             \
244                 dst[j] = (dsttype)cast_macro2(s0);                          \
245             }                                                               \
246                                                                             \
247             for( ; j < win_size.width; j++ )                                \
248             {                                                               \
249                 worktype s0 = cast_macro(src[r.width])*b1 +                 \
250                               cast_macro(src2[r.width])*b2;                 \
251                                                                             \
252                 dst[j] = (dsttype)cast_macro2(s0);                          \
253             }                                                               \
254                                                                             \
255             if( i < r.height )                                              \
256                 src = src2;                                                 \
257         }                                                                   \
258     }                                                                       \
259                                                                             \
260     return CV_OK;                                                           \
261 }
262 
263 
264 #define  ICV_DEF_GET_RECT_SUB_PIX_FUNC_C3( flavor, srctype, dsttype, worktype, \
265                                         cast_macro, scale_macro, mul_macro )\
266 static CvStatus CV_STDCALL icvGetRectSubPix_##flavor##_C3R                  \
267 ( const srctype* src, int src_step, CvSize src_size,                        \
268   dsttype* dst, int dst_step, CvSize win_size, CvPoint2D32f center )        \
269 {                                                                           \
270     CvPoint ip;                                                             \
271     worktype a, b;                                                          \
272     int i, j;                                                               \
273                                                                             \
274     center.x -= (win_size.width-1)*0.5f;                                    \
275     center.y -= (win_size.height-1)*0.5f;                                   \
276                                                                             \
277     ip.x = cvFloor( center.x );                                             \
278     ip.y = cvFloor( center.y );                                             \
279                                                                             \
280     a = scale_macro( center.x - ip.x );                                     \
281     b = scale_macro( center.y - ip.y );                                     \
282                                                                             \
283     src_step /= sizeof( src[0] );                                           \
284     dst_step /= sizeof( dst[0] );                                           \
285                                                                             \
286     if( 0 <= ip.x && ip.x + win_size.width < src_size.width &&              \
287         0 <= ip.y && ip.y + win_size.height < src_size.height )             \
288     {                                                                       \
289         /* extracted rectangle is totally inside the image */               \
290         src += ip.y * src_step + ip.x*3;                                    \
291                                                                             \
292         for( i = 0; i < win_size.height; i++, src += src_step,              \
293                                               dst += dst_step )             \
294         {                                                                   \
295             for( j = 0; j < win_size.width; j++ )                           \
296             {                                                               \
297                 worktype s0 = cast_macro(src[j*3]);                         \
298                 worktype s1 = cast_macro(src[j*3 + src_step]);              \
299                 s0 += mul_macro( a, (cast_macro(src[j*3+3]) - s0));         \
300                 s1 += mul_macro( a, (cast_macro(src[j*3+3+src_step]) - s1));\
301                 dst[j*3] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));        \
302                                                                             \
303                 s0 = cast_macro(src[j*3+1]);                                \
304                 s1 = cast_macro(src[j*3+1 + src_step]);                     \
305                 s0 += mul_macro( a, (cast_macro(src[j*3+4]) - s0));         \
306                 s1 += mul_macro( a, (cast_macro(src[j*3+4+src_step]) - s1));\
307                 dst[j*3+1] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
308                                                                             \
309                 s0 = cast_macro(src[j*3+2]);                                \
310                 s1 = cast_macro(src[j*3+2 + src_step]);                     \
311                 s0 += mul_macro( a, (cast_macro(src[j*3+5]) - s0));         \
312                 s1 += mul_macro( a, (cast_macro(src[j*3+5+src_step]) - s1));\
313                 dst[j*3+2] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
314             }                                                               \
315         }                                                                   \
316     }                                                                       \
317     else                                                                    \
318     {                                                                       \
319         CvRect r;                                                           \
320                                                                             \
321         src = (const srctype*)icvAdjustRect( src, src_step*sizeof(*src),    \
322                              sizeof(*src)*3, src_size, win_size, ip, &r );  \
323                                                                             \
324         for( i = 0; i < win_size.height; i++, dst += dst_step )             \
325         {                                                                   \
326             const srctype *src2 = src + src_step;                           \
327                                                                             \
328             if( i < r.y || i >= r.height )                                  \
329                 src2 -= src_step;                                           \
330                                                                             \
331             for( j = 0; j < r.x; j++ )                                      \
332             {                                                               \
333                 worktype s0 = cast_macro(src[r.x*3]);                       \
334                 worktype s1 = cast_macro(src2[r.x*3]);                      \
335                 dst[j*3] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));        \
336                                                                             \
337                 s0 = cast_macro(src[r.x*3+1]);                              \
338                 s1 = cast_macro(src2[r.x*3+1]);                             \
339                 dst[j*3+1] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
340                                                                             \
341                 s0 = cast_macro(src[r.x*3+2]);                              \
342                 s1 = cast_macro(src2[r.x*3+2]);                             \
343                 dst[j*3+2] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
344             }                                                               \
345                                                                             \
346             for( ; j < r.width; j++ )                                       \
347             {                                                               \
348                 worktype s0 = cast_macro(src[j*3]);                         \
349                 worktype s1 = cast_macro(src2[j*3]);                        \
350                 s0 += mul_macro( a, (cast_macro(src[j*3 + 3]) - s0));       \
351                 s1 += mul_macro( a, (cast_macro(src2[j*3 + 3]) - s1));      \
352                 dst[j*3] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));        \
353                                                                             \
354                 s0 = cast_macro(src[j*3+1]);                                \
355                 s1 = cast_macro(src2[j*3+1]);                               \
356                 s0 += mul_macro( a, (cast_macro(src[j*3 + 4]) - s0));       \
357                 s1 += mul_macro( a, (cast_macro(src2[j*3 + 4]) - s1));      \
358                 dst[j*3+1] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
359                                                                             \
360                 s0 = cast_macro(src[j*3+2]);                                \
361                 s1 = cast_macro(src2[j*3+2]);                               \
362                 s0 += mul_macro( a, (cast_macro(src[j*3 + 5]) - s0));       \
363                 s1 += mul_macro( a, (cast_macro(src2[j*3 + 5]) - s1));      \
364                 dst[j*3+2] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
365             }                                                               \
366                                                                             \
367             for( ; j < win_size.width; j++ )                                \
368             {                                                               \
369                 worktype s0 = cast_macro(src[r.width*3]);                   \
370                 worktype s1 = cast_macro(src2[r.width*3]);                  \
371                 dst[j*3] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));        \
372                                                                             \
373                 s0 = cast_macro(src[r.width*3+1]);                          \
374                 s1 = cast_macro(src2[r.width*3+1]);                         \
375                 dst[j*3+1] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
376                                                                             \
377                 s0 = cast_macro(src[r.width*3+2]);                          \
378                 s1 = cast_macro(src2[r.width*3+2]);                         \
379                 dst[j*3+2] = (dsttype)(s0 + mul_macro( b, (s1 - s0)));      \
380             }                                                               \
381                                                                             \
382             if( i < r.height )                                              \
383                 src = src2;                                                 \
384         }                                                                   \
385     }                                                                       \
386                                                                             \
387     return CV_OK;                                                           \
388 }
389 
390 
391 
icvGetRectSubPix_8u32f_C1R(const uchar * src,int src_step,CvSize src_size,float * dst,int dst_step,CvSize win_size,CvPoint2D32f center)392 CvStatus CV_STDCALL icvGetRectSubPix_8u32f_C1R
393 ( const uchar* src, int src_step, CvSize src_size,
394   float* dst, int dst_step, CvSize win_size, CvPoint2D32f center )
395 {
396     CvPoint ip;
397     float  a12, a22, b1, b2;
398     float a, b;
399     double s = 0;
400     int i, j;
401 
402     center.x -= (win_size.width-1)*0.5f;
403     center.y -= (win_size.height-1)*0.5f;
404 
405     ip.x = cvFloor( center.x );
406     ip.y = cvFloor( center.y );
407 
408     if( win_size.width <= 0 || win_size.height <= 0 )
409         return CV_BADRANGE_ERR;
410 
411     a = center.x - ip.x;
412     b = center.y - ip.y;
413     a = MAX(a,0.0001f);
414     a12 = a*(1.f-b);
415     a22 = a*b;
416     b1 = 1.f - b;
417     b2 = b;
418     s = (1. - a)/a;
419 
420     src_step /= sizeof(src[0]);
421     dst_step /= sizeof(dst[0]);
422 
423     if( 0 <= ip.x && ip.x + win_size.width < src_size.width &&
424         0 <= ip.y && ip.y + win_size.height < src_size.height )
425     {
426         // extracted rectangle is totally inside the image
427         src += ip.y * src_step + ip.x;
428 
429 #if 0
430         if( icvCopySubpix_8u32f_C1R_p &&
431             icvCopySubpix_8u32f_C1R_p( src, src_step, dst,
432                 dst_step*sizeof(dst[0]), win_size, a, b ) >= 0 )
433             return CV_OK;
434 #endif
435 
436         for( ; win_size.height--; src += src_step, dst += dst_step )
437         {
438             float prev = (1 - a)*(b1*CV_8TO32F(src[0]) + b2*CV_8TO32F(src[src_step]));
439             for( j = 0; j < win_size.width; j++ )
440             {
441                 float t = a12*CV_8TO32F(src[j+1]) + a22*CV_8TO32F(src[j+1+src_step]);
442                 dst[j] = prev + t;
443                 prev = (float)(t*s);
444             }
445         }
446     }
447     else
448     {
449         CvRect r;
450 
451         src = (const uchar*)icvAdjustRect( src, src_step*sizeof(*src),
452                                sizeof(*src), src_size, win_size,ip, &r);
453 
454         for( i = 0; i < win_size.height; i++, dst += dst_step )
455         {
456             const uchar *src2 = src + src_step;
457 
458             if( i < r.y || i >= r.height )
459                 src2 -= src_step;
460 
461             for( j = 0; j < r.x; j++ )
462             {
463                 float s0 = CV_8TO32F(src[r.x])*b1 +
464                            CV_8TO32F(src2[r.x])*b2;
465 
466                 dst[j] = (float)(s0);
467             }
468 
469             if( j < r.width )
470             {
471                 float prev = (1 - a)*(b1*CV_8TO32F(src[j]) + b2*CV_8TO32F(src2[j]));
472 
473                 for( ; j < r.width; j++ )
474                 {
475                     float t = a12*CV_8TO32F(src[j+1]) + a22*CV_8TO32F(src2[j+1]);
476                     dst[j] = prev + t;
477                     prev = (float)(t*s);
478                 }
479             }
480 
481             for( ; j < win_size.width; j++ )
482             {
483                 float s0 = CV_8TO32F(src[r.width])*b1 +
484                            CV_8TO32F(src2[r.width])*b2;
485 
486                 dst[j] = (float)(s0);
487             }
488 
489             if( i < r.height )
490                 src = src2;
491         }
492     }
493 
494     return CV_OK;
495 }
496 
497 
498 
499 #define ICV_SHIFT             16
500 #define ICV_SCALE(x)          cvRound((x)*(1 << ICV_SHIFT))
501 #define ICV_MUL_SCALE(x,y)    (((x)*(y) + (1 << (ICV_SHIFT-1))) >> ICV_SHIFT)
502 #define ICV_DESCALE(x)        (((x)+(1 << (ICV_SHIFT-1))) >> ICV_SHIFT)
503 
504 icvCopySubpix_8u_C1R_t icvCopySubpix_8u_C1R_p = 0;
505 icvCopySubpix_8u32f_C1R_t icvCopySubpix_8u32f_C1R_p = 0;
506 icvCopySubpix_32f_C1R_t icvCopySubpix_32f_C1R_p = 0;
507 
508 ICV_DEF_GET_RECT_SUB_PIX_FUNC( 8u, uchar, uchar, int, CV_NOP, ICV_SCALE, ICV_DESCALE )
509 //ICV_DEF_GET_RECT_SUB_PIX_FUNC( 8u32f, uchar, float, float, CV_8TO32F, CV_NOP, CV_NOP )
510 ICV_DEF_GET_RECT_SUB_PIX_FUNC( 32f, float, float, float, CV_NOP, CV_NOP, CV_NOP )
511 
512 ICV_DEF_GET_RECT_SUB_PIX_FUNC_C3( 8u, uchar, uchar, int, CV_NOP, ICV_SCALE, ICV_MUL_SCALE )
513 ICV_DEF_GET_RECT_SUB_PIX_FUNC_C3( 8u32f, uchar, float, float, CV_8TO32F, CV_NOP, CV_MUL )
514 ICV_DEF_GET_RECT_SUB_PIX_FUNC_C3( 32f, float, float, float, CV_NOP, CV_NOP, CV_MUL )
515 
516 
517 #define  ICV_DEF_INIT_SUBPIX_TAB( FUNCNAME, FLAG )                  \
518 static void icvInit##FUNCNAME##FLAG##Table( CvFuncTable* tab )      \
519 {                                                                   \
520     tab->fn_2d[CV_8U] = (void*)icv##FUNCNAME##_8u_##FLAG;           \
521     tab->fn_2d[CV_32F] = (void*)icv##FUNCNAME##_32f_##FLAG;         \
522                                                                     \
523     tab->fn_2d[1] = (void*)icv##FUNCNAME##_8u32f_##FLAG;            \
524 }
525 
526 
527 ICV_DEF_INIT_SUBPIX_TAB( GetRectSubPix, C1R )
528 ICV_DEF_INIT_SUBPIX_TAB( GetRectSubPix, C3R )
529 
530 typedef CvStatus (CV_STDCALL *CvGetRectSubPixFunc)( const void* src, int src_step,
531                                                     CvSize src_size, void* dst,
532                                                     int dst_step, CvSize win_size,
533                                                     CvPoint2D32f center );
534 
535 CV_IMPL void
cvGetRectSubPix(const void * srcarr,void * dstarr,CvPoint2D32f center)536 cvGetRectSubPix( const void* srcarr, void* dstarr, CvPoint2D32f center )
537 {
538     static CvFuncTable gr_tab[2];
539     static int inittab = 0;
540     CV_FUNCNAME( "cvGetRectSubPix" );
541 
542     __BEGIN__;
543 
544     CvMat srcstub, *src = (CvMat*)srcarr;
545     CvMat dststub, *dst = (CvMat*)dstarr;
546     CvSize src_size, dst_size;
547     CvGetRectSubPixFunc func;
548     int cn, src_step, dst_step;
549 
550     if( !inittab )
551     {
552         icvInitGetRectSubPixC1RTable( gr_tab + 0 );
553         icvInitGetRectSubPixC3RTable( gr_tab + 1 );
554         inittab = 1;
555     }
556 
557     if( !CV_IS_MAT(src))
558         CV_CALL( src = cvGetMat( src, &srcstub ));
559 
560     if( !CV_IS_MAT(dst))
561         CV_CALL( dst = cvGetMat( dst, &dststub ));
562 
563     cn = CV_MAT_CN( src->type );
564 
565     if( (cn != 1 && cn != 3) || !CV_ARE_CNS_EQ( src, dst ))
566         CV_ERROR( CV_StsUnsupportedFormat, "" );
567 
568     src_size = cvGetMatSize( src );
569     dst_size = cvGetMatSize( dst );
570     src_step = src->step ? src->step : CV_STUB_STEP;
571     dst_step = dst->step ? dst->step : CV_STUB_STEP;
572 
573     if( dst_size.width > src_size.width || dst_size.height > src_size.height )
574         CV_ERROR( CV_StsBadSize, "destination ROI must be smaller than source ROI" );
575 
576     if( CV_ARE_DEPTHS_EQ( src, dst ))
577     {
578         func = (CvGetRectSubPixFunc)(gr_tab[cn != 1].fn_2d[CV_MAT_DEPTH(src->type)]);
579     }
580     else
581     {
582         if( CV_MAT_DEPTH( src->type ) != CV_8U || CV_MAT_DEPTH( dst->type ) != CV_32F )
583             CV_ERROR( CV_StsUnsupportedFormat, "" );
584 
585         func = (CvGetRectSubPixFunc)(gr_tab[cn != 1].fn_2d[1]);
586     }
587 
588     if( !func )
589         CV_ERROR( CV_StsUnsupportedFormat, "" );
590 
591     IPPI_CALL( func( src->data.ptr, src_step, src_size,
592                      dst->data.ptr, dst_step, dst_size, center ));
593 
594     __END__;
595 }
596 
597 
598 #define ICV_32F8U(x)  ((uchar)cvRound(x))
599 
600 #define ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC( flavor, srctype, dsttype,      \
601                                              worktype, cast_macro, cvt )    \
602 CvStatus CV_STDCALL                                                         \
603 icvGetQuadrangleSubPix_##flavor##_C1R                                       \
604 ( const srctype * src, int src_step, CvSize src_size,                       \
605   dsttype *dst, int dst_step, CvSize win_size, const float *matrix )        \
606 {                                                                           \
607     int x, y;                                                               \
608     double dx = (win_size.width - 1)*0.5;                                   \
609     double dy = (win_size.height - 1)*0.5;                                  \
610     double A11 = matrix[0], A12 = matrix[1], A13 = matrix[2]-A11*dx-A12*dy; \
611     double A21 = matrix[3], A22 = matrix[4], A23 = matrix[5]-A21*dx-A22*dy; \
612                                                                             \
613     src_step /= sizeof(srctype);                                            \
614     dst_step /= sizeof(dsttype);                                            \
615                                                                             \
616     for( y = 0; y < win_size.height; y++, dst += dst_step )                 \
617     {                                                                       \
618         double xs = A12*y + A13;                                            \
619         double ys = A22*y + A23;                                            \
620         double xe = A11*(win_size.width-1) + A12*y + A13;                   \
621         double ye = A21*(win_size.width-1) + A22*y + A23;                   \
622                                                                             \
623         if( (unsigned)(cvFloor(xs)-1) < (unsigned)(src_size.width - 3) &&   \
624             (unsigned)(cvFloor(ys)-1) < (unsigned)(src_size.height - 3) &&  \
625             (unsigned)(cvFloor(xe)-1) < (unsigned)(src_size.width - 3) &&   \
626             (unsigned)(cvFloor(ye)-1) < (unsigned)(src_size.height - 3))    \
627         {                                                                   \
628             for( x = 0; x < win_size.width; x++ )                           \
629             {                                                               \
630                 int ixs = cvFloor( xs );                                    \
631                 int iys = cvFloor( ys );                                    \
632                 const srctype *ptr = src + src_step*iys + ixs;              \
633                 double a = xs - ixs, b = ys - iys, a1 = 1.f - a;            \
634                 worktype p0 = cvt(ptr[0])*a1 + cvt(ptr[1])*a;               \
635                 worktype p1 = cvt(ptr[src_step])*a1 + cvt(ptr[src_step+1])*a;\
636                 xs += A11;                                                  \
637                 ys += A21;                                                  \
638                                                                             \
639                 dst[x] = cast_macro(p0 + b * (p1 - p0));                    \
640             }                                                               \
641         }                                                                   \
642         else                                                                \
643         {                                                                   \
644             for( x = 0; x < win_size.width; x++ )                           \
645             {                                                               \
646                 int ixs = cvFloor( xs ), iys = cvFloor( ys );               \
647                 double a = xs - ixs, b = ys - iys, a1 = 1.f - a;            \
648                 const srctype *ptr0, *ptr1;                                 \
649                 worktype p0, p1;                                            \
650                 xs += A11; ys += A21;                                       \
651                                                                             \
652                 if( (unsigned)iys < (unsigned)(src_size.height-1) )         \
653                     ptr0 = src + src_step*iys, ptr1 = ptr0 + src_step;      \
654                 else                                                        \
655                     ptr0 = ptr1 = src + (iys < 0 ? 0 : src_size.height-1)*src_step; \
656                                                                             \
657                 if( (unsigned)ixs < (unsigned)(src_size.width-1) )          \
658                 {                                                           \
659                     p0 = cvt(ptr0[ixs])*a1 + cvt(ptr0[ixs+1])*a;            \
660                     p1 = cvt(ptr1[ixs])*a1 + cvt(ptr1[ixs+1])*a;            \
661                 }                                                           \
662                 else                                                        \
663                 {                                                           \
664                     ixs = ixs < 0 ? 0 : src_size.width - 1;                 \
665                     p0 = cvt(ptr0[ixs]); p1 = cvt(ptr1[ixs]);               \
666                 }                                                           \
667                 dst[x] = cast_macro(p0 + b * (p1 - p0));                    \
668             }                                                               \
669         }                                                                   \
670     }                                                                       \
671                                                                             \
672     return CV_OK;                                                           \
673 }
674 
675 
676 #define ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC_C3( flavor, srctype, dsttype,   \
677                                                 worktype, cast_macro, cvt ) \
678 static CvStatus CV_STDCALL                                                  \
679 icvGetQuadrangleSubPix_##flavor##_C3R                                       \
680 ( const srctype * src, int src_step, CvSize src_size,                       \
681   dsttype *dst, int dst_step, CvSize win_size, const float *matrix )        \
682 {                                                                           \
683     int x, y;                                                               \
684     double dx = (win_size.width - 1)*0.5;                                   \
685     double dy = (win_size.height - 1)*0.5;                                  \
686     double A11 = matrix[0], A12 = matrix[1], A13 = matrix[2]-A11*dx-A12*dy; \
687     double A21 = matrix[3], A22 = matrix[4], A23 = matrix[5]-A21*dx-A22*dy; \
688                                                                             \
689     src_step /= sizeof(srctype);                                            \
690     dst_step /= sizeof(dsttype);                                            \
691                                                                             \
692     for( y = 0; y < win_size.height; y++, dst += dst_step )                 \
693     {                                                                       \
694         double xs = A12*y + A13;                                            \
695         double ys = A22*y + A23;                                            \
696         double xe = A11*(win_size.width-1) + A12*y + A13;                   \
697         double ye = A21*(win_size.width-1) + A22*y + A23;                   \
698                                                                             \
699         if( (unsigned)(cvFloor(xs)-1) < (unsigned)(src_size.width - 3) &&   \
700             (unsigned)(cvFloor(ys)-1) < (unsigned)(src_size.height - 3) &&  \
701             (unsigned)(cvFloor(xe)-1) < (unsigned)(src_size.width - 3) &&   \
702             (unsigned)(cvFloor(ye)-1) < (unsigned)(src_size.height - 3))    \
703         {                                                                   \
704             for( x = 0; x < win_size.width; x++ )                           \
705             {                                                               \
706                 int ixs = cvFloor( xs );                                    \
707                 int iys = cvFloor( ys );                                    \
708                 const srctype *ptr = src + src_step*iys + ixs*3;            \
709                 double a = xs - ixs, b = ys - iys, a1 = 1.f - a;            \
710                 worktype p0, p1;                                            \
711                 xs += A11;                                                  \
712                 ys += A21;                                                  \
713                                                                             \
714                 p0 = cvt(ptr[0])*a1 + cvt(ptr[3])*a;                        \
715                 p1 = cvt(ptr[src_step])*a1 + cvt(ptr[src_step+3])*a;        \
716                 dst[x*3] = cast_macro(p0 + b * (p1 - p0));                  \
717                                                                             \
718                 p0 = cvt(ptr[1])*a1 + cvt(ptr[4])*a;                        \
719                 p1 = cvt(ptr[src_step+1])*a1 + cvt(ptr[src_step+4])*a;      \
720                 dst[x*3+1] = cast_macro(p0 + b * (p1 - p0));                \
721                                                                             \
722                 p0 = cvt(ptr[2])*a1 + cvt(ptr[5])*a;                        \
723                 p1 = cvt(ptr[src_step+2])*a1 + cvt(ptr[src_step+5])*a;      \
724                 dst[x*3+2] = cast_macro(p0 + b * (p1 - p0));                \
725             }                                                               \
726         }                                                                   \
727         else                                                                \
728         {                                                                   \
729             for( x = 0; x < win_size.width; x++ )                           \
730             {                                                               \
731                 int ixs = cvFloor(xs), iys = cvFloor(ys);                   \
732                 double a = xs - ixs, b = ys - iys;                          \
733                 const srctype *ptr0, *ptr1;                                 \
734                 xs += A11; ys += A21;                                       \
735                                                                             \
736                 if( (unsigned)iys < (unsigned)(src_size.height-1) )         \
737                     ptr0 = src + src_step*iys, ptr1 = ptr0 + src_step;      \
738                 else                                                        \
739                     ptr0 = ptr1 = src + (iys < 0 ? 0 : src_size.height-1)*src_step; \
740                                                                             \
741                 if( (unsigned)ixs < (unsigned)(src_size.width - 1) )        \
742                 {                                                           \
743                     double a1 = 1.f - a;                                    \
744                     worktype p0, p1;                                        \
745                     ptr0 += ixs*3; ptr1 += ixs*3;                           \
746                     p0 = cvt(ptr0[0])*a1 + cvt(ptr0[3])*a;                  \
747                     p1 = cvt(ptr1[0])*a1 + cvt(ptr1[3])*a;                  \
748                     dst[x*3] = cast_macro(p0 + b * (p1 - p0));              \
749                                                                             \
750                     p0 = cvt(ptr0[1])*a1 + cvt(ptr0[4])*a;                  \
751                     p1 = cvt(ptr1[1])*a1 + cvt(ptr1[4])*a;                  \
752                     dst[x*3+1] = cast_macro(p0 + b * (p1 - p0));            \
753                                                                             \
754                     p0 = cvt(ptr0[2])*a1 + cvt(ptr0[5])*a;                  \
755                     p1 = cvt(ptr1[2])*a1 + cvt(ptr1[5])*a;                  \
756                     dst[x*3+2] = cast_macro(p0 + b * (p1 - p0));            \
757                 }                                                           \
758                 else                                                        \
759                 {                                                           \
760                     double b1 = 1.f - b;                                    \
761                     ixs = ixs < 0 ? 0 : src_size.width - 1;                 \
762                     ptr0 += ixs*3; ptr1 += ixs*3;                           \
763                                                                             \
764                     dst[x*3] = cast_macro(cvt(ptr0[0])*b1 + cvt(ptr1[0])*b);\
765                     dst[x*3+1]=cast_macro(cvt(ptr0[1])*b1 + cvt(ptr1[1])*b);\
766                     dst[x*3+2]=cast_macro(cvt(ptr0[2])*b1 + cvt(ptr1[2])*b);\
767                 }                                                           \
768             }                                                               \
769         }                                                                   \
770     }                                                                       \
771                                                                             \
772     return CV_OK;                                                           \
773 }
774 
775 
776 /*#define srctype uchar
777 #define dsttype uchar
778 #define worktype float
779 #define cvt CV_8TO32F
780 #define cast_macro ICV_32F8U
781 
782 #undef srctype
783 #undef dsttype
784 #undef worktype
785 #undef cvt
786 #undef cast_macro*/
787 
788 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC( 8u, uchar, uchar, double, ICV_32F8U, CV_8TO32F )
789 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC( 32f, float, float, double, CV_CAST_32F, CV_NOP )
790 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC( 8u32f, uchar, float, double, CV_CAST_32F, CV_8TO32F )
791 
792 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC_C3( 8u, uchar, uchar, double, ICV_32F8U, CV_8TO32F )
793 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC_C3( 32f, float, float, double, CV_CAST_32F, CV_NOP )
794 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC_C3( 8u32f, uchar, float, double, CV_CAST_32F, CV_8TO32F )
795 
796 ICV_DEF_INIT_SUBPIX_TAB( GetQuadrangleSubPix, C1R )
797 ICV_DEF_INIT_SUBPIX_TAB( GetQuadrangleSubPix, C3R )
798 
799 typedef CvStatus (CV_STDCALL *CvGetQuadrangleSubPixFunc)(
800                                          const void* src, int src_step,
801                                          CvSize src_size, void* dst,
802                                          int dst_step, CvSize win_size,
803                                          const float* matrix );
804 
805 CV_IMPL void
cvGetQuadrangleSubPix(const void * srcarr,void * dstarr,const CvMat * mat)806 cvGetQuadrangleSubPix( const void* srcarr, void* dstarr, const CvMat* mat )
807 {
808     static  CvFuncTable  gq_tab[2];
809     static  int inittab = 0;
810     CV_FUNCNAME( "cvGetQuadrangleSubPix" );
811 
812     __BEGIN__;
813 
814     CvMat srcstub, *src = (CvMat*)srcarr;
815     CvMat dststub, *dst = (CvMat*)dstarr;
816     CvSize src_size, dst_size;
817     CvGetQuadrangleSubPixFunc func;
818     float m[6];
819     int k, cn;
820 
821     if( !inittab )
822     {
823         icvInitGetQuadrangleSubPixC1RTable( gq_tab + 0 );
824         icvInitGetQuadrangleSubPixC3RTable( gq_tab + 1 );
825         inittab = 1;
826     }
827 
828     if( !CV_IS_MAT(src))
829         CV_CALL( src = cvGetMat( src, &srcstub ));
830 
831     if( !CV_IS_MAT(dst))
832         CV_CALL( dst = cvGetMat( dst, &dststub ));
833 
834     if( !CV_IS_MAT(mat))
835         CV_ERROR( CV_StsBadArg, "map matrix is not valid" );
836 
837     cn = CV_MAT_CN( src->type );
838 
839     if( (cn != 1 && cn != 3) || !CV_ARE_CNS_EQ( src, dst ))
840         CV_ERROR( CV_StsUnsupportedFormat, "" );
841 
842     src_size = cvGetMatSize( src );
843     dst_size = cvGetMatSize( dst );
844 
845     /*if( dst_size.width > src_size.width || dst_size.height > src_size.height )
846         CV_ERROR( CV_StsBadSize, "destination ROI must not be larger than source ROI" );*/
847 
848     if( mat->rows != 2 || mat->cols != 3 )
849         CV_ERROR( CV_StsBadArg,
850         "Transformation matrix must be 2x3" );
851 
852     if( CV_MAT_TYPE( mat->type ) == CV_32FC1 )
853     {
854         for( k = 0; k < 3; k++ )
855         {
856             m[k] = mat->data.fl[k];
857             m[3 + k] = ((float*)(mat->data.ptr + mat->step))[k];
858         }
859     }
860     else if( CV_MAT_TYPE( mat->type ) == CV_64FC1 )
861     {
862         for( k = 0; k < 3; k++ )
863         {
864             m[k] = (float)mat->data.db[k];
865             m[3 + k] = (float)((double*)(mat->data.ptr + mat->step))[k];
866         }
867     }
868     else
869         CV_ERROR( CV_StsUnsupportedFormat,
870             "The transformation matrix should have 32fC1 or 64fC1 type" );
871 
872     if( CV_ARE_DEPTHS_EQ( src, dst ))
873     {
874         func = (CvGetQuadrangleSubPixFunc)(gq_tab[cn != 1].fn_2d[CV_MAT_DEPTH(src->type)]);
875     }
876     else
877     {
878         if( CV_MAT_DEPTH( src->type ) != CV_8U || CV_MAT_DEPTH( dst->type ) != CV_32F )
879             CV_ERROR( CV_StsUnsupportedFormat, "" );
880 
881         func = (CvGetQuadrangleSubPixFunc)(gq_tab[cn != 1].fn_2d[1]);
882     }
883 
884     if( !func )
885         CV_ERROR( CV_StsUnsupportedFormat, "" );
886 
887     IPPI_CALL( func( src->data.ptr, src->step, src_size,
888                      dst->data.ptr, dst->step, dst_size, m ));
889 
890     __END__;
891 }
892 
893 
894 /* End of file. */
895