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 #include "precomp.hpp" 42 43 /* default alignment for dynamic data strucutures, resided in storages. */ 44 #define CV_STRUCT_ALIGN ((int)sizeof(double)) 45 46 /* default storage block size */ 47 #define CV_STORAGE_BLOCK_SIZE ((1<<16) - 128) 48 49 #define ICV_FREE_PTR(storage) \ 50 ((schar*)(storage)->top + (storage)->block_size - (storage)->free_space) 51 52 #define ICV_ALIGNED_SEQ_BLOCK_SIZE \ 53 (int)cvAlign(sizeof(CvSeqBlock), CV_STRUCT_ALIGN) 54 55 CV_INLINE int 56 cvAlignLeft( int size, int align ) 57 { 58 return size & -align; 59 } 60 61 #define CV_GET_LAST_ELEM( seq, block ) \ 62 ((block)->data + ((block)->count - 1)*((seq)->elem_size)) 63 64 #define CV_SWAP_ELEMS(a,b,elem_size) \ 65 { \ 66 int k; \ 67 for( k = 0; k < elem_size; k++ ) \ 68 { \ 69 char t0 = (a)[k]; \ 70 char t1 = (b)[k]; \ 71 (a)[k] = t1; \ 72 (b)[k] = t0; \ 73 } \ 74 } 75 76 #define ICV_SHIFT_TAB_MAX 32 77 static const schar icvPower2ShiftTab[] = 78 { 79 0, 1, -1, 2, -1, -1, -1, 3, -1, -1, -1, -1, -1, -1, -1, 4, 80 -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 5 81 }; 82 83 /****************************************************************************************\ 84 * Functions for manipulating memory storage - list of memory blocks * 85 \****************************************************************************************/ 86 87 /* Initialize allocated storage: */ 88 static void 89 icvInitMemStorage( CvMemStorage* storage, int block_size ) 90 { 91 if( !storage ) 92 CV_Error( CV_StsNullPtr, "" ); 93 94 if( block_size <= 0 ) 95 block_size = CV_STORAGE_BLOCK_SIZE; 96 97 block_size = cvAlign( block_size, CV_STRUCT_ALIGN ); 98 assert( sizeof(CvMemBlock) % CV_STRUCT_ALIGN == 0 ); 99 100 memset( storage, 0, sizeof( *storage )); 101 storage->signature = CV_STORAGE_MAGIC_VAL; 102 storage->block_size = block_size; 103 } 104 105 106 /* Create root memory storage: */ 107 CV_IMPL CvMemStorage* 108 cvCreateMemStorage( int block_size ) 109 { 110 CvMemStorage* storage = (CvMemStorage *)cvAlloc( sizeof( CvMemStorage )); 111 icvInitMemStorage( storage, block_size ); 112 return storage; 113 } 114 115 116 /* Create child memory storage: */ 117 CV_IMPL CvMemStorage * 118 cvCreateChildMemStorage( CvMemStorage * parent ) 119 { 120 if( !parent ) 121 CV_Error( CV_StsNullPtr, "" ); 122 123 CvMemStorage* storage = cvCreateMemStorage(parent->block_size); 124 storage->parent = parent; 125 126 return storage; 127 } 128 129 130 /* Release all blocks of the storage (or return them to parent, if any): */ 131 static void 132 icvDestroyMemStorage( CvMemStorage* storage ) 133 { 134 int k = 0; 135 136 CvMemBlock *block; 137 CvMemBlock *dst_top = 0; 138 139 if( !storage ) 140 CV_Error( CV_StsNullPtr, "" ); 141 142 if( storage->parent ) 143 dst_top = storage->parent->top; 144 145 for( block = storage->bottom; block != 0; k++ ) 146 { 147 CvMemBlock *temp = block; 148 149 block = block->next; 150 if( storage->parent ) 151 { 152 if( dst_top ) 153 { 154 temp->prev = dst_top; 155 temp->next = dst_top->next; 156 if( temp->next ) 157 temp->next->prev = temp; 158 dst_top = dst_top->next = temp; 159 } 160 else 161 { 162 dst_top = storage->parent->bottom = storage->parent->top = temp; 163 temp->prev = temp->next = 0; 164 storage->free_space = storage->block_size - sizeof( *temp ); 165 } 166 } 167 else 168 { 169 cvFree( &temp ); 170 } 171 } 172 173 storage->top = storage->bottom = 0; 174 storage->free_space = 0; 175 } 176 177 178 /* Release memory storage: */ 179 CV_IMPL void 180 cvReleaseMemStorage( CvMemStorage** storage ) 181 { 182 if( !storage ) 183 CV_Error( CV_StsNullPtr, "" ); 184 185 CvMemStorage* st = *storage; 186 *storage = 0; 187 if( st ) 188 { 189 icvDestroyMemStorage( st ); 190 cvFree( &st ); 191 } 192 } 193 194 195 /* Clears memory storage (return blocks to the parent, if any): */ 196 CV_IMPL void 197 cvClearMemStorage( CvMemStorage * storage ) 198 { 199 if( !storage ) 200 CV_Error( CV_StsNullPtr, "" ); 201 202 if( storage->parent ) 203 icvDestroyMemStorage( storage ); 204 else 205 { 206 storage->top = storage->bottom; 207 storage->free_space = storage->bottom ? storage->block_size - sizeof(CvMemBlock) : 0; 208 } 209 } 210 211 212 /* Moves stack pointer to next block. 213 If no blocks, allocate new one and link it to the storage: */ 214 static void 215 icvGoNextMemBlock( CvMemStorage * storage ) 216 { 217 if( !storage ) 218 CV_Error( CV_StsNullPtr, "" ); 219 220 if( !storage->top || !storage->top->next ) 221 { 222 CvMemBlock *block; 223 224 if( !(storage->parent) ) 225 { 226 block = (CvMemBlock *)cvAlloc( storage->block_size ); 227 } 228 else 229 { 230 CvMemStorage *parent = storage->parent; 231 CvMemStoragePos parent_pos; 232 233 cvSaveMemStoragePos( parent, &parent_pos ); 234 icvGoNextMemBlock( parent ); 235 236 block = parent->top; 237 cvRestoreMemStoragePos( parent, &parent_pos ); 238 239 if( block == parent->top ) /* the single allocated block */ 240 { 241 assert( parent->bottom == block ); 242 parent->top = parent->bottom = 0; 243 parent->free_space = 0; 244 } 245 else 246 { 247 /* cut the block from the parent's list of blocks */ 248 parent->top->next = block->next; 249 if( block->next ) 250 block->next->prev = parent->top; 251 } 252 } 253 254 /* link block */ 255 block->next = 0; 256 block->prev = storage->top; 257 258 if( storage->top ) 259 storage->top->next = block; 260 else 261 storage->top = storage->bottom = block; 262 } 263 264 if( storage->top->next ) 265 storage->top = storage->top->next; 266 storage->free_space = storage->block_size - sizeof(CvMemBlock); 267 assert( storage->free_space % CV_STRUCT_ALIGN == 0 ); 268 } 269 270 271 /* Remember memory storage position: */ 272 CV_IMPL void 273 cvSaveMemStoragePos( const CvMemStorage * storage, CvMemStoragePos * pos ) 274 { 275 if( !storage || !pos ) 276 CV_Error( CV_StsNullPtr, "" ); 277 278 pos->top = storage->top; 279 pos->free_space = storage->free_space; 280 } 281 282 283 /* Restore memory storage position: */ 284 CV_IMPL void 285 cvRestoreMemStoragePos( CvMemStorage * storage, CvMemStoragePos * pos ) 286 { 287 if( !storage || !pos ) 288 CV_Error( CV_StsNullPtr, "" ); 289 if( pos->free_space > storage->block_size ) 290 CV_Error( CV_StsBadSize, "" ); 291 292 /* 293 // this breaks icvGoNextMemBlock, so comment it off for now 294 if( storage->parent && (!pos->top || pos->top->next) ) 295 { 296 CvMemBlock* save_bottom; 297 if( !pos->top ) 298 save_bottom = 0; 299 else 300 { 301 save_bottom = storage->bottom; 302 storage->bottom = pos->top->next; 303 pos->top->next = 0; 304 storage->bottom->prev = 0; 305 } 306 icvDestroyMemStorage( storage ); 307 storage->bottom = save_bottom; 308 }*/ 309 310 storage->top = pos->top; 311 storage->free_space = pos->free_space; 312 313 if( !storage->top ) 314 { 315 storage->top = storage->bottom; 316 storage->free_space = storage->top ? storage->block_size - sizeof(CvMemBlock) : 0; 317 } 318 } 319 320 321 /* Allocate continuous buffer of the specified size in the storage: */ 322 CV_IMPL void* 323 cvMemStorageAlloc( CvMemStorage* storage, size_t size ) 324 { 325 schar *ptr = 0; 326 if( !storage ) 327 CV_Error( CV_StsNullPtr, "NULL storage pointer" ); 328 329 if( size > INT_MAX ) 330 CV_Error( CV_StsOutOfRange, "Too large memory block is requested" ); 331 332 assert( storage->free_space % CV_STRUCT_ALIGN == 0 ); 333 334 if( (size_t)storage->free_space < size ) 335 { 336 size_t max_free_space = cvAlignLeft(storage->block_size - sizeof(CvMemBlock), CV_STRUCT_ALIGN); 337 if( max_free_space < size ) 338 CV_Error( CV_StsOutOfRange, "requested size is negative or too big" ); 339 340 icvGoNextMemBlock( storage ); 341 } 342 343 ptr = ICV_FREE_PTR(storage); 344 assert( (size_t)ptr % CV_STRUCT_ALIGN == 0 ); 345 storage->free_space = cvAlignLeft(storage->free_space - (int)size, CV_STRUCT_ALIGN ); 346 347 return ptr; 348 } 349 350 351 CV_IMPL CvString 352 cvMemStorageAllocString( CvMemStorage* storage, const char* ptr, int len ) 353 { 354 CvString str; 355 356 str.len = len >= 0 ? len : (int)strlen(ptr); 357 str.ptr = (char*)cvMemStorageAlloc( storage, str.len + 1 ); 358 memcpy( str.ptr, ptr, str.len ); 359 str.ptr[str.len] = '\0'; 360 361 return str; 362 } 363 364 365 /****************************************************************************************\ 366 * Sequence implementation * 367 \****************************************************************************************/ 368 369 /* Create empty sequence: */ 370 CV_IMPL CvSeq * 371 cvCreateSeq( int seq_flags, size_t header_size, size_t elem_size, CvMemStorage* storage ) 372 { 373 CvSeq *seq = 0; 374 375 if( !storage ) 376 CV_Error( CV_StsNullPtr, "" ); 377 if( header_size < sizeof( CvSeq ) || elem_size <= 0 ) 378 CV_Error( CV_StsBadSize, "" ); 379 380 /* allocate sequence header */ 381 seq = (CvSeq*)cvMemStorageAlloc( storage, header_size ); 382 memset( seq, 0, header_size ); 383 384 seq->header_size = (int)header_size; 385 seq->flags = (seq_flags & ~CV_MAGIC_MASK) | CV_SEQ_MAGIC_VAL; 386 { 387 int elemtype = CV_MAT_TYPE(seq_flags); 388 int typesize = CV_ELEM_SIZE(elemtype); 389 390 if( elemtype != CV_SEQ_ELTYPE_GENERIC && elemtype != CV_USRTYPE1 && 391 typesize != 0 && typesize != (int)elem_size ) 392 CV_Error( CV_StsBadSize, 393 "Specified element size doesn't match to the size of the specified element type " 394 "(try to use 0 for element type)" ); 395 } 396 seq->elem_size = (int)elem_size; 397 seq->storage = storage; 398 399 cvSetSeqBlockSize( seq, (int)((1 << 10)/elem_size) ); 400 401 return seq; 402 } 403 404 405 /* adjusts <delta_elems> field of sequence. It determines how much the sequence 406 grows if there are no free space inside the sequence buffers */ 407 CV_IMPL void 408 cvSetSeqBlockSize( CvSeq *seq, int delta_elements ) 409 { 410 int elem_size; 411 int useful_block_size; 412 413 if( !seq || !seq->storage ) 414 CV_Error( CV_StsNullPtr, "" ); 415 if( delta_elements < 0 ) 416 CV_Error( CV_StsOutOfRange, "" ); 417 418 useful_block_size = cvAlignLeft(seq->storage->block_size - sizeof(CvMemBlock) - 419 sizeof(CvSeqBlock), CV_STRUCT_ALIGN); 420 elem_size = seq->elem_size; 421 422 if( delta_elements == 0 ) 423 { 424 delta_elements = (1 << 10) / elem_size; 425 delta_elements = MAX( delta_elements, 1 ); 426 } 427 if( delta_elements * elem_size > useful_block_size ) 428 { 429 delta_elements = useful_block_size / elem_size; 430 if( delta_elements == 0 ) 431 CV_Error( CV_StsOutOfRange, "Storage block size is too small " 432 "to fit the sequence elements" ); 433 } 434 435 seq->delta_elems = delta_elements; 436 } 437 438 439 /* Find a sequence element by its index: */ 440 CV_IMPL schar* 441 cvGetSeqElem( const CvSeq *seq, int index ) 442 { 443 CvSeqBlock *block; 444 int count, total = seq->total; 445 446 if( (unsigned)index >= (unsigned)total ) 447 { 448 index += index < 0 ? total : 0; 449 index -= index >= total ? total : 0; 450 if( (unsigned)index >= (unsigned)total ) 451 return 0; 452 } 453 454 block = seq->first; 455 if( index + index <= total ) 456 { 457 while( index >= (count = block->count) ) 458 { 459 block = block->next; 460 index -= count; 461 } 462 } 463 else 464 { 465 do 466 { 467 block = block->prev; 468 total -= block->count; 469 } 470 while( index < total ); 471 index -= total; 472 } 473 474 return block->data + index * seq->elem_size; 475 } 476 477 478 /* Calculate index of a sequence element: */ 479 CV_IMPL int 480 cvSeqElemIdx( const CvSeq* seq, const void* _element, CvSeqBlock** _block ) 481 { 482 const schar *element = (const schar *)_element; 483 int elem_size; 484 int id = -1; 485 CvSeqBlock *first_block; 486 CvSeqBlock *block; 487 488 if( !seq || !element ) 489 CV_Error( CV_StsNullPtr, "" ); 490 491 block = first_block = seq->first; 492 elem_size = seq->elem_size; 493 494 for( ;; ) 495 { 496 if( (unsigned)(element - block->data) < (unsigned) (block->count * elem_size) ) 497 { 498 if( _block ) 499 *_block = block; 500 if( elem_size <= ICV_SHIFT_TAB_MAX && (id = icvPower2ShiftTab[elem_size - 1]) >= 0 ) 501 id = (int)((size_t)(element - block->data) >> id); 502 else 503 id = (int)((size_t)(element - block->data) / elem_size); 504 id += block->start_index - seq->first->start_index; 505 break; 506 } 507 block = block->next; 508 if( block == first_block ) 509 break; 510 } 511 512 return id; 513 } 514 515 516 CV_IMPL int 517 cvSliceLength( CvSlice slice, const CvSeq* seq ) 518 { 519 int total = seq->total; 520 int length = slice.end_index - slice.start_index; 521 522 if( length != 0 ) 523 { 524 if( slice.start_index < 0 ) 525 slice.start_index += total; 526 if( slice.end_index <= 0 ) 527 slice.end_index += total; 528 529 length = slice.end_index - slice.start_index; 530 } 531 532 while( length < 0 ) 533 length += total; 534 if( length > total ) 535 length = total; 536 537 return length; 538 } 539 540 541 /* Copy all sequence elements into single continuous array: */ 542 CV_IMPL void* 543 cvCvtSeqToArray( const CvSeq *seq, void *array, CvSlice slice ) 544 { 545 int elem_size, total; 546 CvSeqReader reader; 547 char *dst = (char*)array; 548 549 if( !seq || !array ) 550 CV_Error( CV_StsNullPtr, "" ); 551 552 elem_size = seq->elem_size; 553 total = cvSliceLength( slice, seq )*elem_size; 554 555 if( total == 0 ) 556 return 0; 557 558 cvStartReadSeq( seq, &reader, 0 ); 559 cvSetSeqReaderPos( &reader, slice.start_index, 0 ); 560 561 do 562 { 563 int count = (int)(reader.block_max - reader.ptr); 564 if( count > total ) 565 count = total; 566 567 memcpy( dst, reader.ptr, count ); 568 dst += count; 569 reader.block = reader.block->next; 570 reader.ptr = reader.block->data; 571 reader.block_max = reader.ptr + reader.block->count*elem_size; 572 total -= count; 573 } 574 while( total > 0 ); 575 576 return array; 577 } 578 579 580 /* Construct a sequence from an array without copying any data. 581 NB: The resultant sequence cannot grow beyond its initial size: */ 582 CV_IMPL CvSeq* 583 cvMakeSeqHeaderForArray( int seq_flags, int header_size, int elem_size, 584 void *array, int total, CvSeq *seq, CvSeqBlock * block ) 585 { 586 CvSeq* result = 0; 587 588 if( elem_size <= 0 || header_size < (int)sizeof( CvSeq ) || total < 0 ) 589 CV_Error( CV_StsBadSize, "" ); 590 591 if( !seq || ((!array || !block) && total > 0) ) 592 CV_Error( CV_StsNullPtr, "" ); 593 594 memset( seq, 0, header_size ); 595 596 seq->header_size = header_size; 597 seq->flags = (seq_flags & ~CV_MAGIC_MASK) | CV_SEQ_MAGIC_VAL; 598 { 599 int elemtype = CV_MAT_TYPE(seq_flags); 600 int typesize = CV_ELEM_SIZE(elemtype); 601 602 if( elemtype != CV_SEQ_ELTYPE_GENERIC && 603 typesize != 0 && typesize != elem_size ) 604 CV_Error( CV_StsBadSize, 605 "Element size doesn't match to the size of predefined element type " 606 "(try to use 0 for sequence element type)" ); 607 } 608 seq->elem_size = elem_size; 609 seq->total = total; 610 seq->block_max = seq->ptr = (schar *) array + total * elem_size; 611 612 if( total > 0 ) 613 { 614 seq->first = block; 615 block->prev = block->next = block; 616 block->start_index = 0; 617 block->count = total; 618 block->data = (schar *) array; 619 } 620 621 result = seq; 622 623 return result; 624 } 625 626 627 /* The function allocates space for at least one more sequence element. 628 If there are free sequence blocks (seq->free_blocks != 0) 629 they are reused, otherwise the space is allocated in the storage: */ 630 static void 631 icvGrowSeq( CvSeq *seq, int in_front_of ) 632 { 633 CvSeqBlock *block; 634 635 if( !seq ) 636 CV_Error( CV_StsNullPtr, "" ); 637 block = seq->free_blocks; 638 639 if( !block ) 640 { 641 int elem_size = seq->elem_size; 642 int delta_elems = seq->delta_elems; 643 CvMemStorage *storage = seq->storage; 644 645 if( seq->total >= delta_elems*4 ) 646 cvSetSeqBlockSize( seq, delta_elems*2 ); 647 648 if( !storage ) 649 CV_Error( CV_StsNullPtr, "The sequence has NULL storage pointer" ); 650 651 /* If there is a free space just after last allocated block 652 and it is big enough then enlarge the last block. 653 This can happen only if the new block is added to the end of sequence: */ 654 if( (size_t)(ICV_FREE_PTR(storage) - seq->block_max) < CV_STRUCT_ALIGN && 655 storage->free_space >= seq->elem_size && !in_front_of ) 656 { 657 int delta = storage->free_space / elem_size; 658 659 delta = MIN( delta, delta_elems ) * elem_size; 660 seq->block_max += delta; 661 storage->free_space = cvAlignLeft((int)(((schar*)storage->top + storage->block_size) - 662 seq->block_max), CV_STRUCT_ALIGN ); 663 return; 664 } 665 else 666 { 667 int delta = elem_size * delta_elems + ICV_ALIGNED_SEQ_BLOCK_SIZE; 668 669 /* Try to allocate <delta_elements> elements: */ 670 if( storage->free_space < delta ) 671 { 672 int small_block_size = MAX(1, delta_elems/3)*elem_size + 673 ICV_ALIGNED_SEQ_BLOCK_SIZE; 674 /* try to allocate smaller part */ 675 if( storage->free_space >= small_block_size + CV_STRUCT_ALIGN ) 676 { 677 delta = (storage->free_space - ICV_ALIGNED_SEQ_BLOCK_SIZE)/seq->elem_size; 678 delta = delta*seq->elem_size + ICV_ALIGNED_SEQ_BLOCK_SIZE; 679 } 680 else 681 { 682 icvGoNextMemBlock( storage ); 683 assert( storage->free_space >= delta ); 684 } 685 } 686 687 block = (CvSeqBlock*)cvMemStorageAlloc( storage, delta ); 688 block->data = (schar*)cvAlignPtr( block + 1, CV_STRUCT_ALIGN ); 689 block->count = delta - ICV_ALIGNED_SEQ_BLOCK_SIZE; 690 block->prev = block->next = 0; 691 } 692 } 693 else 694 { 695 seq->free_blocks = block->next; 696 } 697 698 if( !(seq->first) ) 699 { 700 seq->first = block; 701 block->prev = block->next = block; 702 } 703 else 704 { 705 block->prev = seq->first->prev; 706 block->next = seq->first; 707 block->prev->next = block->next->prev = block; 708 } 709 710 /* For free blocks the <count> field means 711 * total number of bytes in the block. 712 * 713 * For used blocks it means current number 714 * of sequence elements in the block: 715 */ 716 assert( block->count % seq->elem_size == 0 && block->count > 0 ); 717 718 if( !in_front_of ) 719 { 720 seq->ptr = block->data; 721 seq->block_max = block->data + block->count; 722 block->start_index = block == block->prev ? 0 : 723 block->prev->start_index + block->prev->count; 724 } 725 else 726 { 727 int delta = block->count / seq->elem_size; 728 block->data += block->count; 729 730 if( block != block->prev ) 731 { 732 assert( seq->first->start_index == 0 ); 733 seq->first = block; 734 } 735 else 736 { 737 seq->block_max = seq->ptr = block->data; 738 } 739 740 block->start_index = 0; 741 742 for( ;; ) 743 { 744 block->start_index += delta; 745 block = block->next; 746 if( block == seq->first ) 747 break; 748 } 749 } 750 751 block->count = 0; 752 } 753 754 /* Recycle a sequence block: */ 755 static void 756 icvFreeSeqBlock( CvSeq *seq, int in_front_of ) 757 { 758 CvSeqBlock *block = seq->first; 759 760 assert( (in_front_of ? block : block->prev)->count == 0 ); 761 762 if( block == block->prev ) /* single block case */ 763 { 764 block->count = (int)(seq->block_max - block->data) + block->start_index * seq->elem_size; 765 block->data = seq->block_max - block->count; 766 seq->first = 0; 767 seq->ptr = seq->block_max = 0; 768 seq->total = 0; 769 } 770 else 771 { 772 if( !in_front_of ) 773 { 774 block = block->prev; 775 assert( seq->ptr == block->data ); 776 777 block->count = (int)(seq->block_max - seq->ptr); 778 seq->block_max = seq->ptr = block->prev->data + 779 block->prev->count * seq->elem_size; 780 } 781 else 782 { 783 int delta = block->start_index; 784 785 block->count = delta * seq->elem_size; 786 block->data -= block->count; 787 788 /* Update start indices of sequence blocks: */ 789 for( ;; ) 790 { 791 block->start_index -= delta; 792 block = block->next; 793 if( block == seq->first ) 794 break; 795 } 796 797 seq->first = block->next; 798 } 799 800 block->prev->next = block->next; 801 block->next->prev = block->prev; 802 } 803 804 assert( block->count > 0 && block->count % seq->elem_size == 0 ); 805 block->next = seq->free_blocks; 806 seq->free_blocks = block; 807 } 808 809 810 /****************************************************************************************\ 811 * Sequence Writer implementation * 812 \****************************************************************************************/ 813 814 /* Initialize sequence writer: */ 815 CV_IMPL void 816 cvStartAppendToSeq( CvSeq *seq, CvSeqWriter * writer ) 817 { 818 if( !seq || !writer ) 819 CV_Error( CV_StsNullPtr, "" ); 820 821 memset( writer, 0, sizeof( *writer )); 822 writer->header_size = sizeof( CvSeqWriter ); 823 824 writer->seq = seq; 825 writer->block = seq->first ? seq->first->prev : 0; 826 writer->ptr = seq->ptr; 827 writer->block_max = seq->block_max; 828 } 829 830 831 /* Initialize sequence writer: */ 832 CV_IMPL void 833 cvStartWriteSeq( int seq_flags, int header_size, 834 int elem_size, CvMemStorage * storage, CvSeqWriter * writer ) 835 { 836 if( !storage || !writer ) 837 CV_Error( CV_StsNullPtr, "" ); 838 839 CvSeq* seq = cvCreateSeq( seq_flags, header_size, elem_size, storage ); 840 cvStartAppendToSeq( seq, writer ); 841 } 842 843 844 /* Update sequence header: */ 845 CV_IMPL void 846 cvFlushSeqWriter( CvSeqWriter * writer ) 847 { 848 if( !writer ) 849 CV_Error( CV_StsNullPtr, "" ); 850 851 CvSeq* seq = writer->seq; 852 seq->ptr = writer->ptr; 853 854 if( writer->block ) 855 { 856 int total = 0; 857 CvSeqBlock *first_block = writer->seq->first; 858 CvSeqBlock *block = first_block; 859 860 writer->block->count = (int)((writer->ptr - writer->block->data) / seq->elem_size); 861 assert( writer->block->count > 0 ); 862 863 do 864 { 865 total += block->count; 866 block = block->next; 867 } 868 while( block != first_block ); 869 870 writer->seq->total = total; 871 } 872 } 873 874 875 /* Calls icvFlushSeqWriter and finishes writing process: */ 876 CV_IMPL CvSeq * 877 cvEndWriteSeq( CvSeqWriter * writer ) 878 { 879 if( !writer ) 880 CV_Error( CV_StsNullPtr, "" ); 881 882 cvFlushSeqWriter( writer ); 883 CvSeq* seq = writer->seq; 884 885 /* Truncate the last block: */ 886 if( writer->block && writer->seq->storage ) 887 { 888 CvMemStorage *storage = seq->storage; 889 schar *storage_block_max = (schar *) storage->top + storage->block_size; 890 891 assert( writer->block->count > 0 ); 892 893 if( (unsigned)((storage_block_max - storage->free_space) 894 - seq->block_max) < CV_STRUCT_ALIGN ) 895 { 896 storage->free_space = cvAlignLeft((int)(storage_block_max - seq->ptr), CV_STRUCT_ALIGN); 897 seq->block_max = seq->ptr; 898 } 899 } 900 901 writer->ptr = 0; 902 return seq; 903 } 904 905 906 /* Create new sequence block: */ 907 CV_IMPL void 908 cvCreateSeqBlock( CvSeqWriter * writer ) 909 { 910 if( !writer || !writer->seq ) 911 CV_Error( CV_StsNullPtr, "" ); 912 913 CvSeq* seq = writer->seq; 914 915 cvFlushSeqWriter( writer ); 916 917 icvGrowSeq( seq, 0 ); 918 919 writer->block = seq->first->prev; 920 writer->ptr = seq->ptr; 921 writer->block_max = seq->block_max; 922 } 923 924 925 /****************************************************************************************\ 926 * Sequence Reader implementation * 927 \****************************************************************************************/ 928 929 /* Initialize sequence reader: */ 930 CV_IMPL void 931 cvStartReadSeq( const CvSeq *seq, CvSeqReader * reader, int reverse ) 932 { 933 CvSeqBlock *first_block; 934 CvSeqBlock *last_block; 935 936 if( reader ) 937 { 938 reader->seq = 0; 939 reader->block = 0; 940 reader->ptr = reader->block_max = reader->block_min = 0; 941 } 942 943 if( !seq || !reader ) 944 CV_Error( CV_StsNullPtr, "" ); 945 946 reader->header_size = sizeof( CvSeqReader ); 947 reader->seq = (CvSeq*)seq; 948 949 first_block = seq->first; 950 951 if( first_block ) 952 { 953 last_block = first_block->prev; 954 reader->ptr = first_block->data; 955 reader->prev_elem = CV_GET_LAST_ELEM( seq, last_block ); 956 reader->delta_index = seq->first->start_index; 957 958 if( reverse ) 959 { 960 schar *temp = reader->ptr; 961 962 reader->ptr = reader->prev_elem; 963 reader->prev_elem = temp; 964 965 reader->block = last_block; 966 } 967 else 968 { 969 reader->block = first_block; 970 } 971 972 reader->block_min = reader->block->data; 973 reader->block_max = reader->block_min + reader->block->count * seq->elem_size; 974 } 975 else 976 { 977 reader->delta_index = 0; 978 reader->block = 0; 979 980 reader->ptr = reader->prev_elem = reader->block_min = reader->block_max = 0; 981 } 982 } 983 984 985 /* Change the current reading block 986 * to the previous or to the next: 987 */ 988 CV_IMPL void 989 cvChangeSeqBlock( void* _reader, int direction ) 990 { 991 CvSeqReader* reader = (CvSeqReader*)_reader; 992 993 if( !reader ) 994 CV_Error( CV_StsNullPtr, "" ); 995 996 if( direction > 0 ) 997 { 998 reader->block = reader->block->next; 999 reader->ptr = reader->block->data; 1000 } 1001 else 1002 { 1003 reader->block = reader->block->prev; 1004 reader->ptr = CV_GET_LAST_ELEM( reader->seq, reader->block ); 1005 } 1006 reader->block_min = reader->block->data; 1007 reader->block_max = reader->block_min + reader->block->count * reader->seq->elem_size; 1008 } 1009 1010 1011 /* Return the current reader position: */ 1012 CV_IMPL int 1013 cvGetSeqReaderPos( CvSeqReader* reader ) 1014 { 1015 int elem_size; 1016 int index = -1; 1017 1018 if( !reader || !reader->ptr ) 1019 CV_Error( CV_StsNullPtr, "" ); 1020 1021 elem_size = reader->seq->elem_size; 1022 if( elem_size <= ICV_SHIFT_TAB_MAX && (index = icvPower2ShiftTab[elem_size - 1]) >= 0 ) 1023 index = (int)((reader->ptr - reader->block_min) >> index); 1024 else 1025 index = (int)((reader->ptr - reader->block_min) / elem_size); 1026 1027 index += reader->block->start_index - reader->delta_index; 1028 1029 return index; 1030 } 1031 1032 1033 /* Set reader position to given position, 1034 * either absolute or relative to the 1035 * current one: 1036 */ 1037 CV_IMPL void 1038 cvSetSeqReaderPos( CvSeqReader* reader, int index, int is_relative ) 1039 { 1040 CvSeqBlock *block; 1041 int elem_size, count, total; 1042 1043 if( !reader || !reader->seq ) 1044 CV_Error( CV_StsNullPtr, "" ); 1045 1046 total = reader->seq->total; 1047 elem_size = reader->seq->elem_size; 1048 1049 if( !is_relative ) 1050 { 1051 if( index < 0 ) 1052 { 1053 if( index < -total ) 1054 CV_Error( CV_StsOutOfRange, "" ); 1055 index += total; 1056 } 1057 else if( index >= total ) 1058 { 1059 index -= total; 1060 if( index >= total ) 1061 CV_Error( CV_StsOutOfRange, "" ); 1062 } 1063 1064 block = reader->seq->first; 1065 if( index >= (count = block->count) ) 1066 { 1067 if( index + index <= total ) 1068 { 1069 do 1070 { 1071 block = block->next; 1072 index -= count; 1073 } 1074 while( index >= (count = block->count) ); 1075 } 1076 else 1077 { 1078 do 1079 { 1080 block = block->prev; 1081 total -= block->count; 1082 } 1083 while( index < total ); 1084 index -= total; 1085 } 1086 } 1087 reader->ptr = block->data + index * elem_size; 1088 if( reader->block != block ) 1089 { 1090 reader->block = block; 1091 reader->block_min = block->data; 1092 reader->block_max = block->data + block->count * elem_size; 1093 } 1094 } 1095 else 1096 { 1097 schar* ptr = reader->ptr; 1098 index *= elem_size; 1099 block = reader->block; 1100 1101 if( index > 0 ) 1102 { 1103 while( ptr + index >= reader->block_max ) 1104 { 1105 int delta = (int)(reader->block_max - ptr); 1106 index -= delta; 1107 reader->block = block = block->next; 1108 reader->block_min = ptr = block->data; 1109 reader->block_max = block->data + block->count*elem_size; 1110 } 1111 reader->ptr = ptr + index; 1112 } 1113 else 1114 { 1115 while( ptr + index < reader->block_min ) 1116 { 1117 int delta = (int)(ptr - reader->block_min); 1118 index += delta; 1119 reader->block = block = block->prev; 1120 reader->block_min = block->data; 1121 reader->block_max = ptr = block->data + block->count*elem_size; 1122 } 1123 reader->ptr = ptr + index; 1124 } 1125 } 1126 } 1127 1128 1129 /* Push element onto the sequence: */ 1130 CV_IMPL schar* 1131 cvSeqPush( CvSeq *seq, const void *element ) 1132 { 1133 schar *ptr = 0; 1134 size_t elem_size; 1135 1136 if( !seq ) 1137 CV_Error( CV_StsNullPtr, "" ); 1138 1139 elem_size = seq->elem_size; 1140 ptr = seq->ptr; 1141 1142 if( ptr >= seq->block_max ) 1143 { 1144 icvGrowSeq( seq, 0 ); 1145 1146 ptr = seq->ptr; 1147 assert( ptr + elem_size <= seq->block_max /*&& ptr == seq->block_min */ ); 1148 } 1149 1150 if( element ) 1151 memcpy( ptr, element, elem_size ); 1152 seq->first->prev->count++; 1153 seq->total++; 1154 seq->ptr = ptr + elem_size; 1155 1156 return ptr; 1157 } 1158 1159 1160 /* Pop last element off of the sequence: */ 1161 CV_IMPL void 1162 cvSeqPop( CvSeq *seq, void *element ) 1163 { 1164 schar *ptr; 1165 int elem_size; 1166 1167 if( !seq ) 1168 CV_Error( CV_StsNullPtr, "" ); 1169 if( seq->total <= 0 ) 1170 CV_Error( CV_StsBadSize, "" ); 1171 1172 elem_size = seq->elem_size; 1173 seq->ptr = ptr = seq->ptr - elem_size; 1174 1175 if( element ) 1176 memcpy( element, ptr, elem_size ); 1177 seq->ptr = ptr; 1178 seq->total--; 1179 1180 if( --(seq->first->prev->count) == 0 ) 1181 { 1182 icvFreeSeqBlock( seq, 0 ); 1183 assert( seq->ptr == seq->block_max ); 1184 } 1185 } 1186 1187 1188 /* Push element onto the front of the sequence: */ 1189 CV_IMPL schar* 1190 cvSeqPushFront( CvSeq *seq, const void *element ) 1191 { 1192 schar* ptr = 0; 1193 int elem_size; 1194 CvSeqBlock *block; 1195 1196 if( !seq ) 1197 CV_Error( CV_StsNullPtr, "" ); 1198 1199 elem_size = seq->elem_size; 1200 block = seq->first; 1201 1202 if( !block || block->start_index == 0 ) 1203 { 1204 icvGrowSeq( seq, 1 ); 1205 1206 block = seq->first; 1207 assert( block->start_index > 0 ); 1208 } 1209 1210 ptr = block->data -= elem_size; 1211 1212 if( element ) 1213 memcpy( ptr, element, elem_size ); 1214 block->count++; 1215 block->start_index--; 1216 seq->total++; 1217 1218 return ptr; 1219 } 1220 1221 1222 /* Shift out first element of the sequence: */ 1223 CV_IMPL void 1224 cvSeqPopFront( CvSeq *seq, void *element ) 1225 { 1226 int elem_size; 1227 CvSeqBlock *block; 1228 1229 if( !seq ) 1230 CV_Error( CV_StsNullPtr, "" ); 1231 if( seq->total <= 0 ) 1232 CV_Error( CV_StsBadSize, "" ); 1233 1234 elem_size = seq->elem_size; 1235 block = seq->first; 1236 1237 if( element ) 1238 memcpy( element, block->data, elem_size ); 1239 block->data += elem_size; 1240 block->start_index++; 1241 seq->total--; 1242 1243 if( --(block->count) == 0 ) 1244 icvFreeSeqBlock( seq, 1 ); 1245 } 1246 1247 /* Insert new element in middle of sequence: */ 1248 CV_IMPL schar* 1249 cvSeqInsert( CvSeq *seq, int before_index, const void *element ) 1250 { 1251 int elem_size; 1252 int block_size; 1253 CvSeqBlock *block; 1254 int delta_index; 1255 int total; 1256 schar* ret_ptr = 0; 1257 1258 if( !seq ) 1259 CV_Error( CV_StsNullPtr, "" ); 1260 1261 total = seq->total; 1262 before_index += before_index < 0 ? total : 0; 1263 before_index -= before_index > total ? total : 0; 1264 1265 if( (unsigned)before_index > (unsigned)total ) 1266 CV_Error( CV_StsOutOfRange, "" ); 1267 1268 if( before_index == total ) 1269 { 1270 ret_ptr = cvSeqPush( seq, element ); 1271 } 1272 else if( before_index == 0 ) 1273 { 1274 ret_ptr = cvSeqPushFront( seq, element ); 1275 } 1276 else 1277 { 1278 elem_size = seq->elem_size; 1279 1280 if( before_index >= total >> 1 ) 1281 { 1282 schar *ptr = seq->ptr + elem_size; 1283 1284 if( ptr > seq->block_max ) 1285 { 1286 icvGrowSeq( seq, 0 ); 1287 1288 ptr = seq->ptr + elem_size; 1289 assert( ptr <= seq->block_max ); 1290 } 1291 1292 delta_index = seq->first->start_index; 1293 block = seq->first->prev; 1294 block->count++; 1295 block_size = (int)(ptr - block->data); 1296 1297 while( before_index < block->start_index - delta_index ) 1298 { 1299 CvSeqBlock *prev_block = block->prev; 1300 1301 memmove( block->data + elem_size, block->data, block_size - elem_size ); 1302 block_size = prev_block->count * elem_size; 1303 memcpy( block->data, prev_block->data + block_size - elem_size, elem_size ); 1304 block = prev_block; 1305 1306 /* Check that we don't fall into an infinite loop: */ 1307 assert( block != seq->first->prev ); 1308 } 1309 1310 before_index = (before_index - block->start_index + delta_index) * elem_size; 1311 memmove( block->data + before_index + elem_size, block->data + before_index, 1312 block_size - before_index - elem_size ); 1313 1314 ret_ptr = block->data + before_index; 1315 1316 if( element ) 1317 memcpy( ret_ptr, element, elem_size ); 1318 seq->ptr = ptr; 1319 } 1320 else 1321 { 1322 block = seq->first; 1323 1324 if( block->start_index == 0 ) 1325 { 1326 icvGrowSeq( seq, 1 ); 1327 1328 block = seq->first; 1329 } 1330 1331 delta_index = block->start_index; 1332 block->count++; 1333 block->start_index--; 1334 block->data -= elem_size; 1335 1336 while( before_index > block->start_index - delta_index + block->count ) 1337 { 1338 CvSeqBlock *next_block = block->next; 1339 1340 block_size = block->count * elem_size; 1341 memmove( block->data, block->data + elem_size, block_size - elem_size ); 1342 memcpy( block->data + block_size - elem_size, next_block->data, elem_size ); 1343 block = next_block; 1344 1345 /* Check that we don't fall into an infinite loop: */ 1346 assert( block != seq->first ); 1347 } 1348 1349 before_index = (before_index - block->start_index + delta_index) * elem_size; 1350 memmove( block->data, block->data + elem_size, before_index - elem_size ); 1351 1352 ret_ptr = block->data + before_index - elem_size; 1353 1354 if( element ) 1355 memcpy( ret_ptr, element, elem_size ); 1356 } 1357 1358 seq->total = total + 1; 1359 } 1360 1361 return ret_ptr; 1362 } 1363 1364 1365 /* Removes element from sequence: */ 1366 CV_IMPL void 1367 cvSeqRemove( CvSeq *seq, int index ) 1368 { 1369 schar *ptr; 1370 int elem_size; 1371 int block_size; 1372 CvSeqBlock *block; 1373 int delta_index; 1374 int total, front = 0; 1375 1376 if( !seq ) 1377 CV_Error( CV_StsNullPtr, "" ); 1378 1379 total = seq->total; 1380 1381 index += index < 0 ? total : 0; 1382 index -= index >= total ? total : 0; 1383 1384 if( (unsigned) index >= (unsigned) total ) 1385 CV_Error( CV_StsOutOfRange, "Invalid index" ); 1386 1387 if( index == total - 1 ) 1388 { 1389 cvSeqPop( seq, 0 ); 1390 } 1391 else if( index == 0 ) 1392 { 1393 cvSeqPopFront( seq, 0 ); 1394 } 1395 else 1396 { 1397 block = seq->first; 1398 elem_size = seq->elem_size; 1399 delta_index = block->start_index; 1400 while( block->start_index - delta_index + block->count <= index ) 1401 block = block->next; 1402 1403 ptr = block->data + (index - block->start_index + delta_index) * elem_size; 1404 1405 front = index < total >> 1; 1406 if( !front ) 1407 { 1408 block_size = block->count * elem_size - (int)(ptr - block->data); 1409 1410 while( block != seq->first->prev ) /* while not the last block */ 1411 { 1412 CvSeqBlock *next_block = block->next; 1413 1414 memmove( ptr, ptr + elem_size, block_size - elem_size ); 1415 memcpy( ptr + block_size - elem_size, next_block->data, elem_size ); 1416 block = next_block; 1417 ptr = block->data; 1418 block_size = block->count * elem_size; 1419 } 1420 1421 memmove( ptr, ptr + elem_size, block_size - elem_size ); 1422 seq->ptr -= elem_size; 1423 } 1424 else 1425 { 1426 ptr += elem_size; 1427 block_size = (int)(ptr - block->data); 1428 1429 while( block != seq->first ) 1430 { 1431 CvSeqBlock *prev_block = block->prev; 1432 1433 memmove( block->data + elem_size, block->data, block_size - elem_size ); 1434 block_size = prev_block->count * elem_size; 1435 memcpy( block->data, prev_block->data + block_size - elem_size, elem_size ); 1436 block = prev_block; 1437 } 1438 1439 memmove( block->data + elem_size, block->data, block_size - elem_size ); 1440 block->data += elem_size; 1441 block->start_index++; 1442 } 1443 1444 seq->total = total - 1; 1445 if( --block->count == 0 ) 1446 icvFreeSeqBlock( seq, front ); 1447 } 1448 } 1449 1450 1451 /* Add several elements to the beginning or end of a sequence: */ 1452 CV_IMPL void 1453 cvSeqPushMulti( CvSeq *seq, const void *_elements, int count, int front ) 1454 { 1455 char *elements = (char *) _elements; 1456 1457 if( !seq ) 1458 CV_Error( CV_StsNullPtr, "NULL sequence pointer" ); 1459 if( count < 0 ) 1460 CV_Error( CV_StsBadSize, "number of removed elements is negative" ); 1461 1462 int elem_size = seq->elem_size; 1463 1464 if( !front ) 1465 { 1466 while( count > 0 ) 1467 { 1468 int delta = (int)((seq->block_max - seq->ptr) / elem_size); 1469 1470 delta = MIN( delta, count ); 1471 if( delta > 0 ) 1472 { 1473 seq->first->prev->count += delta; 1474 seq->total += delta; 1475 count -= delta; 1476 delta *= elem_size; 1477 if( elements ) 1478 { 1479 memcpy( seq->ptr, elements, delta ); 1480 elements += delta; 1481 } 1482 seq->ptr += delta; 1483 } 1484 1485 if( count > 0 ) 1486 icvGrowSeq( seq, 0 ); 1487 } 1488 } 1489 else 1490 { 1491 CvSeqBlock* block = seq->first; 1492 1493 while( count > 0 ) 1494 { 1495 int delta; 1496 1497 if( !block || block->start_index == 0 ) 1498 { 1499 icvGrowSeq( seq, 1 ); 1500 1501 block = seq->first; 1502 assert( block->start_index > 0 ); 1503 } 1504 1505 delta = MIN( block->start_index, count ); 1506 count -= delta; 1507 block->start_index -= delta; 1508 block->count += delta; 1509 seq->total += delta; 1510 delta *= elem_size; 1511 block->data -= delta; 1512 1513 if( elements ) 1514 memcpy( block->data, elements + count*elem_size, delta ); 1515 } 1516 } 1517 } 1518 1519 1520 /* Remove several elements from the end of sequence: */ 1521 CV_IMPL void 1522 cvSeqPopMulti( CvSeq *seq, void *_elements, int count, int front ) 1523 { 1524 char *elements = (char *) _elements; 1525 1526 if( !seq ) 1527 CV_Error( CV_StsNullPtr, "NULL sequence pointer" ); 1528 if( count < 0 ) 1529 CV_Error( CV_StsBadSize, "number of removed elements is negative" ); 1530 1531 count = MIN( count, seq->total ); 1532 1533 if( !front ) 1534 { 1535 if( elements ) 1536 elements += count * seq->elem_size; 1537 1538 while( count > 0 ) 1539 { 1540 int delta = seq->first->prev->count; 1541 1542 delta = MIN( delta, count ); 1543 assert( delta > 0 ); 1544 1545 seq->first->prev->count -= delta; 1546 seq->total -= delta; 1547 count -= delta; 1548 delta *= seq->elem_size; 1549 seq->ptr -= delta; 1550 1551 if( elements ) 1552 { 1553 elements -= delta; 1554 memcpy( elements, seq->ptr, delta ); 1555 } 1556 1557 if( seq->first->prev->count == 0 ) 1558 icvFreeSeqBlock( seq, 0 ); 1559 } 1560 } 1561 else 1562 { 1563 while( count > 0 ) 1564 { 1565 int delta = seq->first->count; 1566 1567 delta = MIN( delta, count ); 1568 assert( delta > 0 ); 1569 1570 seq->first->count -= delta; 1571 seq->total -= delta; 1572 count -= delta; 1573 seq->first->start_index += delta; 1574 delta *= seq->elem_size; 1575 1576 if( elements ) 1577 { 1578 memcpy( elements, seq->first->data, delta ); 1579 elements += delta; 1580 } 1581 1582 seq->first->data += delta; 1583 if( seq->first->count == 0 ) 1584 icvFreeSeqBlock( seq, 1 ); 1585 } 1586 } 1587 } 1588 1589 1590 /* Remove all elements from a sequence: */ 1591 CV_IMPL void 1592 cvClearSeq( CvSeq *seq ) 1593 { 1594 if( !seq ) 1595 CV_Error( CV_StsNullPtr, "" ); 1596 cvSeqPopMulti( seq, 0, seq->total ); 1597 } 1598 1599 1600 CV_IMPL CvSeq* 1601 cvSeqSlice( const CvSeq* seq, CvSlice slice, CvMemStorage* storage, int copy_data ) 1602 { 1603 CvSeq* subseq = 0; 1604 int elem_size, count, length; 1605 CvSeqReader reader; 1606 CvSeqBlock *block, *first_block = 0, *last_block = 0; 1607 1608 if( !CV_IS_SEQ(seq) ) 1609 CV_Error( CV_StsBadArg, "Invalid sequence header" ); 1610 1611 if( !storage ) 1612 { 1613 storage = seq->storage; 1614 if( !storage ) 1615 CV_Error( CV_StsNullPtr, "NULL storage pointer" ); 1616 } 1617 1618 elem_size = seq->elem_size; 1619 length = cvSliceLength( slice, seq ); 1620 if( slice.start_index < 0 ) 1621 slice.start_index += seq->total; 1622 else if( slice.start_index >= seq->total ) 1623 slice.start_index -= seq->total; 1624 if( (unsigned)length > (unsigned)seq->total || 1625 ((unsigned)slice.start_index >= (unsigned)seq->total && length != 0) ) 1626 CV_Error( CV_StsOutOfRange, "Bad sequence slice" ); 1627 1628 subseq = cvCreateSeq( seq->flags, seq->header_size, elem_size, storage ); 1629 1630 if( length > 0 ) 1631 { 1632 cvStartReadSeq( seq, &reader, 0 ); 1633 cvSetSeqReaderPos( &reader, slice.start_index, 0 ); 1634 count = (int)((reader.block_max - reader.ptr)/elem_size); 1635 1636 do 1637 { 1638 int bl = MIN( count, length ); 1639 1640 if( !copy_data ) 1641 { 1642 block = (CvSeqBlock*)cvMemStorageAlloc( storage, sizeof(*block) ); 1643 if( !first_block ) 1644 { 1645 first_block = subseq->first = block->prev = block->next = block; 1646 block->start_index = 0; 1647 } 1648 else 1649 { 1650 block->prev = last_block; 1651 block->next = first_block; 1652 last_block->next = first_block->prev = block; 1653 block->start_index = last_block->start_index + last_block->count; 1654 } 1655 last_block = block; 1656 block->data = reader.ptr; 1657 block->count = bl; 1658 subseq->total += bl; 1659 } 1660 else 1661 cvSeqPushMulti( subseq, reader.ptr, bl, 0 ); 1662 length -= bl; 1663 reader.block = reader.block->next; 1664 reader.ptr = reader.block->data; 1665 count = reader.block->count; 1666 } 1667 while( length > 0 ); 1668 } 1669 1670 return subseq; 1671 } 1672 1673 1674 // Remove slice from the middle of the sequence. 1675 // !!! TODO !!! Implement more efficient algorithm 1676 CV_IMPL void 1677 cvSeqRemoveSlice( CvSeq* seq, CvSlice slice ) 1678 { 1679 int total, length; 1680 1681 if( !CV_IS_SEQ(seq) ) 1682 CV_Error( CV_StsBadArg, "Invalid sequence header" ); 1683 1684 length = cvSliceLength( slice, seq ); 1685 total = seq->total; 1686 1687 if( slice.start_index < 0 ) 1688 slice.start_index += total; 1689 else if( slice.start_index >= total ) 1690 slice.start_index -= total; 1691 1692 if( (unsigned)slice.start_index >= (unsigned)total ) 1693 CV_Error( CV_StsOutOfRange, "start slice index is out of range" ); 1694 1695 slice.end_index = slice.start_index + length; 1696 1697 if( slice.end_index < total ) 1698 { 1699 CvSeqReader reader_to, reader_from; 1700 int elem_size = seq->elem_size; 1701 1702 cvStartReadSeq( seq, &reader_to ); 1703 cvStartReadSeq( seq, &reader_from ); 1704 1705 if( slice.start_index > total - slice.end_index ) 1706 { 1707 int i, count = seq->total - slice.end_index; 1708 cvSetSeqReaderPos( &reader_to, slice.start_index ); 1709 cvSetSeqReaderPos( &reader_from, slice.end_index ); 1710 1711 for( i = 0; i < count; i++ ) 1712 { 1713 memcpy( reader_to.ptr, reader_from.ptr, elem_size ); 1714 CV_NEXT_SEQ_ELEM( elem_size, reader_to ); 1715 CV_NEXT_SEQ_ELEM( elem_size, reader_from ); 1716 } 1717 1718 cvSeqPopMulti( seq, 0, slice.end_index - slice.start_index ); 1719 } 1720 else 1721 { 1722 int i, count = slice.start_index; 1723 cvSetSeqReaderPos( &reader_to, slice.end_index ); 1724 cvSetSeqReaderPos( &reader_from, slice.start_index ); 1725 1726 for( i = 0; i < count; i++ ) 1727 { 1728 CV_PREV_SEQ_ELEM( elem_size, reader_to ); 1729 CV_PREV_SEQ_ELEM( elem_size, reader_from ); 1730 1731 memcpy( reader_to.ptr, reader_from.ptr, elem_size ); 1732 } 1733 1734 cvSeqPopMulti( seq, 0, slice.end_index - slice.start_index, 1 ); 1735 } 1736 } 1737 else 1738 { 1739 cvSeqPopMulti( seq, 0, total - slice.start_index ); 1740 cvSeqPopMulti( seq, 0, slice.end_index - total, 1 ); 1741 } 1742 } 1743 1744 1745 // Insert a sequence into the middle of another sequence: 1746 // !!! TODO !!! Implement more efficient algorithm 1747 CV_IMPL void 1748 cvSeqInsertSlice( CvSeq* seq, int index, const CvArr* from_arr ) 1749 { 1750 CvSeqReader reader_to, reader_from; 1751 int i, elem_size, total, from_total; 1752 CvSeq from_header, *from = (CvSeq*)from_arr; 1753 CvSeqBlock block; 1754 1755 if( !CV_IS_SEQ(seq) ) 1756 CV_Error( CV_StsBadArg, "Invalid destination sequence header" ); 1757 1758 if( !CV_IS_SEQ(from)) 1759 { 1760 CvMat* mat = (CvMat*)from; 1761 if( !CV_IS_MAT(mat)) 1762 CV_Error( CV_StsBadArg, "Source is not a sequence nor matrix" ); 1763 1764 if( !CV_IS_MAT_CONT(mat->type) || (mat->rows != 1 && mat->cols != 1) ) 1765 CV_Error( CV_StsBadArg, "The source array must be 1d coninuous vector" ); 1766 1767 from = cvMakeSeqHeaderForArray( CV_SEQ_KIND_GENERIC, sizeof(from_header), 1768 CV_ELEM_SIZE(mat->type), 1769 mat->data.ptr, mat->cols + mat->rows - 1, 1770 &from_header, &block ); 1771 } 1772 1773 if( seq->elem_size != from->elem_size ) 1774 CV_Error( CV_StsUnmatchedSizes, 1775 "Source and destination sequence element sizes are different." ); 1776 1777 from_total = from->total; 1778 1779 if( from_total == 0 ) 1780 return; 1781 1782 total = seq->total; 1783 index += index < 0 ? total : 0; 1784 index -= index > total ? total : 0; 1785 1786 if( (unsigned)index > (unsigned)total ) 1787 CV_Error( CV_StsOutOfRange, "" ); 1788 1789 elem_size = seq->elem_size; 1790 1791 if( index < (total >> 1) ) 1792 { 1793 cvSeqPushMulti( seq, 0, from_total, 1 ); 1794 1795 cvStartReadSeq( seq, &reader_to ); 1796 cvStartReadSeq( seq, &reader_from ); 1797 cvSetSeqReaderPos( &reader_from, from_total ); 1798 1799 for( i = 0; i < index; i++ ) 1800 { 1801 memcpy( reader_to.ptr, reader_from.ptr, elem_size ); 1802 CV_NEXT_SEQ_ELEM( elem_size, reader_to ); 1803 CV_NEXT_SEQ_ELEM( elem_size, reader_from ); 1804 } 1805 } 1806 else 1807 { 1808 cvSeqPushMulti( seq, 0, from_total ); 1809 1810 cvStartReadSeq( seq, &reader_to ); 1811 cvStartReadSeq( seq, &reader_from ); 1812 cvSetSeqReaderPos( &reader_from, total ); 1813 cvSetSeqReaderPos( &reader_to, seq->total ); 1814 1815 for( i = 0; i < total - index; i++ ) 1816 { 1817 CV_PREV_SEQ_ELEM( elem_size, reader_to ); 1818 CV_PREV_SEQ_ELEM( elem_size, reader_from ); 1819 memcpy( reader_to.ptr, reader_from.ptr, elem_size ); 1820 } 1821 } 1822 1823 cvStartReadSeq( from, &reader_from ); 1824 cvSetSeqReaderPos( &reader_to, index ); 1825 1826 for( i = 0; i < from_total; i++ ) 1827 { 1828 memcpy( reader_to.ptr, reader_from.ptr, elem_size ); 1829 CV_NEXT_SEQ_ELEM( elem_size, reader_to ); 1830 CV_NEXT_SEQ_ELEM( elem_size, reader_from ); 1831 } 1832 } 1833 1834 // Sort the sequence using user-specified comparison function. 1835 // The semantics is similar to qsort() function. 1836 // The code is based on BSD system qsort(): 1837 // * Copyright (c) 1992, 1993 1838 // * The Regents of the University of California. All rights reserved. 1839 // * 1840 // * Redistribution and use in source and binary forms, with or without 1841 // * modification, are permitted provided that the following conditions 1842 // * are met: 1843 // * 1. Redistributions of source code must retain the above copyright 1844 // * notice, this list of conditions and the following disclaimer. 1845 // * 2. Redistributions in binary form must reproduce the above copyright 1846 // * notice, this list of conditions and the following disclaimer in the 1847 // * documentation and/or other materials provided with the distribution. 1848 // * 3. All advertising materials mentioning features or use of this software 1849 // * must display the following acknowledgement: 1850 // * This product includes software developed by the University of 1851 // * California, Berkeley and its contributors. 1852 // * 4. Neither the name of the University nor the names of its contributors 1853 // * may be used to endorse or promote products derived from this software 1854 // * without specific prior written permission. 1855 // * 1856 // * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND 1857 // * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 1858 // * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 1859 // * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE 1860 // * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 1861 // * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 1862 // * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 1863 // * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 1864 // * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 1865 // * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 1866 // * SUCH DAMAGE. 1867 1868 typedef struct CvSeqReaderPos 1869 { 1870 CvSeqBlock* block; 1871 schar* ptr; 1872 schar* block_min; 1873 schar* block_max; 1874 } 1875 CvSeqReaderPos; 1876 1877 #define CV_SAVE_READER_POS( reader, pos ) \ 1878 { \ 1879 (pos).block = (reader).block; \ 1880 (pos).ptr = (reader).ptr; \ 1881 (pos).block_min = (reader).block_min; \ 1882 (pos).block_max = (reader).block_max; \ 1883 } 1884 1885 #define CV_RESTORE_READER_POS( reader, pos )\ 1886 { \ 1887 (reader).block = (pos).block; \ 1888 (reader).ptr = (pos).ptr; \ 1889 (reader).block_min = (pos).block_min; \ 1890 (reader).block_max = (pos).block_max; \ 1891 } 1892 1893 inline schar* 1894 icvMed3( schar* a, schar* b, schar* c, CvCmpFunc cmp_func, void* aux ) 1895 { 1896 return cmp_func(a, b, aux) < 0 ? 1897 (cmp_func(b, c, aux) < 0 ? b : cmp_func(a, c, aux) < 0 ? c : a) 1898 :(cmp_func(b, c, aux) > 0 ? b : cmp_func(a, c, aux) < 0 ? a : c); 1899 } 1900 1901 CV_IMPL void 1902 cvSeqSort( CvSeq* seq, CvCmpFunc cmp_func, void* aux ) 1903 { 1904 int elem_size; 1905 int isort_thresh = 7; 1906 CvSeqReader left, right; 1907 int sp = 0; 1908 1909 struct 1910 { 1911 CvSeqReaderPos lb; 1912 CvSeqReaderPos ub; 1913 } 1914 stack[48]; 1915 1916 if( !CV_IS_SEQ(seq) ) 1917 CV_Error( !seq ? CV_StsNullPtr : CV_StsBadArg, "Bad input sequence" ); 1918 1919 if( !cmp_func ) 1920 CV_Error( CV_StsNullPtr, "Null compare function" ); 1921 1922 if( seq->total <= 1 ) 1923 return; 1924 1925 elem_size = seq->elem_size; 1926 isort_thresh *= elem_size; 1927 1928 cvStartReadSeq( seq, &left, 0 ); 1929 right = left; 1930 CV_SAVE_READER_POS( left, stack[0].lb ); 1931 CV_PREV_SEQ_ELEM( elem_size, right ); 1932 CV_SAVE_READER_POS( right, stack[0].ub ); 1933 1934 while( sp >= 0 ) 1935 { 1936 CV_RESTORE_READER_POS( left, stack[sp].lb ); 1937 CV_RESTORE_READER_POS( right, stack[sp].ub ); 1938 sp--; 1939 1940 for(;;) 1941 { 1942 int i, n, m; 1943 CvSeqReader ptr, ptr2; 1944 1945 if( left.block == right.block ) 1946 n = (int)(right.ptr - left.ptr) + elem_size; 1947 else 1948 { 1949 n = cvGetSeqReaderPos( &right ); 1950 n = (n - cvGetSeqReaderPos( &left ) + 1)*elem_size; 1951 } 1952 1953 if( n <= isort_thresh ) 1954 { 1955 insert_sort: 1956 ptr = ptr2 = left; 1957 CV_NEXT_SEQ_ELEM( elem_size, ptr ); 1958 CV_NEXT_SEQ_ELEM( elem_size, right ); 1959 while( ptr.ptr != right.ptr ) 1960 { 1961 ptr2.ptr = ptr.ptr; 1962 if( ptr2.block != ptr.block ) 1963 { 1964 ptr2.block = ptr.block; 1965 ptr2.block_min = ptr.block_min; 1966 ptr2.block_max = ptr.block_max; 1967 } 1968 while( ptr2.ptr != left.ptr ) 1969 { 1970 schar* cur = ptr2.ptr; 1971 CV_PREV_SEQ_ELEM( elem_size, ptr2 ); 1972 if( cmp_func( ptr2.ptr, cur, aux ) <= 0 ) 1973 break; 1974 CV_SWAP_ELEMS( ptr2.ptr, cur, elem_size ); 1975 } 1976 CV_NEXT_SEQ_ELEM( elem_size, ptr ); 1977 } 1978 break; 1979 } 1980 else 1981 { 1982 CvSeqReader left0, left1, right0, right1; 1983 CvSeqReader tmp0, tmp1; 1984 schar *m1, *m2, *m3, *pivot; 1985 int swap_cnt = 0; 1986 int l, l0, l1, r, r0, r1; 1987 1988 left0 = tmp0 = left; 1989 right0 = right1 = right; 1990 n /= elem_size; 1991 1992 if( n > 40 ) 1993 { 1994 int d = n / 8; 1995 schar *p1, *p2, *p3; 1996 p1 = tmp0.ptr; 1997 cvSetSeqReaderPos( &tmp0, d, 1 ); 1998 p2 = tmp0.ptr; 1999 cvSetSeqReaderPos( &tmp0, d, 1 ); 2000 p3 = tmp0.ptr; 2001 m1 = icvMed3( p1, p2, p3, cmp_func, aux ); 2002 cvSetSeqReaderPos( &tmp0, (n/2) - d*3, 1 ); 2003 p1 = tmp0.ptr; 2004 cvSetSeqReaderPos( &tmp0, d, 1 ); 2005 p2 = tmp0.ptr; 2006 cvSetSeqReaderPos( &tmp0, d, 1 ); 2007 p3 = tmp0.ptr; 2008 m2 = icvMed3( p1, p2, p3, cmp_func, aux ); 2009 cvSetSeqReaderPos( &tmp0, n - 1 - d*3 - n/2, 1 ); 2010 p1 = tmp0.ptr; 2011 cvSetSeqReaderPos( &tmp0, d, 1 ); 2012 p2 = tmp0.ptr; 2013 cvSetSeqReaderPos( &tmp0, d, 1 ); 2014 p3 = tmp0.ptr; 2015 m3 = icvMed3( p1, p2, p3, cmp_func, aux ); 2016 } 2017 else 2018 { 2019 m1 = tmp0.ptr; 2020 cvSetSeqReaderPos( &tmp0, n/2, 1 ); 2021 m2 = tmp0.ptr; 2022 cvSetSeqReaderPos( &tmp0, n - 1 - n/2, 1 ); 2023 m3 = tmp0.ptr; 2024 } 2025 2026 pivot = icvMed3( m1, m2, m3, cmp_func, aux ); 2027 left = left0; 2028 if( pivot != left.ptr ) 2029 { 2030 CV_SWAP_ELEMS( pivot, left.ptr, elem_size ); 2031 pivot = left.ptr; 2032 } 2033 CV_NEXT_SEQ_ELEM( elem_size, left ); 2034 left1 = left; 2035 2036 for(;;) 2037 { 2038 while( left.ptr != right.ptr && (r = cmp_func(left.ptr, pivot, aux)) <= 0 ) 2039 { 2040 if( r == 0 ) 2041 { 2042 if( left1.ptr != left.ptr ) 2043 CV_SWAP_ELEMS( left1.ptr, left.ptr, elem_size ); 2044 swap_cnt = 1; 2045 CV_NEXT_SEQ_ELEM( elem_size, left1 ); 2046 } 2047 CV_NEXT_SEQ_ELEM( elem_size, left ); 2048 } 2049 2050 while( left.ptr != right.ptr && (r = cmp_func(right.ptr,pivot, aux)) >= 0 ) 2051 { 2052 if( r == 0 ) 2053 { 2054 if( right1.ptr != right.ptr ) 2055 CV_SWAP_ELEMS( right1.ptr, right.ptr, elem_size ); 2056 swap_cnt = 1; 2057 CV_PREV_SEQ_ELEM( elem_size, right1 ); 2058 } 2059 CV_PREV_SEQ_ELEM( elem_size, right ); 2060 } 2061 2062 if( left.ptr == right.ptr ) 2063 { 2064 r = cmp_func(left.ptr, pivot, aux); 2065 if( r == 0 ) 2066 { 2067 if( left1.ptr != left.ptr ) 2068 CV_SWAP_ELEMS( left1.ptr, left.ptr, elem_size ); 2069 swap_cnt = 1; 2070 CV_NEXT_SEQ_ELEM( elem_size, left1 ); 2071 } 2072 if( r <= 0 ) 2073 { 2074 CV_NEXT_SEQ_ELEM( elem_size, left ); 2075 } 2076 else 2077 { 2078 CV_PREV_SEQ_ELEM( elem_size, right ); 2079 } 2080 break; 2081 } 2082 2083 CV_SWAP_ELEMS( left.ptr, right.ptr, elem_size ); 2084 CV_NEXT_SEQ_ELEM( elem_size, left ); 2085 r = left.ptr == right.ptr; 2086 CV_PREV_SEQ_ELEM( elem_size, right ); 2087 swap_cnt = 1; 2088 if( r ) 2089 break; 2090 } 2091 2092 if( swap_cnt == 0 ) 2093 { 2094 left = left0, right = right0; 2095 goto insert_sort; 2096 } 2097 2098 l = cvGetSeqReaderPos( &left ); 2099 if( l == 0 ) 2100 l = seq->total; 2101 l0 = cvGetSeqReaderPos( &left0 ); 2102 l1 = cvGetSeqReaderPos( &left1 ); 2103 if( l1 == 0 ) 2104 l1 = seq->total; 2105 2106 n = MIN( l - l1, l1 - l0 ); 2107 if( n > 0 ) 2108 { 2109 tmp0 = left0; 2110 tmp1 = left; 2111 cvSetSeqReaderPos( &tmp1, 0-n, 1 ); 2112 for( i = 0; i < n; i++ ) 2113 { 2114 CV_SWAP_ELEMS( tmp0.ptr, tmp1.ptr, elem_size ); 2115 CV_NEXT_SEQ_ELEM( elem_size, tmp0 ); 2116 CV_NEXT_SEQ_ELEM( elem_size, tmp1 ); 2117 } 2118 } 2119 2120 r = cvGetSeqReaderPos( &right ); 2121 r0 = cvGetSeqReaderPos( &right0 ); 2122 r1 = cvGetSeqReaderPos( &right1 ); 2123 m = MIN( r0 - r1, r1 - r ); 2124 if( m > 0 ) 2125 { 2126 tmp0 = left; 2127 tmp1 = right0; 2128 cvSetSeqReaderPos( &tmp1, 1-m, 1 ); 2129 for( i = 0; i < m; i++ ) 2130 { 2131 CV_SWAP_ELEMS( tmp0.ptr, tmp1.ptr, elem_size ); 2132 CV_NEXT_SEQ_ELEM( elem_size, tmp0 ); 2133 CV_NEXT_SEQ_ELEM( elem_size, tmp1 ); 2134 } 2135 } 2136 2137 n = l - l1; 2138 m = r1 - r; 2139 if( n > 1 ) 2140 { 2141 if( m > 1 ) 2142 { 2143 if( n > m ) 2144 { 2145 sp++; 2146 CV_SAVE_READER_POS( left0, stack[sp].lb ); 2147 cvSetSeqReaderPos( &left0, n - 1, 1 ); 2148 CV_SAVE_READER_POS( left0, stack[sp].ub ); 2149 left = right = right0; 2150 cvSetSeqReaderPos( &left, 1 - m, 1 ); 2151 } 2152 else 2153 { 2154 sp++; 2155 CV_SAVE_READER_POS( right0, stack[sp].ub ); 2156 cvSetSeqReaderPos( &right0, 1 - m, 1 ); 2157 CV_SAVE_READER_POS( right0, stack[sp].lb ); 2158 left = right = left0; 2159 cvSetSeqReaderPos( &right, n - 1, 1 ); 2160 } 2161 } 2162 else 2163 { 2164 left = right = left0; 2165 cvSetSeqReaderPos( &right, n - 1, 1 ); 2166 } 2167 } 2168 else if( m > 1 ) 2169 { 2170 left = right = right0; 2171 cvSetSeqReaderPos( &left, 1 - m, 1 ); 2172 } 2173 else 2174 break; 2175 } 2176 } 2177 } 2178 } 2179 2180 2181 CV_IMPL schar* 2182 cvSeqSearch( CvSeq* seq, const void* _elem, CvCmpFunc cmp_func, 2183 int is_sorted, int* _idx, void* userdata ) 2184 { 2185 schar* result = 0; 2186 const schar* elem = (const schar*)_elem; 2187 int idx = -1; 2188 int i, j; 2189 2190 if( _idx ) 2191 *_idx = idx; 2192 2193 if( !CV_IS_SEQ(seq) ) 2194 CV_Error( !seq ? CV_StsNullPtr : CV_StsBadArg, "Bad input sequence" ); 2195 2196 if( !elem ) 2197 CV_Error( CV_StsNullPtr, "Null element pointer" ); 2198 2199 int elem_size = seq->elem_size; 2200 int total = seq->total; 2201 2202 if( total == 0 ) 2203 return 0; 2204 2205 if( !is_sorted ) 2206 { 2207 CvSeqReader reader; 2208 cvStartReadSeq( seq, &reader, 0 ); 2209 2210 if( cmp_func ) 2211 { 2212 for( i = 0; i < total; i++ ) 2213 { 2214 if( cmp_func( elem, reader.ptr, userdata ) == 0 ) 2215 break; 2216 CV_NEXT_SEQ_ELEM( elem_size, reader ); 2217 } 2218 } 2219 else if( (elem_size & (sizeof(int)-1)) == 0 ) 2220 { 2221 for( i = 0; i < total; i++ ) 2222 { 2223 for( j = 0; j < elem_size; j += sizeof(int) ) 2224 { 2225 if( *(const int*)(reader.ptr + j) != *(const int*)(elem + j) ) 2226 break; 2227 } 2228 if( j == elem_size ) 2229 break; 2230 CV_NEXT_SEQ_ELEM( elem_size, reader ); 2231 } 2232 } 2233 else 2234 { 2235 for( i = 0; i < total; i++ ) 2236 { 2237 for( j = 0; j < elem_size; j++ ) 2238 { 2239 if( reader.ptr[j] != elem[j] ) 2240 break; 2241 } 2242 if( j == elem_size ) 2243 break; 2244 CV_NEXT_SEQ_ELEM( elem_size, reader ); 2245 } 2246 } 2247 2248 idx = i; 2249 if( i < total ) 2250 result = reader.ptr; 2251 } 2252 else 2253 { 2254 if( !cmp_func ) 2255 CV_Error( CV_StsNullPtr, "Null compare function" ); 2256 2257 i = 0, j = total; 2258 2259 while( j > i ) 2260 { 2261 int k = (i+j)>>1, code; 2262 schar* ptr = cvGetSeqElem( seq, k ); 2263 code = cmp_func( elem, ptr, userdata ); 2264 if( !code ) 2265 { 2266 result = ptr; 2267 idx = k; 2268 if( _idx ) 2269 *_idx = idx; 2270 return result; 2271 } 2272 if( code < 0 ) 2273 j = k; 2274 else 2275 i = k+1; 2276 } 2277 idx = j; 2278 } 2279 2280 if( _idx ) 2281 *_idx = idx; 2282 2283 return result; 2284 } 2285 2286 2287 CV_IMPL void 2288 cvSeqInvert( CvSeq* seq ) 2289 { 2290 CvSeqReader left_reader, right_reader; 2291 int elem_size; 2292 int i, count; 2293 2294 cvStartReadSeq( seq, &left_reader, 0 ); 2295 cvStartReadSeq( seq, &right_reader, 1 ); 2296 elem_size = seq->elem_size; 2297 count = seq->total >> 1; 2298 2299 for( i = 0; i < count; i++ ) 2300 { 2301 CV_SWAP_ELEMS( left_reader.ptr, right_reader.ptr, elem_size ); 2302 CV_NEXT_SEQ_ELEM( elem_size, left_reader ); 2303 CV_PREV_SEQ_ELEM( elem_size, right_reader ); 2304 } 2305 } 2306 2307 2308 typedef struct CvPTreeNode 2309 { 2310 struct CvPTreeNode* parent; 2311 schar* element; 2312 int rank; 2313 } 2314 CvPTreeNode; 2315 2316 2317 // This function splits the input sequence or set into one or more equivalence classes. 2318 // is_equal(a,b,...) returns non-zero if the two sequence elements 2319 // belong to the same class. The function returns sequence of integers - 2320 // 0-based class indexes for each element. 2321 // 2322 // The algorithm is described in "Introduction to Algorithms" 2323 // by Cormen, Leiserson and Rivest, chapter "Data structures for disjoint sets" 2324 CV_IMPL int 2325 cvSeqPartition( const CvSeq* seq, CvMemStorage* storage, CvSeq** labels, 2326 CvCmpFunc is_equal, void* userdata ) 2327 { 2328 CvSeq* result = 0; 2329 CvMemStorage* temp_storage = 0; 2330 int class_idx = 0; 2331 2332 CvSeqWriter writer; 2333 CvSeqReader reader, reader0; 2334 CvSeq* nodes; 2335 int i, j; 2336 int is_set; 2337 2338 if( !labels ) 2339 CV_Error( CV_StsNullPtr, "" ); 2340 2341 if( !seq || !is_equal ) 2342 CV_Error( CV_StsNullPtr, "" ); 2343 2344 if( !storage ) 2345 storage = seq->storage; 2346 2347 if( !storage ) 2348 CV_Error( CV_StsNullPtr, "" ); 2349 2350 is_set = CV_IS_SET(seq); 2351 2352 temp_storage = cvCreateChildMemStorage( storage ); 2353 2354 nodes = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvPTreeNode), temp_storage ); 2355 2356 cvStartReadSeq( seq, &reader ); 2357 memset( &writer, 0, sizeof(writer)); 2358 cvStartAppendToSeq( nodes, &writer ); 2359 2360 // Initial O(N) pass. Make a forest of single-vertex trees. 2361 for( i = 0; i < seq->total; i++ ) 2362 { 2363 CvPTreeNode node = { 0, 0, 0 }; 2364 if( !is_set || CV_IS_SET_ELEM( reader.ptr )) 2365 node.element = reader.ptr; 2366 CV_WRITE_SEQ_ELEM( node, writer ); 2367 CV_NEXT_SEQ_ELEM( seq->elem_size, reader ); 2368 } 2369 2370 cvEndWriteSeq( &writer ); 2371 2372 // Because in the next loop we will iterate 2373 // through all the sequence nodes each time, 2374 // we do not need to initialize reader every time: 2375 cvStartReadSeq( nodes, &reader ); 2376 cvStartReadSeq( nodes, &reader0 ); 2377 2378 // The main O(N^2) pass. Merge connected components. 2379 for( i = 0; i < nodes->total; i++ ) 2380 { 2381 CvPTreeNode* node = (CvPTreeNode*)(reader0.ptr); 2382 CvPTreeNode* root = node; 2383 CV_NEXT_SEQ_ELEM( nodes->elem_size, reader0 ); 2384 2385 if( !node->element ) 2386 continue; 2387 2388 // find root 2389 while( root->parent ) 2390 root = root->parent; 2391 2392 for( j = 0; j < nodes->total; j++ ) 2393 { 2394 CvPTreeNode* node2 = (CvPTreeNode*)reader.ptr; 2395 2396 if( node2->element && node2 != node && 2397 is_equal( node->element, node2->element, userdata )) 2398 { 2399 CvPTreeNode* root2 = node2; 2400 2401 // unite both trees 2402 while( root2->parent ) 2403 root2 = root2->parent; 2404 2405 if( root2 != root ) 2406 { 2407 if( root->rank > root2->rank ) 2408 root2->parent = root; 2409 else 2410 { 2411 root->parent = root2; 2412 root2->rank += root->rank == root2->rank; 2413 root = root2; 2414 } 2415 assert( root->parent == 0 ); 2416 2417 // Compress path from node2 to the root: 2418 while( node2->parent ) 2419 { 2420 CvPTreeNode* temp = node2; 2421 node2 = node2->parent; 2422 temp->parent = root; 2423 } 2424 2425 // Compress path from node to the root: 2426 node2 = node; 2427 while( node2->parent ) 2428 { 2429 CvPTreeNode* temp = node2; 2430 node2 = node2->parent; 2431 temp->parent = root; 2432 } 2433 } 2434 } 2435 2436 CV_NEXT_SEQ_ELEM( sizeof(*node), reader ); 2437 } 2438 } 2439 2440 // Final O(N) pass (Enumerate classes) 2441 // Reuse reader one more time 2442 result = cvCreateSeq( 0, sizeof(CvSeq), sizeof(int), storage ); 2443 cvStartAppendToSeq( result, &writer ); 2444 2445 for( i = 0; i < nodes->total; i++ ) 2446 { 2447 CvPTreeNode* node = (CvPTreeNode*)reader.ptr; 2448 int idx = -1; 2449 2450 if( node->element ) 2451 { 2452 while( node->parent ) 2453 node = node->parent; 2454 if( node->rank >= 0 ) 2455 node->rank = ~class_idx++; 2456 idx = ~node->rank; 2457 } 2458 2459 CV_NEXT_SEQ_ELEM( sizeof(*node), reader ); 2460 CV_WRITE_SEQ_ELEM( idx, writer ); 2461 } 2462 2463 cvEndWriteSeq( &writer ); 2464 2465 if( labels ) 2466 *labels = result; 2467 2468 cvReleaseMemStorage( &temp_storage ); 2469 return class_idx; 2470 } 2471 2472 2473 /****************************************************************************************\ 2474 * Set implementation * 2475 \****************************************************************************************/ 2476 2477 /* Creates empty set: */ 2478 CV_IMPL CvSet* 2479 cvCreateSet( int set_flags, int header_size, int elem_size, CvMemStorage * storage ) 2480 { 2481 if( !storage ) 2482 CV_Error( CV_StsNullPtr, "" ); 2483 if( header_size < (int)sizeof( CvSet ) || 2484 elem_size < (int)sizeof(void*)*2 || 2485 (elem_size & (sizeof(void*)-1)) != 0 ) 2486 CV_Error( CV_StsBadSize, "" ); 2487 2488 CvSet* set = (CvSet*) cvCreateSeq( set_flags, header_size, elem_size, storage ); 2489 set->flags = (set->flags & ~CV_MAGIC_MASK) | CV_SET_MAGIC_VAL; 2490 2491 return set; 2492 } 2493 2494 2495 /* Add new element to the set: */ 2496 CV_IMPL int 2497 cvSetAdd( CvSet* set, CvSetElem* element, CvSetElem** inserted_element ) 2498 { 2499 int id = -1; 2500 CvSetElem *free_elem; 2501 2502 if( !set ) 2503 CV_Error( CV_StsNullPtr, "" ); 2504 2505 if( !(set->free_elems) ) 2506 { 2507 int count = set->total; 2508 int elem_size = set->elem_size; 2509 schar *ptr; 2510 icvGrowSeq( (CvSeq *) set, 0 ); 2511 2512 set->free_elems = (CvSetElem*) (ptr = set->ptr); 2513 for( ; ptr + elem_size <= set->block_max; ptr += elem_size, count++ ) 2514 { 2515 ((CvSetElem*)ptr)->flags = count | CV_SET_ELEM_FREE_FLAG; 2516 ((CvSetElem*)ptr)->next_free = (CvSetElem*)(ptr + elem_size); 2517 } 2518 assert( count <= CV_SET_ELEM_IDX_MASK+1 ); 2519 ((CvSetElem*)(ptr - elem_size))->next_free = 0; 2520 set->first->prev->count += count - set->total; 2521 set->total = count; 2522 set->ptr = set->block_max; 2523 } 2524 2525 free_elem = set->free_elems; 2526 set->free_elems = free_elem->next_free; 2527 2528 id = free_elem->flags & CV_SET_ELEM_IDX_MASK; 2529 if( element ) 2530 memcpy( free_elem, element, set->elem_size ); 2531 2532 free_elem->flags = id; 2533 set->active_count++; 2534 2535 if( inserted_element ) 2536 *inserted_element = free_elem; 2537 2538 return id; 2539 } 2540 2541 2542 /* Remove element from a set given element index: */ 2543 CV_IMPL void 2544 cvSetRemove( CvSet* set, int index ) 2545 { 2546 CvSetElem* elem = cvGetSetElem( set, index ); 2547 if( elem ) 2548 cvSetRemoveByPtr( set, elem ); 2549 else if( !set ) 2550 CV_Error( CV_StsNullPtr, "" ); 2551 } 2552 2553 2554 /* Remove all elements from a set: */ 2555 CV_IMPL void 2556 cvClearSet( CvSet* set ) 2557 { 2558 cvClearSeq( (CvSeq*)set ); 2559 set->free_elems = 0; 2560 set->active_count = 0; 2561 } 2562 2563 2564 /****************************************************************************************\ 2565 * Graph implementation * 2566 \****************************************************************************************/ 2567 2568 /* Create a new graph: */ 2569 CV_IMPL CvGraph * 2570 cvCreateGraph( int graph_type, int header_size, 2571 int vtx_size, int edge_size, CvMemStorage * storage ) 2572 { 2573 CvGraph *graph = 0; 2574 CvSet *edges = 0; 2575 CvSet *vertices = 0; 2576 2577 if( header_size < (int) sizeof( CvGraph ) 2578 || edge_size < (int) sizeof( CvGraphEdge ) 2579 || vtx_size < (int) sizeof( CvGraphVtx ) 2580 ){ 2581 CV_Error( CV_StsBadSize, "" ); 2582 } 2583 2584 vertices = cvCreateSet( graph_type, header_size, vtx_size, storage ); 2585 edges = cvCreateSet( CV_SEQ_KIND_GENERIC | CV_SEQ_ELTYPE_GRAPH_EDGE, 2586 sizeof( CvSet ), edge_size, storage ); 2587 2588 graph = (CvGraph*)vertices; 2589 graph->edges = edges; 2590 2591 return graph; 2592 } 2593 2594 2595 /* Remove all vertices and edges from a graph: */ 2596 CV_IMPL void 2597 cvClearGraph( CvGraph * graph ) 2598 { 2599 if( !graph ) 2600 CV_Error( CV_StsNullPtr, "" ); 2601 2602 cvClearSet( graph->edges ); 2603 cvClearSet( (CvSet*)graph ); 2604 } 2605 2606 2607 /* Add a vertex to a graph: */ 2608 CV_IMPL int 2609 cvGraphAddVtx( CvGraph* graph, const CvGraphVtx* _vertex, CvGraphVtx** _inserted_vertex ) 2610 { 2611 CvGraphVtx *vertex = 0; 2612 int index = -1; 2613 2614 if( !graph ) 2615 CV_Error( CV_StsNullPtr, "" ); 2616 2617 vertex = (CvGraphVtx*)cvSetNew((CvSet*)graph); 2618 if( vertex ) 2619 { 2620 if( _vertex ) 2621 memcpy( vertex + 1, _vertex + 1, graph->elem_size - sizeof(CvGraphVtx) ); 2622 vertex->first = 0; 2623 index = vertex->flags; 2624 } 2625 2626 if( _inserted_vertex ) 2627 *_inserted_vertex = vertex; 2628 2629 return index; 2630 } 2631 2632 2633 /* Remove a vertex from the graph together with its incident edges: */ 2634 CV_IMPL int 2635 cvGraphRemoveVtxByPtr( CvGraph* graph, CvGraphVtx* vtx ) 2636 { 2637 int count = -1; 2638 2639 if( !graph || !vtx ) 2640 CV_Error( CV_StsNullPtr, "" ); 2641 2642 if( !CV_IS_SET_ELEM(vtx)) 2643 CV_Error( CV_StsBadArg, "The vertex does not belong to the graph" ); 2644 2645 count = graph->edges->active_count; 2646 for( ;; ) 2647 { 2648 CvGraphEdge *edge = vtx->first; 2649 if( !edge ) 2650 break; 2651 cvGraphRemoveEdgeByPtr( graph, edge->vtx[0], edge->vtx[1] ); 2652 } 2653 count -= graph->edges->active_count; 2654 cvSetRemoveByPtr( (CvSet*)graph, vtx ); 2655 2656 return count; 2657 } 2658 2659 2660 /* Remove a vertex from the graph together with its incident edges: */ 2661 CV_IMPL int 2662 cvGraphRemoveVtx( CvGraph* graph, int index ) 2663 { 2664 int count = -1; 2665 CvGraphVtx *vtx = 0; 2666 2667 if( !graph ) 2668 CV_Error( CV_StsNullPtr, "" ); 2669 2670 vtx = cvGetGraphVtx( graph, index ); 2671 if( !vtx ) 2672 CV_Error( CV_StsBadArg, "The vertex is not found" ); 2673 2674 count = graph->edges->active_count; 2675 for( ;; ) 2676 { 2677 CvGraphEdge *edge = vtx->first; 2678 count++; 2679 2680 if( !edge ) 2681 break; 2682 cvGraphRemoveEdgeByPtr( graph, edge->vtx[0], edge->vtx[1] ); 2683 } 2684 count -= graph->edges->active_count; 2685 cvSetRemoveByPtr( (CvSet*)graph, vtx ); 2686 2687 return count; 2688 } 2689 2690 2691 /* Find a graph edge given pointers to the ending vertices: */ 2692 CV_IMPL CvGraphEdge* 2693 cvFindGraphEdgeByPtr( const CvGraph* graph, 2694 const CvGraphVtx* start_vtx, 2695 const CvGraphVtx* end_vtx ) 2696 { 2697 int ofs = 0; 2698 2699 if( !graph || !start_vtx || !end_vtx ) 2700 CV_Error( CV_StsNullPtr, "" ); 2701 2702 if( start_vtx == end_vtx ) 2703 return 0; 2704 2705 if( !CV_IS_GRAPH_ORIENTED( graph ) && 2706 (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) ) 2707 { 2708 const CvGraphVtx* t; 2709 CV_SWAP( start_vtx, end_vtx, t ); 2710 } 2711 2712 CvGraphEdge* edge = start_vtx->first; 2713 for( ; edge; edge = edge->next[ofs] ) 2714 { 2715 ofs = start_vtx == edge->vtx[1]; 2716 assert( ofs == 1 || start_vtx == edge->vtx[0] ); 2717 if( edge->vtx[1] == end_vtx ) 2718 break; 2719 } 2720 2721 return edge; 2722 } 2723 2724 2725 /* Find an edge in the graph given indices of the ending vertices: */ 2726 CV_IMPL CvGraphEdge * 2727 cvFindGraphEdge( const CvGraph* graph, int start_idx, int end_idx ) 2728 { 2729 CvGraphVtx *start_vtx; 2730 CvGraphVtx *end_vtx; 2731 2732 if( !graph ) 2733 CV_Error( CV_StsNullPtr, "graph pointer is NULL" ); 2734 2735 start_vtx = cvGetGraphVtx( graph, start_idx ); 2736 end_vtx = cvGetGraphVtx( graph, end_idx ); 2737 2738 return cvFindGraphEdgeByPtr( graph, start_vtx, end_vtx ); 2739 } 2740 2741 2742 /* Given two vertices, return the edge 2743 * connecting them, creating it if it 2744 * did not already exist: 2745 */ 2746 CV_IMPL int 2747 cvGraphAddEdgeByPtr( CvGraph* graph, 2748 CvGraphVtx* start_vtx, CvGraphVtx* end_vtx, 2749 const CvGraphEdge* _edge, 2750 CvGraphEdge ** _inserted_edge ) 2751 { 2752 CvGraphEdge *edge = 0; 2753 int result = -1; 2754 int delta; 2755 2756 if( !graph ) 2757 CV_Error( CV_StsNullPtr, "graph pointer is NULL" ); 2758 2759 if( !CV_IS_GRAPH_ORIENTED( graph ) && 2760 (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) ) 2761 { 2762 CvGraphVtx* t; 2763 CV_SWAP( start_vtx, end_vtx, t ); 2764 } 2765 2766 edge = cvFindGraphEdgeByPtr( graph, start_vtx, end_vtx ); 2767 if( edge ) 2768 { 2769 result = 0; 2770 if( _inserted_edge ) 2771 *_inserted_edge = edge; 2772 return result; 2773 } 2774 2775 if( start_vtx == end_vtx ) 2776 CV_Error( start_vtx ? CV_StsBadArg : CV_StsNullPtr, 2777 "vertex pointers coinside (or set to NULL)" ); 2778 2779 edge = (CvGraphEdge*)cvSetNew( (CvSet*)(graph->edges) ); 2780 assert( edge->flags >= 0 ); 2781 2782 edge->vtx[0] = start_vtx; 2783 edge->vtx[1] = end_vtx; 2784 edge->next[0] = start_vtx->first; 2785 edge->next[1] = end_vtx->first; 2786 start_vtx->first = end_vtx->first = edge; 2787 2788 delta = graph->edges->elem_size - sizeof(*edge); 2789 if( _edge ) 2790 { 2791 if( delta > 0 ) 2792 memcpy( edge + 1, _edge + 1, delta ); 2793 edge->weight = _edge->weight; 2794 } 2795 else 2796 { 2797 if( delta > 0 ) 2798 memset( edge + 1, 0, delta ); 2799 edge->weight = 1.f; 2800 } 2801 2802 result = 1; 2803 2804 if( _inserted_edge ) 2805 *_inserted_edge = edge; 2806 2807 return result; 2808 } 2809 2810 /* Given two vertices, return the edge 2811 * connecting them, creating it if it 2812 * did not already exist: 2813 */ 2814 CV_IMPL int 2815 cvGraphAddEdge( CvGraph* graph, 2816 int start_idx, int end_idx, 2817 const CvGraphEdge* _edge, 2818 CvGraphEdge ** _inserted_edge ) 2819 { 2820 CvGraphVtx *start_vtx; 2821 CvGraphVtx *end_vtx; 2822 2823 if( !graph ) 2824 CV_Error( CV_StsNullPtr, "" ); 2825 2826 start_vtx = cvGetGraphVtx( graph, start_idx ); 2827 end_vtx = cvGetGraphVtx( graph, end_idx ); 2828 2829 return cvGraphAddEdgeByPtr( graph, start_vtx, end_vtx, _edge, _inserted_edge ); 2830 } 2831 2832 2833 /* Remove the graph edge connecting two given vertices: */ 2834 CV_IMPL void 2835 cvGraphRemoveEdgeByPtr( CvGraph* graph, CvGraphVtx* start_vtx, CvGraphVtx* end_vtx ) 2836 { 2837 int ofs, prev_ofs; 2838 CvGraphEdge *edge, *next_edge, *prev_edge; 2839 2840 if( !graph || !start_vtx || !end_vtx ) 2841 CV_Error( CV_StsNullPtr, "" ); 2842 2843 if( start_vtx == end_vtx ) 2844 return; 2845 2846 if( !CV_IS_GRAPH_ORIENTED( graph ) && 2847 (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) ) 2848 { 2849 CvGraphVtx* t; 2850 CV_SWAP( start_vtx, end_vtx, t ); 2851 } 2852 2853 for( ofs = prev_ofs = 0, prev_edge = 0, edge = start_vtx->first; edge != 0; 2854 prev_ofs = ofs, prev_edge = edge, edge = edge->next[ofs] ) 2855 { 2856 ofs = start_vtx == edge->vtx[1]; 2857 assert( ofs == 1 || start_vtx == edge->vtx[0] ); 2858 if( edge->vtx[1] == end_vtx ) 2859 break; 2860 } 2861 2862 if( !edge ) 2863 return; 2864 2865 next_edge = edge->next[ofs]; 2866 if( prev_edge ) 2867 prev_edge->next[prev_ofs] = next_edge; 2868 else 2869 start_vtx->first = next_edge; 2870 2871 for( ofs = prev_ofs = 0, prev_edge = 0, edge = end_vtx->first; edge != 0; 2872 prev_ofs = ofs, prev_edge = edge, edge = edge->next[ofs] ) 2873 { 2874 ofs = end_vtx == edge->vtx[1]; 2875 assert( ofs == 1 || end_vtx == edge->vtx[0] ); 2876 if( edge->vtx[0] == start_vtx ) 2877 break; 2878 } 2879 2880 assert( edge != 0 ); 2881 2882 next_edge = edge->next[ofs]; 2883 if( prev_edge ) 2884 prev_edge->next[prev_ofs] = next_edge; 2885 else 2886 end_vtx->first = next_edge; 2887 2888 cvSetRemoveByPtr( graph->edges, edge ); 2889 } 2890 2891 2892 /* Remove the graph edge connecting two given vertices: */ 2893 CV_IMPL void 2894 cvGraphRemoveEdge( CvGraph* graph, int start_idx, int end_idx ) 2895 { 2896 CvGraphVtx *start_vtx; 2897 CvGraphVtx *end_vtx; 2898 2899 if( !graph ) 2900 CV_Error( CV_StsNullPtr, "" ); 2901 2902 start_vtx = cvGetGraphVtx( graph, start_idx ); 2903 end_vtx = cvGetGraphVtx( graph, end_idx ); 2904 2905 cvGraphRemoveEdgeByPtr( graph, start_vtx, end_vtx ); 2906 } 2907 2908 2909 /* Count number of edges incident to a given vertex: */ 2910 CV_IMPL int 2911 cvGraphVtxDegreeByPtr( const CvGraph* graph, const CvGraphVtx* vertex ) 2912 { 2913 CvGraphEdge *edge; 2914 int count; 2915 2916 if( !graph || !vertex ) 2917 CV_Error( CV_StsNullPtr, "" ); 2918 2919 for( edge = vertex->first, count = 0; edge; ) 2920 { 2921 count++; 2922 edge = CV_NEXT_GRAPH_EDGE( edge, vertex ); 2923 } 2924 2925 return count; 2926 } 2927 2928 2929 /* Count number of edges incident to a given vertex: */ 2930 CV_IMPL int 2931 cvGraphVtxDegree( const CvGraph* graph, int vtx_idx ) 2932 { 2933 CvGraphVtx *vertex; 2934 CvGraphEdge *edge; 2935 int count; 2936 2937 if( !graph ) 2938 CV_Error( CV_StsNullPtr, "" ); 2939 2940 vertex = cvGetGraphVtx( graph, vtx_idx ); 2941 if( !vertex ) 2942 CV_Error( CV_StsObjectNotFound, "" ); 2943 2944 for( edge = vertex->first, count = 0; edge; ) 2945 { 2946 count++; 2947 edge = CV_NEXT_GRAPH_EDGE( edge, vertex ); 2948 } 2949 2950 return count; 2951 } 2952 2953 2954 typedef struct CvGraphItem 2955 { 2956 CvGraphVtx* vtx; 2957 CvGraphEdge* edge; 2958 } 2959 CvGraphItem; 2960 2961 2962 static void 2963 icvSeqElemsClearFlags( CvSeq* seq, int offset, int clear_mask ) 2964 { 2965 CvSeqReader reader; 2966 int i, total, elem_size; 2967 2968 if( !seq ) 2969 CV_Error( CV_StsNullPtr, "" ); 2970 2971 elem_size = seq->elem_size; 2972 total = seq->total; 2973 2974 if( (unsigned)offset > (unsigned)elem_size ) 2975 CV_Error( CV_StsBadArg, "" ); 2976 2977 cvStartReadSeq( seq, &reader ); 2978 2979 for( i = 0; i < total; i++ ) 2980 { 2981 int* flag_ptr = (int*)(reader.ptr + offset); 2982 *flag_ptr &= ~clear_mask; 2983 2984 CV_NEXT_SEQ_ELEM( elem_size, reader ); 2985 } 2986 } 2987 2988 2989 static schar* 2990 icvSeqFindNextElem( CvSeq* seq, int offset, int mask, 2991 int value, int* start_index ) 2992 { 2993 schar* elem_ptr = 0; 2994 2995 CvSeqReader reader; 2996 int total, elem_size, index; 2997 2998 if( !seq || !start_index ) 2999 CV_Error( CV_StsNullPtr, "" ); 3000 3001 elem_size = seq->elem_size; 3002 total = seq->total; 3003 index = *start_index; 3004 3005 if( (unsigned)offset > (unsigned)elem_size ) 3006 CV_Error( CV_StsBadArg, "" ); 3007 3008 if( total == 0 ) 3009 return 0; 3010 3011 if( (unsigned)index >= (unsigned)total ) 3012 { 3013 index %= total; 3014 index += index < 0 ? total : 0; 3015 } 3016 3017 cvStartReadSeq( seq, &reader ); 3018 3019 if( index != 0 ) 3020 cvSetSeqReaderPos( &reader, index ); 3021 3022 for( index = 0; index < total; index++ ) 3023 { 3024 int* flag_ptr = (int*)(reader.ptr + offset); 3025 if( (*flag_ptr & mask) == value ) 3026 break; 3027 3028 CV_NEXT_SEQ_ELEM( elem_size, reader ); 3029 } 3030 3031 if( index < total ) 3032 { 3033 elem_ptr = reader.ptr; 3034 *start_index = index; 3035 } 3036 3037 return elem_ptr; 3038 } 3039 3040 #define CV_FIELD_OFFSET( field, structtype ) ((int)(size_t)&((structtype*)0)->field) 3041 3042 CV_IMPL CvGraphScanner* 3043 cvCreateGraphScanner( CvGraph* graph, CvGraphVtx* vtx, int mask ) 3044 { 3045 if( !graph ) 3046 CV_Error( CV_StsNullPtr, "Null graph pointer" ); 3047 3048 CV_Assert( graph->storage != 0 ); 3049 3050 CvGraphScanner* scanner = (CvGraphScanner*)cvAlloc( sizeof(*scanner) ); 3051 memset( scanner, 0, sizeof(*scanner)); 3052 3053 scanner->graph = graph; 3054 scanner->mask = mask; 3055 scanner->vtx = vtx; 3056 scanner->index = vtx == 0 ? 0 : -1; 3057 3058 CvMemStorage* child_storage = cvCreateChildMemStorage( graph->storage ); 3059 3060 scanner->stack = cvCreateSeq( 0, sizeof(CvSet), 3061 sizeof(CvGraphItem), child_storage ); 3062 3063 icvSeqElemsClearFlags( (CvSeq*)graph, 3064 CV_FIELD_OFFSET( flags, CvGraphVtx), 3065 CV_GRAPH_ITEM_VISITED_FLAG| 3066 CV_GRAPH_SEARCH_TREE_NODE_FLAG ); 3067 3068 icvSeqElemsClearFlags( (CvSeq*)(graph->edges), 3069 CV_FIELD_OFFSET( flags, CvGraphEdge), 3070 CV_GRAPH_ITEM_VISITED_FLAG ); 3071 3072 return scanner; 3073 } 3074 3075 3076 CV_IMPL void 3077 cvReleaseGraphScanner( CvGraphScanner** scanner ) 3078 { 3079 if( !scanner ) 3080 CV_Error( CV_StsNullPtr, "Null double pointer to graph scanner" ); 3081 3082 if( *scanner ) 3083 { 3084 if( (*scanner)->stack ) 3085 cvReleaseMemStorage( &((*scanner)->stack->storage)); 3086 cvFree( scanner ); 3087 } 3088 } 3089 3090 3091 CV_IMPL int 3092 cvNextGraphItem( CvGraphScanner* scanner ) 3093 { 3094 int code = -1; 3095 CvGraphVtx* vtx; 3096 CvGraphVtx* dst; 3097 CvGraphEdge* edge; 3098 CvGraphItem item; 3099 3100 if( !scanner || !(scanner->stack)) 3101 CV_Error( CV_StsNullPtr, "Null graph scanner" ); 3102 3103 dst = scanner->dst; 3104 vtx = scanner->vtx; 3105 edge = scanner->edge; 3106 3107 for(;;) 3108 { 3109 for(;;) 3110 { 3111 if( dst && !CV_IS_GRAPH_VERTEX_VISITED(dst) ) 3112 { 3113 scanner->vtx = vtx = dst; 3114 edge = vtx->first; 3115 dst->flags |= CV_GRAPH_ITEM_VISITED_FLAG; 3116 3117 if((scanner->mask & CV_GRAPH_VERTEX)) 3118 { 3119 scanner->vtx = vtx; 3120 scanner->edge = vtx->first; 3121 scanner->dst = 0; 3122 code = CV_GRAPH_VERTEX; 3123 return code; 3124 } 3125 } 3126 3127 while( edge ) 3128 { 3129 dst = edge->vtx[vtx == edge->vtx[0]]; 3130 3131 if( !CV_IS_GRAPH_EDGE_VISITED(edge) ) 3132 { 3133 // Check that the edge is outgoing: 3134 if( !CV_IS_GRAPH_ORIENTED( scanner->graph ) || dst != edge->vtx[0] ) 3135 { 3136 edge->flags |= CV_GRAPH_ITEM_VISITED_FLAG; 3137 3138 if( !CV_IS_GRAPH_VERTEX_VISITED(dst) ) 3139 { 3140 item.vtx = vtx; 3141 item.edge = edge; 3142 3143 vtx->flags |= CV_GRAPH_SEARCH_TREE_NODE_FLAG; 3144 3145 cvSeqPush( scanner->stack, &item ); 3146 3147 if( scanner->mask & CV_GRAPH_TREE_EDGE ) 3148 { 3149 code = CV_GRAPH_TREE_EDGE; 3150 scanner->vtx = vtx; 3151 scanner->dst = dst; 3152 scanner->edge = edge; 3153 return code; 3154 } 3155 break; 3156 } 3157 else 3158 { 3159 if( scanner->mask & (CV_GRAPH_BACK_EDGE| 3160 CV_GRAPH_CROSS_EDGE| 3161 CV_GRAPH_FORWARD_EDGE) ) 3162 { 3163 code = (dst->flags & CV_GRAPH_SEARCH_TREE_NODE_FLAG) ? 3164 CV_GRAPH_BACK_EDGE : 3165 (edge->flags & CV_GRAPH_FORWARD_EDGE_FLAG) ? 3166 CV_GRAPH_FORWARD_EDGE : CV_GRAPH_CROSS_EDGE; 3167 edge->flags &= ~CV_GRAPH_FORWARD_EDGE_FLAG; 3168 if( scanner->mask & code ) 3169 { 3170 scanner->vtx = vtx; 3171 scanner->dst = dst; 3172 scanner->edge = edge; 3173 return code; 3174 } 3175 } 3176 } 3177 } 3178 else if( (dst->flags & (CV_GRAPH_ITEM_VISITED_FLAG| 3179 CV_GRAPH_SEARCH_TREE_NODE_FLAG)) == 3180 (CV_GRAPH_ITEM_VISITED_FLAG| 3181 CV_GRAPH_SEARCH_TREE_NODE_FLAG)) 3182 { 3183 edge->flags |= CV_GRAPH_FORWARD_EDGE_FLAG; 3184 } 3185 } 3186 3187 edge = CV_NEXT_GRAPH_EDGE( edge, vtx ); 3188 } 3189 3190 if( !edge ) /* need to backtrack */ 3191 { 3192 if( scanner->stack->total == 0 ) 3193 { 3194 if( scanner->index >= 0 ) 3195 vtx = 0; 3196 else 3197 scanner->index = 0; 3198 break; 3199 } 3200 cvSeqPop( scanner->stack, &item ); 3201 vtx = item.vtx; 3202 vtx->flags &= ~CV_GRAPH_SEARCH_TREE_NODE_FLAG; 3203 edge = item.edge; 3204 dst = 0; 3205 3206 if( scanner->mask & CV_GRAPH_BACKTRACKING ) 3207 { 3208 scanner->vtx = vtx; 3209 scanner->edge = edge; 3210 scanner->dst = edge->vtx[vtx == edge->vtx[0]]; 3211 code = CV_GRAPH_BACKTRACKING; 3212 return code; 3213 } 3214 } 3215 } 3216 3217 if( !vtx ) 3218 { 3219 vtx = (CvGraphVtx*)icvSeqFindNextElem( (CvSeq*)(scanner->graph), 3220 CV_FIELD_OFFSET( flags, CvGraphVtx ), CV_GRAPH_ITEM_VISITED_FLAG|INT_MIN, 3221 0, &(scanner->index) ); 3222 3223 if( !vtx ) 3224 { 3225 code = CV_GRAPH_OVER; 3226 break; 3227 } 3228 } 3229 3230 dst = vtx; 3231 if( scanner->mask & CV_GRAPH_NEW_TREE ) 3232 { 3233 scanner->dst = dst; 3234 scanner->edge = 0; 3235 scanner->vtx = 0; 3236 code = CV_GRAPH_NEW_TREE; 3237 break; 3238 } 3239 } 3240 3241 return code; 3242 } 3243 3244 3245 CV_IMPL CvGraph* 3246 cvCloneGraph( const CvGraph* graph, CvMemStorage* storage ) 3247 { 3248 int* flag_buffer = 0; 3249 CvGraphVtx** ptr_buffer = 0; 3250 CvGraph* result = 0; 3251 3252 int i, k; 3253 int vtx_size, edge_size; 3254 CvSeqReader reader; 3255 3256 if( !CV_IS_GRAPH(graph)) 3257 CV_Error( CV_StsBadArg, "Invalid graph pointer" ); 3258 3259 if( !storage ) 3260 storage = graph->storage; 3261 3262 if( !storage ) 3263 CV_Error( CV_StsNullPtr, "NULL storage pointer" ); 3264 3265 vtx_size = graph->elem_size; 3266 edge_size = graph->edges->elem_size; 3267 3268 flag_buffer = (int*)cvAlloc( graph->total*sizeof(flag_buffer[0])); 3269 ptr_buffer = (CvGraphVtx**)cvAlloc( graph->total*sizeof(ptr_buffer[0])); 3270 result = cvCreateGraph( graph->flags, graph->header_size, 3271 vtx_size, edge_size, storage ); 3272 memcpy( result + sizeof(CvGraph), graph + sizeof(CvGraph), 3273 graph->header_size - sizeof(CvGraph)); 3274 3275 // Pass 1. Save flags, copy vertices: 3276 cvStartReadSeq( (CvSeq*)graph, &reader ); 3277 for( i = 0, k = 0; i < graph->total; i++ ) 3278 { 3279 if( CV_IS_SET_ELEM( reader.ptr )) 3280 { 3281 CvGraphVtx* vtx = (CvGraphVtx*)reader.ptr; 3282 CvGraphVtx* dstvtx = 0; 3283 cvGraphAddVtx( result, vtx, &dstvtx ); 3284 flag_buffer[k] = dstvtx->flags = vtx->flags; 3285 vtx->flags = k; 3286 ptr_buffer[k++] = dstvtx; 3287 } 3288 CV_NEXT_SEQ_ELEM( vtx_size, reader ); 3289 } 3290 3291 // Pass 2. Copy edges: 3292 cvStartReadSeq( (CvSeq*)graph->edges, &reader ); 3293 for( i = 0; i < graph->edges->total; i++ ) 3294 { 3295 if( CV_IS_SET_ELEM( reader.ptr )) 3296 { 3297 CvGraphEdge* edge = (CvGraphEdge*)reader.ptr; 3298 CvGraphEdge* dstedge = 0; 3299 CvGraphVtx* new_org = ptr_buffer[edge->vtx[0]->flags]; 3300 CvGraphVtx* new_dst = ptr_buffer[edge->vtx[1]->flags]; 3301 cvGraphAddEdgeByPtr( result, new_org, new_dst, edge, &dstedge ); 3302 dstedge->flags = edge->flags; 3303 } 3304 CV_NEXT_SEQ_ELEM( edge_size, reader ); 3305 } 3306 3307 // Pass 3. Restore flags: 3308 cvStartReadSeq( (CvSeq*)graph, &reader ); 3309 for( i = 0, k = 0; i < graph->edges->total; i++ ) 3310 { 3311 if( CV_IS_SET_ELEM( reader.ptr )) 3312 { 3313 CvGraphVtx* vtx = (CvGraphVtx*)reader.ptr; 3314 vtx->flags = flag_buffer[k++]; 3315 } 3316 CV_NEXT_SEQ_ELEM( vtx_size, reader ); 3317 } 3318 3319 cvFree( &flag_buffer ); 3320 cvFree( &ptr_buffer ); 3321 3322 if( cvGetErrStatus() < 0 ) 3323 result = 0; 3324 3325 return result; 3326 } 3327 3328 3329 /****************************************************************************************\ 3330 * Working with sequence tree * 3331 \****************************************************************************************/ 3332 3333 // Gather pointers to all the sequences, accessible from the <first>, to the single sequence. 3334 CV_IMPL CvSeq* 3335 cvTreeToNodeSeq( const void* first, int header_size, CvMemStorage* storage ) 3336 { 3337 CvSeq* allseq = 0; 3338 CvTreeNodeIterator iterator; 3339 3340 if( !storage ) 3341 CV_Error( CV_StsNullPtr, "NULL storage pointer" ); 3342 3343 allseq = cvCreateSeq( 0, header_size, sizeof(first), storage ); 3344 3345 if( first ) 3346 { 3347 cvInitTreeNodeIterator( &iterator, first, INT_MAX ); 3348 3349 for(;;) 3350 { 3351 void* node = cvNextTreeNode( &iterator ); 3352 if( !node ) 3353 break; 3354 cvSeqPush( allseq, &node ); 3355 } 3356 } 3357 3358 3359 3360 return allseq; 3361 } 3362 3363 3364 typedef struct CvTreeNode 3365 { 3366 int flags; /* micsellaneous flags */ 3367 int header_size; /* size of sequence header */ 3368 struct CvTreeNode* h_prev; /* previous sequence */ 3369 struct CvTreeNode* h_next; /* next sequence */ 3370 struct CvTreeNode* v_prev; /* 2nd previous sequence */ 3371 struct CvTreeNode* v_next; /* 2nd next sequence */ 3372 } 3373 CvTreeNode; 3374 3375 3376 3377 // Insert contour into tree given certain parent sequence. 3378 // If parent is equal to frame (the most external contour), 3379 // then added contour will have null pointer to parent: 3380 CV_IMPL void 3381 cvInsertNodeIntoTree( void* _node, void* _parent, void* _frame ) 3382 { 3383 CvTreeNode* node = (CvTreeNode*)_node; 3384 CvTreeNode* parent = (CvTreeNode*)_parent; 3385 3386 if( !node || !parent ) 3387 CV_Error( CV_StsNullPtr, "" ); 3388 3389 node->v_prev = _parent != _frame ? parent : 0; 3390 node->h_next = parent->v_next; 3391 3392 assert( parent->v_next != node ); 3393 3394 if( parent->v_next ) 3395 parent->v_next->h_prev = node; 3396 parent->v_next = node; 3397 } 3398 3399 3400 // Remove contour from tree, together with the contour's children: 3401 CV_IMPL void 3402 cvRemoveNodeFromTree( void* _node, void* _frame ) 3403 { 3404 CvTreeNode* node = (CvTreeNode*)_node; 3405 CvTreeNode* frame = (CvTreeNode*)_frame; 3406 3407 if( !node ) 3408 CV_Error( CV_StsNullPtr, "" ); 3409 3410 if( node == frame ) 3411 CV_Error( CV_StsBadArg, "frame node could not be deleted" ); 3412 3413 if( node->h_next ) 3414 node->h_next->h_prev = node->h_prev; 3415 3416 if( node->h_prev ) 3417 node->h_prev->h_next = node->h_next; 3418 else 3419 { 3420 CvTreeNode* parent = node->v_prev; 3421 if( !parent ) 3422 parent = frame; 3423 3424 if( parent ) 3425 { 3426 assert( parent->v_next == node ); 3427 parent->v_next = node->h_next; 3428 } 3429 } 3430 } 3431 3432 3433 CV_IMPL void 3434 cvInitTreeNodeIterator( CvTreeNodeIterator* treeIterator, 3435 const void* first, int max_level ) 3436 { 3437 if( !treeIterator || !first ) 3438 CV_Error( CV_StsNullPtr, "" ); 3439 3440 if( max_level < 0 ) 3441 CV_Error( CV_StsOutOfRange, "" ); 3442 3443 treeIterator->node = (void*)first; 3444 treeIterator->level = 0; 3445 treeIterator->max_level = max_level; 3446 } 3447 3448 3449 CV_IMPL void* 3450 cvNextTreeNode( CvTreeNodeIterator* treeIterator ) 3451 { 3452 CvTreeNode* prevNode = 0; 3453 CvTreeNode* node; 3454 int level; 3455 3456 if( !treeIterator ) 3457 CV_Error( CV_StsNullPtr, "NULL iterator pointer" ); 3458 3459 prevNode = node = (CvTreeNode*)treeIterator->node; 3460 level = treeIterator->level; 3461 3462 if( node ) 3463 { 3464 if( node->v_next && level+1 < treeIterator->max_level ) 3465 { 3466 node = node->v_next; 3467 level++; 3468 } 3469 else 3470 { 3471 while( node->h_next == 0 ) 3472 { 3473 node = node->v_prev; 3474 if( --level < 0 ) 3475 { 3476 node = 0; 3477 break; 3478 } 3479 } 3480 node = node && treeIterator->max_level != 0 ? node->h_next : 0; 3481 } 3482 } 3483 3484 treeIterator->node = node; 3485 treeIterator->level = level; 3486 return prevNode; 3487 } 3488 3489 3490 CV_IMPL void* 3491 cvPrevTreeNode( CvTreeNodeIterator* treeIterator ) 3492 { 3493 CvTreeNode* prevNode = 0; 3494 CvTreeNode* node; 3495 int level; 3496 3497 if( !treeIterator ) 3498 CV_Error( CV_StsNullPtr, "" ); 3499 3500 prevNode = node = (CvTreeNode*)treeIterator->node; 3501 level = treeIterator->level; 3502 3503 if( node ) 3504 { 3505 if( !node->h_prev ) 3506 { 3507 node = node->v_prev; 3508 if( --level < 0 ) 3509 node = 0; 3510 } 3511 else 3512 { 3513 node = node->h_prev; 3514 3515 while( node->v_next && level < treeIterator->max_level ) 3516 { 3517 node = node->v_next; 3518 level++; 3519 3520 while( node->h_next ) 3521 node = node->h_next; 3522 } 3523 } 3524 } 3525 3526 treeIterator->node = node; 3527 treeIterator->level = level; 3528 return prevNode; 3529 } 3530 3531 namespace cv 3532 { 3533 3534 //////////////////////////////////////////////////////////////////////////////// 3535 3536 schar* seqPush( CvSeq* seq, const void* element ) 3537 { 3538 return cvSeqPush(seq, element); 3539 } 3540 3541 schar* seqPushFront( CvSeq* seq, const void* element ) 3542 { 3543 return cvSeqPushFront(seq, element); 3544 } 3545 3546 void seqPop( CvSeq* seq, void* element ) 3547 { 3548 cvSeqPop(seq, element); 3549 } 3550 3551 void seqPopFront( CvSeq* seq, void* element ) 3552 { 3553 cvSeqPopFront(seq, element); 3554 } 3555 3556 void seqRemove( CvSeq* seq, int index ) 3557 { 3558 cvSeqRemove(seq, index); 3559 } 3560 3561 void clearSeq( CvSeq* seq ) 3562 { 3563 cvClearSeq(seq); 3564 } 3565 3566 schar* getSeqElem( const CvSeq* seq, int index ) 3567 { 3568 return cvGetSeqElem(seq, index); 3569 } 3570 3571 void seqRemoveSlice( CvSeq* seq, CvSlice slice ) 3572 { 3573 return cvSeqRemoveSlice(seq, slice); 3574 } 3575 3576 void seqInsertSlice( CvSeq* seq, int before_index, const CvArr* from_arr ) 3577 { 3578 cvSeqInsertSlice(seq, before_index, from_arr); 3579 } 3580 3581 } 3582 3583 /* End of file. */ 3584
