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