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