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