modules/imgproc/src/samplers.cpp

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