vcs.maemo.org Git - opencv/blob - cxcore/src/cxdatastructs.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41 #include "_cxcore.h"
  42
  43 #define ICV_FREE_PTR(storage)  \
  44     ((char*)(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 char 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 /* initializes 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 /* creates 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 /* creates 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 /* releases all blocks of the storage (or returns 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 /* releases 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 (returns 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 /* remembers 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 /* restores 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 /* Allocates continuous buffer of the specified size in the storage */
 384 CV_IMPL void*
 385 cvMemStorageAlloc( CvMemStorage* storage, size_t size )
 386 {
 387     char *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 /* creates 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 /* finds sequence element by its index */
 526 CV_IMPL char*
 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 /* calculates index of sequence element */
 565 CV_IMPL int
 566 cvSeqElemIdx( const CvSeq* seq, const void* _element, CvSeqBlock** _block )
 567 {
 568     const char *element = (const char *)_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 /* copies all the 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 /* constructs sequence from array without copying any data.
 683    the resultant sequence can't grow above 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 = (char *) 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 = (char *) 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's 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)(((char*)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 = (char*)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 total number of bytes in the block.
 823        And for used blocks it means a current number of sequence
 824        elements in the block */
 825     assert( block->count % seq->elem_size == 0 && block->count > 0 );
 826
 827     if( !in_front_of )
 828     {
 829         seq->ptr = block->data;
 830         seq->block_max = block->data + block->count;
 831         block->start_index = block == block->prev ? 0 :
 832             block->prev->start_index + block->prev->count;
 833     }
 834     else
 835     {
 836         int delta = block->count / seq->elem_size;
 837         block->data += block->count;
 838
 839         if( block != block->prev )
 840         {
 841             assert( seq->first->start_index == 0 );
 842             seq->first = block;
 843         }
 844         else
 845         {
 846             seq->block_max = seq->ptr = block->data;
 847         }
 848
 849         block->start_index = 0;
 850
 851         for( ;; )
 852         {
 853             block->start_index += delta;
 854             block = block->next;
 855             if( block == seq->first )
 856                 break;
 857         }
 858     }
 859
 860     block->count = 0;
 861
 862     __END__;
 863 }
 864
 865 /* recycles a sequence block for the further use */
 866 static void
 867 icvFreeSeqBlock( CvSeq *seq, int in_front_of )
 868 {
 869     /*CV_FUNCNAME( "icvFreeSeqBlock" );*/
 870
 871     __BEGIN__;
 872
 873     CvSeqBlock *block = seq->first;
 874
 875     assert( (in_front_of ? block : block->prev)->count == 0 );
 876
 877     if( block == block->prev )  /* single block case */
 878     {
 879         block->count = (int)(seq->block_max - block->data) + block->start_index * seq->elem_size;
 880         block->data = seq->block_max - block->count;
 881         seq->first = 0;
 882         seq->ptr = seq->block_max = 0;
 883         seq->total = 0;
 884     }
 885     else
 886     {
 887         if( !in_front_of )
 888         {
 889             block = block->prev;
 890             assert( seq->ptr == block->data );
 891
 892             block->count = (int)(seq->block_max - seq->ptr);
 893             seq->block_max = seq->ptr = block->prev->data +
 894                 block->prev->count * seq->elem_size;
 895         }
 896         else
 897         {
 898             int delta = block->start_index;
 899
 900             block->count = delta * seq->elem_size;
 901             block->data -= block->count;
 902
 903             /* update start indices of sequence blocks */
 904             for( ;; )
 905             {
 906                 block->start_index -= delta;
 907                 block = block->next;
 908                 if( block == seq->first )
 909                     break;
 910             }
 911
 912             seq->first = block->next;
 913         }
 914
 915         block->prev->next = block->next;
 916         block->next->prev = block->prev;
 917     }
 918
 919     assert( block->count > 0 && block->count % seq->elem_size == 0 );
 920     block->next = seq->free_blocks;
 921     seq->free_blocks = block;
 922
 923     __END__;
 924 }
 925
 926
 927 /****************************************************************************************\
 928 *                             Sequence Writer implementation                             *
 929 \****************************************************************************************/
 930
 931 /* initializes sequence writer */
 932 CV_IMPL void
 933 cvStartAppendToSeq( CvSeq *seq, CvSeqWriter * writer )
 934 {
 935     CV_FUNCNAME( "cvStartAppendToSeq" );
 936
 937     __BEGIN__;
 938
 939     if( !seq || !writer )
 940         CV_ERROR( CV_StsNullPtr, "" );
 941
 942     memset( writer, 0, sizeof( *writer ));
 943     writer->header_size = sizeof( CvSeqWriter );
 944
 945     writer->seq = seq;
 946     writer->block = seq->first ? seq->first->prev : 0;
 947     writer->ptr = seq->ptr;
 948     writer->block_max = seq->block_max;
 949
 950     __END__;
 951 }
 952
 953
 954 /* initializes sequence writer */
 955 CV_IMPL void
 956 cvStartWriteSeq( int seq_flags, int header_size,
 957                  int elem_size, CvMemStorage * storage, CvSeqWriter * writer )
 958 {
 959     CvSeq *seq = 0;
 960
 961     CV_FUNCNAME( "cvStartWriteSeq" );
 962
 963     __BEGIN__;
 964
 965     if( !storage || !writer )
 966         CV_ERROR( CV_StsNullPtr, "" );
 967
 968     CV_CALL( seq = cvCreateSeq( seq_flags, header_size, elem_size, storage ));
 969     cvStartAppendToSeq( seq, writer );
 970
 971     __END__;
 972 }
 973
 974
 975 /* updates sequence header */
 976 CV_IMPL void
 977 cvFlushSeqWriter( CvSeqWriter * writer )
 978 {
 979     CvSeq *seq = 0;
 980
 981     CV_FUNCNAME( "cvFlushSeqWriter" );
 982
 983     __BEGIN__;
 984
 985     if( !writer )
 986         CV_ERROR( CV_StsNullPtr, "" );
 987
 988     seq = writer->seq;
 989     seq->ptr = writer->ptr;
 990
 991     if( writer->block )
 992     {
 993         int total = 0;
 994         CvSeqBlock *first_block = writer->seq->first;
 995         CvSeqBlock *block = first_block;
 996
 997         writer->block->count = (int)((writer->ptr - writer->block->data) / seq->elem_size);
 998         assert( writer->block->count > 0 );
 999
1000         do
1001         {
1002             total += block->count;
1003             block = block->next;
1004         }
1005         while( block != first_block );
1006
1007         writer->seq->total = total;
1008     }
1009
1010     __END__;
1011 }
1012
1013
1014 /* calls icvFlushSeqWriter and finishes writing process */
1015 CV_IMPL CvSeq *
1016 cvEndWriteSeq( CvSeqWriter * writer )
1017 {
1018     CvSeq *seq = 0;
1019
1020     CV_FUNCNAME( "cvEndWriteSeq" );
1021
1022     __BEGIN__;
1023
1024     if( !writer )
1025         CV_ERROR( CV_StsNullPtr, "" );
1026
1027     CV_CALL( cvFlushSeqWriter( writer ));
1028     seq = writer->seq;
1029
1030     /* truncate the last block */
1031     if( writer->block && writer->seq->storage )
1032     {
1033         CvMemStorage *storage = seq->storage;
1034         char *storage_block_max = (char *) storage->top + storage->block_size;
1035
1036         assert( writer->block->count > 0 );
1037
1038         if( (unsigned)((storage_block_max - storage->free_space)
1039             - seq->block_max) < CV_STRUCT_ALIGN )
1040         {
1041             storage->free_space = cvAlignLeft((int)(storage_block_max - seq->ptr), CV_STRUCT_ALIGN);
1042             seq->block_max = seq->ptr;
1043         }
1044     }
1045
1046     writer->ptr = 0;
1047
1048     __END__;
1049
1050     return seq;
1051 }
1052
1053
1054 /* creates new sequence block */
1055 CV_IMPL void
1056 cvCreateSeqBlock( CvSeqWriter * writer )
1057 {
1058     CV_FUNCNAME( "cvCreateSeqBlock" );
1059
1060     __BEGIN__;
1061
1062     CvSeq *seq;
1063
1064     if( !writer || !writer->seq )
1065         CV_ERROR( CV_StsNullPtr, "" );
1066
1067     seq = writer->seq;
1068
1069     cvFlushSeqWriter( writer );
1070
1071     CV_CALL( icvGrowSeq( seq, 0 ));
1072
1073     writer->block = seq->first->prev;
1074     writer->ptr = seq->ptr;
1075     writer->block_max = seq->block_max;
1076
1077     __END__;
1078 }
1079
1080
1081 /****************************************************************************************\
1082 *                               Sequence Reader implementation                           *
1083 \****************************************************************************************/
1084
1085 /* initializes sequence reader */
1086 CV_IMPL void
1087 cvStartReadSeq( const CvSeq *seq, CvSeqReader * reader, int reverse )
1088 {
1089     CvSeqBlock *first_block;
1090     CvSeqBlock *last_block;
1091
1092     CV_FUNCNAME( "cvStartReadSeq" );
1093
1094     if( reader )
1095     {
1096         reader->seq = 0;
1097         reader->block = 0;
1098         reader->ptr = reader->block_max = reader->block_min = 0;
1099     }
1100
1101     __BEGIN__;
1102
1103     if( !seq || !reader )
1104         CV_ERROR( CV_StsNullPtr, "" );
1105
1106     reader->header_size = sizeof( CvSeqReader );
1107     reader->seq = (CvSeq*)seq;
1108
1109     first_block = seq->first;
1110
1111     if( first_block )
1112     {
1113         last_block = first_block->prev;
1114         reader->ptr = first_block->data;
1115         reader->prev_elem = CV_GET_LAST_ELEM( seq, last_block );
1116         reader->delta_index = seq->first->start_index;
1117
1118         if( reverse )
1119         {
1120             char *temp = reader->ptr;
1121
1122             reader->ptr = reader->prev_elem;
1123             reader->prev_elem = temp;
1124
1125             reader->block = last_block;
1126         }
1127         else
1128         {
1129             reader->block = first_block;
1130         }
1131
1132         reader->block_min = reader->block->data;
1133         reader->block_max = reader->block_min + reader->block->count * seq->elem_size;
1134     }
1135     else
1136     {
1137         reader->delta_index = 0;
1138         reader->block = 0;
1139
1140         reader->ptr = reader->prev_elem = reader->block_min = reader->block_max = 0;
1141     }
1142
1143     __END__;
1144 }
1145
1146
1147 /* changes the current reading block to the previous or to the next */
1148 CV_IMPL void
1149 cvChangeSeqBlock( void* _reader, int direction )
1150 {
1151     CV_FUNCNAME( "cvChangeSeqBlock" );
1152
1153     __BEGIN__;
1154
1155     CvSeqReader* reader = (CvSeqReader*)_reader;
1156
1157     if( !reader )
1158         CV_ERROR( CV_StsNullPtr, "" );
1159
1160     if( direction > 0 )
1161     {
1162         reader->block = reader->block->next;
1163         reader->ptr = reader->block->data;
1164     }
1165     else
1166     {
1167         reader->block = reader->block->prev;
1168         reader->ptr = CV_GET_LAST_ELEM( reader->seq, reader->block );
1169     }
1170     reader->block_min = reader->block->data;
1171     reader->block_max = reader->block_min + reader->block->count * reader->seq->elem_size;
1172
1173     __END__;
1174 }
1175
1176
1177 /* returns the current reader position */
1178 CV_IMPL int
1179 cvGetSeqReaderPos( CvSeqReader* reader )
1180 {
1181     int elem_size;
1182     int index = -1;
1183
1184     CV_FUNCNAME( "cvGetSeqReaderPos" );
1185
1186     __BEGIN__;
1187
1188     if( !reader || !reader->ptr )
1189         CV_ERROR( CV_StsNullPtr, "" );
1190
1191     elem_size = reader->seq->elem_size;
1192     if( elem_size <= ICV_SHIFT_TAB_MAX && (index = icvPower2ShiftTab[elem_size - 1]) >= 0 )
1193         index = (int)((reader->ptr - reader->block_min) >> index);
1194     else
1195         index = (int)((reader->ptr - reader->block_min) / elem_size);
1196
1197     index += reader->block->start_index - reader->delta_index;
1198
1199     __END__;
1200
1201     return index;
1202 }
1203
1204
1205 /* sets reader position to given absolute or relative
1206    (relatively to the current one) position */
1207 CV_IMPL void
1208 cvSetSeqReaderPos( CvSeqReader* reader, int index, int is_relative )
1209 {
1210     CV_FUNCNAME( "cvSetSeqReaderPos" );
1211
1212     __BEGIN__;
1213
1214     CvSeqBlock *block;
1215     int elem_size, count, total;
1216
1217     if( !reader || !reader->seq )
1218         CV_ERROR( CV_StsNullPtr, "" );
1219
1220     total = reader->seq->total;
1221     elem_size = reader->seq->elem_size;
1222
1223     if( !is_relative )
1224     {
1225         if( index < 0 )
1226         {
1227             if( index < -total )
1228                 CV_ERROR( CV_StsOutOfRange, "" );
1229             index += total;
1230         }
1231         else if( index >= total )
1232         {
1233             index -= total;
1234             if( index >= total )
1235                 CV_ERROR( CV_StsOutOfRange, "" );
1236         }
1237
1238         block = reader->seq->first;
1239         if( index >= (count = block->count) )
1240         {
1241             if( index + index <= total )
1242             {
1243                 do
1244                 {
1245                     block = block->next;
1246                     index -= count;
1247                 }
1248                 while( index >= (count = block->count) );
1249             }
1250             else
1251             {
1252                 do
1253                 {
1254                     block = block->prev;
1255                     total -= block->count;
1256                 }
1257                 while( index < total );
1258                 index -= total;
1259             }
1260         }
1261         reader->ptr = block->data + index * elem_size;
1262         if( reader->block != block )
1263         {
1264             reader->block = block;
1265             reader->block_min = block->data;
1266             reader->block_max = block->data + block->count * elem_size;
1267         }
1268     }
1269     else
1270     {
1271         char* ptr = reader->ptr;
1272         index *= elem_size;
1273         block = reader->block;
1274
1275         if( index > 0 )
1276         {
1277             while( ptr + index >= reader->block_max )
1278             {
1279                 int delta = (int)(reader->block_max - ptr);
1280                 index -= delta;
1281                 reader->block = block = block->next;
1282                 reader->block_min = ptr = block->data;
1283                 reader->block_max = block->data + block->count*elem_size;
1284             }
1285             reader->ptr = ptr + index;
1286         }
1287         else
1288         {
1289             while( ptr + index < reader->block_min )
1290             {
1291                 int delta = (int)(ptr - reader->block_min);
1292                 index += delta;
1293                 reader->block = block = block->prev;
1294                 reader->block_min = block->data;
1295                 reader->block_max = ptr = block->data + block->count*elem_size;
1296             }
1297             reader->ptr = ptr + index;
1298         }
1299     }
1300
1301     __END__;
1302 }
1303
1304
1305 /* pushes element to the sequence */
1306 CV_IMPL char*
1307 cvSeqPush( CvSeq *seq, void *element )
1308 {
1309     char *ptr = 0;
1310     size_t elem_size;
1311
1312     CV_FUNCNAME( "cvSeqPush" );
1313
1314     __BEGIN__;
1315
1316     if( !seq )
1317         CV_ERROR( CV_StsNullPtr, "" );
1318
1319     elem_size = seq->elem_size;
1320     ptr = seq->ptr;
1321
1322     if( ptr >= seq->block_max )
1323     {
1324         CV_CALL( icvGrowSeq( seq, 0 ));
1325
1326         ptr = seq->ptr;
1327         assert( ptr + elem_size <= seq->block_max /*&& ptr == seq->block_min */  );
1328     }
1329
1330     if( element )
1331         CV_MEMCPY_AUTO( ptr, element, elem_size );
1332     seq->first->prev->count++;
1333     seq->total++;
1334     seq->ptr = ptr + elem_size;
1335
1336     __END__;
1337
1338     return ptr;
1339 }
1340
1341
1342 /* pops the last element out of the sequence */
1343 CV_IMPL void
1344 cvSeqPop( CvSeq *seq, void *element )
1345 {
1346     char *ptr;
1347     int elem_size;
1348
1349     CV_FUNCNAME( "cvSeqPop" );
1350
1351     __BEGIN__;
1352
1353     if( !seq )
1354         CV_ERROR( CV_StsNullPtr, "" );
1355     if( seq->total <= 0 )
1356         CV_ERROR( CV_StsBadSize, "" );
1357
1358     elem_size = seq->elem_size;
1359     seq->ptr = ptr = seq->ptr - elem_size;
1360
1361     if( element )
1362         CV_MEMCPY_AUTO( element, ptr, elem_size );
1363     seq->ptr = ptr;
1364     seq->total--;
1365
1366     if( --(seq->first->prev->count) == 0 )
1367     {
1368         icvFreeSeqBlock( seq, 0 );
1369         assert( seq->ptr == seq->block_max );
1370     }
1371
1372     __END__;
1373 }
1374
1375
1376 /* pushes element to the front of the sequence */
1377 CV_IMPL char*
1378 cvSeqPushFront( CvSeq *seq, void *element )
1379 {
1380     char* ptr = 0;
1381     int elem_size;
1382     CvSeqBlock *block;
1383
1384     CV_FUNCNAME( "cvSeqPushFront" );
1385
1386     __BEGIN__;
1387
1388     if( !seq )
1389         CV_ERROR( CV_StsNullPtr, "" );
1390
1391     elem_size = seq->elem_size;
1392     block = seq->first;
1393
1394     if( !block || block->start_index == 0 )
1395     {
1396         CV_CALL( icvGrowSeq( seq, 1 ));
1397
1398         block = seq->first;
1399         assert( block->start_index > 0 );
1400     }
1401
1402     ptr = block->data -= elem_size;
1403
1404     if( element )
1405         CV_MEMCPY_AUTO( ptr, element, elem_size );
1406     block->count++;
1407     block->start_index--;
1408     seq->total++;
1409
1410     __END__;
1411
1412     return ptr;
1413 }
1414
1415
1416 /* pulls out the first element of the sequence */
1417 CV_IMPL void
1418 cvSeqPopFront( CvSeq *seq, void *element )
1419 {
1420     int elem_size;
1421     CvSeqBlock *block;
1422
1423     CV_FUNCNAME( "cvSeqPopFront" );
1424
1425     __BEGIN__;
1426
1427     if( !seq )
1428         CV_ERROR( CV_StsNullPtr, "" );
1429     if( seq->total <= 0 )
1430         CV_ERROR( CV_StsBadSize, "" );
1431
1432     elem_size = seq->elem_size;
1433     block = seq->first;
1434
1435     if( element )
1436         CV_MEMCPY_AUTO( element, block->data, elem_size );
1437     block->data += elem_size;
1438     block->start_index++;
1439     seq->total--;
1440
1441     if( --(block->count) == 0 )
1442     {
1443         icvFreeSeqBlock( seq, 1 );
1444     }
1445
1446     __END__;
1447 }
1448
1449 /* inserts new element in the middle of the sequence */
1450 CV_IMPL char*
1451 cvSeqInsert( CvSeq *seq, int before_index, void *element )
1452 {
1453     int elem_size;
1454     int block_size;
1455     CvSeqBlock *block;
1456     int delta_index;
1457     int total;
1458     char* ret_ptr = 0;
1459
1460     CV_FUNCNAME( "cvSeqInsert" );
1461
1462     __BEGIN__;
1463
1464     if( !seq )
1465         CV_ERROR( CV_StsNullPtr, "" );
1466
1467     total = seq->total;
1468     before_index += before_index < 0 ? total : 0;
1469     before_index -= before_index > total ? total : 0;
1470
1471     if( (unsigned)before_index > (unsigned)total )
1472         CV_ERROR( CV_StsOutOfRange, "" );
1473
1474     if( before_index == total )
1475     {
1476         CV_CALL( ret_ptr = cvSeqPush( seq, element ));
1477     }
1478     else if( before_index == 0 )
1479     {
1480         CV_CALL( ret_ptr = cvSeqPushFront( seq, element ));
1481     }
1482     else
1483     {
1484         elem_size = seq->elem_size;
1485
1486         if( before_index >= total >> 1 )
1487         {
1488             char *ptr = seq->ptr + elem_size;
1489
1490             if( ptr > seq->block_max )
1491             {
1492                 CV_CALL( icvGrowSeq( seq, 0 ));
1493
1494                 ptr = seq->ptr + elem_size;
1495                 assert( ptr <= seq->block_max );
1496             }
1497
1498             delta_index = seq->first->start_index;
1499             block = seq->first->prev;
1500             block->count++;
1501             block_size = (int)(ptr - block->data);
1502
1503             while( before_index < block->start_index - delta_index )
1504             {
1505                 CvSeqBlock *prev_block = block->prev;
1506
1507                 memmove( block->data + elem_size, block->data, block_size - elem_size );
1508                 block_size = prev_block->count * elem_size;
1509                 memcpy( block->data, prev_block->data + block_size - elem_size, elem_size );
1510                 block = prev_block;
1511
1512                 /* check that we don't fall in the infinite loop */
1513                 assert( block != seq->first->prev );
1514             }
1515
1516             before_index = (before_index - block->start_index + delta_index) * elem_size;
1517             memmove( block->data + before_index + elem_size, block->data + before_index,
1518                      block_size - before_index - elem_size );
1519
1520             ret_ptr = block->data + before_index;
1521
1522             if( element )
1523                 memcpy( ret_ptr, element, elem_size );
1524             seq->ptr = ptr;
1525         }
1526         else
1527         {
1528             block = seq->first;
1529
1530             if( block->start_index == 0 )
1531             {
1532                 CV_CALL( icvGrowSeq( seq, 1 ));
1533
1534                 block = seq->first;
1535             }
1536
1537             delta_index = block->start_index;
1538             block->count++;
1539             block->start_index--;
1540             block->data -= elem_size;
1541
1542             while( before_index > block->start_index - delta_index + block->count )
1543             {
1544                 CvSeqBlock *next_block = block->next;
1545
1546                 block_size = block->count * elem_size;
1547                 memmove( block->data, block->data + elem_size, block_size - elem_size );
1548                 memcpy( block->data + block_size - elem_size, next_block->data, elem_size );
1549                 block = next_block;
1550                 /* check that we don't fall in the infinite loop */
1551                 assert( block != seq->first );
1552             }
1553
1554             before_index = (before_index - block->start_index + delta_index) * elem_size;
1555             memmove( block->data, block->data + elem_size, before_index - elem_size );
1556
1557             ret_ptr = block->data + before_index - elem_size;
1558
1559             if( element )
1560                 memcpy( ret_ptr, element, elem_size );
1561         }
1562
1563         seq->total = total + 1;
1564     }
1565
1566     __END__;
1567
1568     return ret_ptr;
1569 }
1570
1571
1572 /* removes element from the sequence */
1573 CV_IMPL void
1574 cvSeqRemove( CvSeq *seq, int index )
1575 {
1576     char *ptr;
1577     int elem_size;
1578     int block_size;
1579     CvSeqBlock *block;
1580     int delta_index;
1581     int total, front = 0;
1582
1583     CV_FUNCNAME( "cvSeqRemove" );
1584
1585     __BEGIN__;
1586
1587     if( !seq )
1588         CV_ERROR( CV_StsNullPtr, "" );
1589
1590     total = seq->total;
1591
1592     index += index < 0 ? total : 0;
1593     index -= index >= total ? total : 0;
1594
1595     if( (unsigned) index >= (unsigned) total )
1596         CV_ERROR( CV_StsOutOfRange, "Invalid index" );
1597
1598     if( index == total - 1 )
1599     {
1600         cvSeqPop( seq, 0 );
1601     }
1602     else if( index == 0 )
1603     {
1604         cvSeqPopFront( seq, 0 );
1605     }
1606     else
1607     {
1608         block = seq->first;
1609         elem_size = seq->elem_size;
1610         delta_index = block->start_index;
1611         while( block->start_index - delta_index + block->count <= index )
1612             block = block->next;
1613
1614         ptr = block->data + (index - block->start_index + delta_index) * elem_size;
1615
1616         front = index < total >> 1;
1617         if( !front )
1618         {
1619             block_size = block->count * elem_size - (int)(ptr - block->data);
1620
1621             while( block != seq->first->prev )  /* while not the last block */
1622             {
1623                 CvSeqBlock *next_block = block->next;
1624
1625                 memmove( ptr, ptr + elem_size, block_size - elem_size );
1626                 memcpy( ptr + block_size - elem_size, next_block->data, elem_size );
1627                 block = next_block;
1628                 ptr = block->data;
1629                 block_size = block->count * elem_size;
1630             }
1631
1632             memmove( ptr, ptr + elem_size, block_size - elem_size );
1633             seq->ptr -= elem_size;
1634         }
1635         else
1636         {
1637             ptr += elem_size;
1638             block_size = (int)(ptr - block->data);
1639
1640             while( block != seq->first )
1641             {
1642                 CvSeqBlock *prev_block = block->prev;
1643
1644                 memmove( block->data + elem_size, block->data, block_size - elem_size );
1645                 block_size = prev_block->count * elem_size;
1646                 memcpy( block->data, prev_block->data + block_size - elem_size, elem_size );
1647                 block = prev_block;
1648             }
1649
1650             memmove( block->data + elem_size, block->data, block_size - elem_size );
1651             block->data += elem_size;
1652             block->start_index++;
1653         }
1654
1655         seq->total = total - 1;
1656         if( --block->count == 0 )
1657             icvFreeSeqBlock( seq, front );
1658     }
1659
1660     __END__;
1661 }
1662
1663
1664 /* adds several elements to the end or in the beginning of sequence */
1665 CV_IMPL void
1666 cvSeqPushMulti( CvSeq *seq, void *_elements, int count, int front )
1667 {
1668     char *elements = (char *) _elements;
1669
1670     CV_FUNCNAME( "cvSeqPushMulti" );
1671
1672     __BEGIN__;
1673     int elem_size;
1674
1675     if( !seq )
1676         CV_ERROR( CV_StsNullPtr, "NULL sequence pointer" );
1677     if( count < 0 )
1678         CV_ERROR( CV_StsBadSize, "number of removed elements is negative" );
1679
1680     elem_size = seq->elem_size;
1681
1682     if( !front )
1683     {
1684         while( count > 0 )
1685         {
1686             int delta = (int)((seq->block_max - seq->ptr) / elem_size);
1687
1688             delta = MIN( delta, count );
1689             if( delta > 0 )
1690             {
1691                 seq->first->prev->count += delta;
1692                 seq->total += delta;
1693                 count -= delta;
1694                 delta *= elem_size;
1695                 if( elements )
1696                 {
1697                     memcpy( seq->ptr, elements, delta );
1698                     elements += delta;
1699                 }
1700                 seq->ptr += delta;
1701             }
1702
1703             if( count > 0 )
1704                 CV_CALL( icvGrowSeq( seq, 0 ));
1705         }
1706     }
1707     else
1708     {
1709         CvSeqBlock* block = seq->first;
1710
1711         while( count > 0 )
1712         {
1713             int delta;
1714
1715             if( !block || block->start_index == 0 )
1716             {
1717                 CV_CALL( icvGrowSeq( seq, 1 ));
1718
1719                 block = seq->first;
1720                 assert( block->start_index > 0 );
1721             }
1722
1723             delta = MIN( block->start_index, count );
1724             count -= delta;
1725             block->start_index -= delta;
1726             block->count += delta;
1727             seq->total += delta;
1728             delta *= elem_size;
1729             block->data -= delta;
1730
1731             if( elements )
1732                 memcpy( block->data, elements + count*elem_size, delta );
1733         }
1734     }
1735
1736     __END__;
1737 }
1738
1739
1740 /* removes several elements from the end of sequence */
1741 CV_IMPL void
1742 cvSeqPopMulti( CvSeq *seq, void *_elements, int count, int front )
1743 {
1744     char *elements = (char *) _elements;
1745
1746     CV_FUNCNAME( "cvSeqPopMulti" );
1747
1748     __BEGIN__;
1749
1750     if( !seq )
1751         CV_ERROR( CV_StsNullPtr, "NULL sequence pointer" );
1752     if( count < 0 )
1753         CV_ERROR( CV_StsBadSize, "number of removed elements is negative" );
1754
1755     count = MIN( count, seq->total );
1756
1757     if( !front )
1758     {
1759         if( elements )
1760             elements += count * seq->elem_size;
1761
1762         while( count > 0 )
1763         {
1764             int delta = seq->first->prev->count;
1765
1766             delta = MIN( delta, count );
1767             assert( delta > 0 );
1768
1769             seq->first->prev->count -= delta;
1770             seq->total -= delta;
1771             count -= delta;
1772             delta *= seq->elem_size;
1773             seq->ptr -= delta;
1774
1775             if( elements )
1776             {
1777                 elements -= delta;
1778                 memcpy( elements, seq->ptr, delta );
1779             }
1780
1781             if( seq->first->prev->count == 0 )
1782                 icvFreeSeqBlock( seq, 0 );
1783         }
1784     }
1785     else
1786     {
1787         while( count > 0 )
1788         {
1789             int delta = seq->first->count;
1790
1791             delta = MIN( delta, count );
1792             assert( delta > 0 );
1793
1794             seq->first->count -= delta;
1795             seq->total -= delta;
1796             count -= delta;
1797             seq->first->start_index += delta;
1798             delta *= seq->elem_size;
1799
1800             if( elements )
1801             {
1802                 memcpy( elements, seq->first->data, delta );
1803                 elements += delta;
1804             }
1805
1806             seq->first->data += delta;
1807             if( seq->first->count == 0 )
1808                 icvFreeSeqBlock( seq, 1 );
1809         }
1810     }
1811
1812     __END__;
1813 }
1814
1815
1816 /* removes all elements from the sequence */
1817 CV_IMPL void
1818 cvClearSeq( CvSeq *seq )
1819 {
1820     CV_FUNCNAME( "cvClearSeq" );
1821
1822     __BEGIN__;
1823
1824     if( !seq )
1825         CV_ERROR( CV_StsNullPtr, "" );
1826     cvSeqPopMulti( seq, 0, seq->total );
1827
1828     __END__;
1829 }
1830
1831
1832 CV_IMPL CvSeq*
1833 cvSeqSlice( const CvSeq* seq, CvSlice slice, CvMemStorage* storage, int copy_data )
1834 {
1835     CvSeq* subseq = 0;
1836
1837     CV_FUNCNAME("cvSeqSlice");
1838
1839     __BEGIN__;
1840
1841     int elem_size, count, length;
1842     CvSeqReader reader;
1843     CvSeqBlock *block, *first_block = 0, *last_block = 0;
1844
1845     if( !CV_IS_SEQ(seq) )
1846         CV_ERROR( CV_StsBadArg, "Invalid sequence header" );
1847
1848     if( !storage )
1849     {
1850         storage = seq->storage;
1851         if( !storage )
1852             CV_ERROR( CV_StsNullPtr, "NULL storage pointer" );
1853     }
1854
1855     elem_size = seq->elem_size;
1856     length = cvSliceLength( slice, seq );
1857     if( slice.start_index < 0 )
1858         slice.start_index += seq->total;
1859     else if( slice.start_index >= seq->total )
1860         slice.start_index -= seq->total;
1861     if( (unsigned)length > (unsigned)seq->total ||
1862         ((unsigned)slice.start_index >= (unsigned)seq->total && length != 0) )
1863         CV_ERROR( CV_StsOutOfRange, "Bad sequence slice" );
1864
1865     CV_CALL( subseq = cvCreateSeq( seq->flags, seq->header_size, elem_size, storage ));
1866
1867     if( length > 0 )
1868     {
1869         cvStartReadSeq( seq, &reader, 0 );
1870         cvSetSeqReaderPos( &reader, slice.start_index, 0 );
1871         count = (int)((reader.block_max - reader.ptr)/elem_size);
1872
1873         do
1874         {
1875             int bl = MIN( count, length );
1876
1877             if( !copy_data )
1878             {
1879                 block = (CvSeqBlock*)cvMemStorageAlloc( storage, sizeof(*block) );
1880                 if( !first_block )
1881                 {
1882                     first_block = subseq->first = block->prev = block->next = block;
1883                     block->start_index = 0;
1884                 }
1885                 else
1886                 {
1887                     block->prev = last_block;
1888                     block->next = first_block;
1889                     last_block->next = first_block->prev = block;
1890                     block->start_index = last_block->start_index + last_block->count;
1891                 }
1892                 last_block = block;
1893                 block->data = reader.ptr;
1894                 block->count = bl;
1895                 subseq->total += bl;
1896             }
1897             else
1898                 cvSeqPushMulti( subseq, reader.ptr, bl, 0 );
1899             length -= bl;
1900             reader.block = reader.block->next;
1901             reader.ptr = reader.block->data;
1902             count = reader.block->count;
1903         }
1904         while( length > 0 );
1905     }
1906
1907     __END__;
1908
1909     return subseq;
1910 }
1911
1912
1913 // Remove slice from the middle of the sequence
1914 // !!! TODO !!! Implement more efficient algorithm
1915 CV_IMPL void
1916 cvSeqRemoveSlice( CvSeq* seq, CvSlice slice )
1917 {
1918     CV_FUNCNAME("cvSeqRemoveSlice");
1919
1920     __BEGIN__;
1921
1922     int total, length;
1923
1924     if( !CV_IS_SEQ(seq) )
1925         CV_ERROR( CV_StsBadArg, "Invalid sequence header" );
1926
1927     length = cvSliceLength( slice, seq );
1928     total = seq->total;
1929
1930     if( slice.start_index < 0 )
1931         slice.start_index += total;
1932     else if( slice.start_index >= total )
1933         slice.start_index -= total;
1934
1935     if( (unsigned)slice.start_index >= (unsigned)total )
1936         CV_ERROR( CV_StsOutOfRange, "start slice index is out of range" );
1937
1938     slice.end_index = slice.start_index + length;
1939
1940     if( slice.end_index < total )
1941     {
1942         CvSeqReader reader_to, reader_from;
1943         int elem_size = seq->elem_size;
1944
1945         cvStartReadSeq( seq, &reader_to );
1946         cvStartReadSeq( seq, &reader_from );
1947
1948         if( slice.start_index > total - slice.end_index )
1949         {
1950             int i, count = seq->total - slice.end_index;
1951             cvSetSeqReaderPos( &reader_to, slice.start_index );
1952             cvSetSeqReaderPos( &reader_from, slice.end_index );
1953
1954             for( i = 0; i < count; i++ )
1955             {
1956                 CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );
1957                 CV_NEXT_SEQ_ELEM( elem_size, reader_to );
1958                 CV_NEXT_SEQ_ELEM( elem_size, reader_from );
1959             }
1960
1961             cvSeqPopMulti( seq, 0, slice.end_index - slice.start_index );
1962         }
1963         else
1964         {
1965             int i, count = slice.start_index;
1966             cvSetSeqReaderPos( &reader_to, slice.end_index );
1967             cvSetSeqReaderPos( &reader_from, slice.start_index );
1968
1969             for( i = 0; i < count; i++ )
1970             {
1971                 CV_PREV_SEQ_ELEM( elem_size, reader_to );
1972                 CV_PREV_SEQ_ELEM( elem_size, reader_from );
1973
1974                 CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );
1975             }
1976
1977             cvSeqPopMulti( seq, 0, slice.end_index - slice.start_index, 1 );
1978         }
1979     }
1980     else
1981     {
1982         cvSeqPopMulti( seq, 0, total - slice.start_index );
1983         cvSeqPopMulti( seq, 0, slice.end_index - total, 1 );
1984     }
1985
1986     __END__;
1987 }
1988
1989
1990 // Inserts a new sequence into the middle of another sequence
1991 // !!! TODO !!! Implement more efficient algorithm
1992 CV_IMPL void
1993 cvSeqInsertSlice( CvSeq* seq, int index, const CvArr* from_arr )
1994 {
1995     CvSeqReader reader_to, reader_from;
1996     int i, elem_size, total, from_total;
1997
1998     CV_FUNCNAME("cvSeqInsertSlice");
1999
2000     __BEGIN__;
2001
2002     CvSeq from_header, *from = (CvSeq*)from_arr;
2003     CvSeqBlock block;
2004
2005     if( !CV_IS_SEQ(seq) )
2006         CV_ERROR( CV_StsBadArg, "Invalid destination sequence header" );
2007
2008     if( !CV_IS_SEQ(from))
2009     {
2010         CvMat* mat = (CvMat*)from;
2011         if( !CV_IS_MAT(mat))
2012             CV_ERROR( CV_StsBadArg, "Source is not a sequence nor matrix" );
2013
2014         if( !CV_IS_MAT_CONT(mat->type) || (mat->rows != 1 && mat->cols != 1) )
2015             CV_ERROR( CV_StsBadArg, "The source array must be 1d coninuous vector" );
2016
2017         CV_CALL( from = cvMakeSeqHeaderForArray( CV_SEQ_KIND_GENERIC, sizeof(from_header),
2018                                                  CV_ELEM_SIZE(mat->type),
2019                                                  mat->data.ptr, mat->cols + mat->rows - 1,
2020                                                  &from_header, &block ));
2021     }
2022
2023     if( seq->elem_size != from->elem_size )
2024         CV_ERROR( CV_StsUnmatchedSizes,
2025         "Sizes of source and destination sequences' elements are different" );
2026
2027     from_total = from->total;
2028
2029     if( from_total == 0 )
2030         EXIT;
2031
2032     total = seq->total;
2033     index += index < 0 ? total : 0;
2034     index -= index > total ? total : 0;
2035
2036     if( (unsigned)index > (unsigned)total )
2037         CV_ERROR( CV_StsOutOfRange, "" );
2038
2039     elem_size = seq->elem_size;
2040
2041     if( index < (total >> 1) )
2042     {
2043         cvSeqPushMulti( seq, 0, from_total, 1 );
2044
2045         cvStartReadSeq( seq, &reader_to );
2046         cvStartReadSeq( seq, &reader_from );
2047         cvSetSeqReaderPos( &reader_from, from_total );
2048
2049         for( i = 0; i < index; i++ )
2050         {
2051             CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );
2052             CV_NEXT_SEQ_ELEM( elem_size, reader_to );
2053             CV_NEXT_SEQ_ELEM( elem_size, reader_from );
2054         }
2055     }
2056     else
2057     {
2058         cvSeqPushMulti( seq, 0, from_total );
2059
2060         cvStartReadSeq( seq, &reader_to );
2061         cvStartReadSeq( seq, &reader_from );
2062         cvSetSeqReaderPos( &reader_from, total );
2063         cvSetSeqReaderPos( &reader_to, seq->total );
2064
2065         for( i = 0; i < total - index; i++ )
2066         {
2067             CV_PREV_SEQ_ELEM( elem_size, reader_to );
2068             CV_PREV_SEQ_ELEM( elem_size, reader_from );
2069             CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );
2070         }
2071     }
2072
2073     cvStartReadSeq( from, &reader_from );
2074     cvSetSeqReaderPos( &reader_to, index );
2075
2076     for( i = 0; i < from_total; i++ )
2077     {
2078         CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );
2079         CV_NEXT_SEQ_ELEM( elem_size, reader_to );
2080         CV_NEXT_SEQ_ELEM( elem_size, reader_from );
2081     }
2082
2083     __END__;
2084 }
2085
2086 // Sort the sequence using user-specified comparison function.
2087 // The semantics is similar to qsort() function. The code is based
2088 // on BSD system qsort():
2089 //    * Copyright (c) 1992, 1993
2090 //    *  The Regents of the University of California.  All rights reserved.
2091 //    *
2092 //    * Redistribution and use in source and binary forms, with or without
2093 //    * modification, are permitted provided that the following conditions
2094 //    * are met:
2095 //    * 1. Redistributions of source code must retain the above copyright
2096 //    *    notice, this list of conditions and the following disclaimer.
2097 //    * 2. Redistributions in binary form must reproduce the above copyright
2098 //    *    notice, this list of conditions and the following disclaimer in the
2099 //    *    documentation and/or other materials provided with the distribution.
2100 //    * 3. All advertising materials mentioning features or use of this software
2101 //    *    must display the following acknowledgement:
2102 //    *  This product includes software developed by the University of
2103 //    *  California, Berkeley and its contributors.
2104 //    * 4. Neither the name of the University nor the names of its contributors
2105 //    *    may be used to endorse or promote products derived from this software
2106 //    *    without specific prior written permission.
2107 //    *
2108 //    * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
2109 //    * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
2110 //    * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
2111 //    * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
2112 //    * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
2113 //    * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
2114 //    * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
2115 //    * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
2116 //    * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
2117 //    * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
2118 //    * SUCH DAMAGE.
2119
2120 typedef struct CvSeqReaderPos
2121 {
2122     CvSeqBlock* block;
2123     char* ptr;
2124     char* block_min;
2125     char* block_max;
2126 }
2127 CvSeqReaderPos;
2128
2129 #define CV_SAVE_READER_POS( reader, pos )   \
2130 {                                           \
2131     (pos).block = (reader).block;           \
2132     (pos).ptr = (reader).ptr;               \
2133     (pos).block_min = (reader).block_min;   \
2134     (pos).block_max = (reader).block_max;   \
2135 }
2136
2137 #define CV_RESTORE_READER_POS( reader, pos )\
2138 {                                           \
2139     (reader).block = (pos).block;           \
2140     (reader).ptr = (pos).ptr;               \
2141     (reader).block_min = (pos).block_min;   \
2142     (reader).block_max = (pos).block_max;   \
2143 }
2144
2145 inline char*
2146 icvMed3( char* a, char* b, char* c, CvCmpFunc cmp_func, void* aux )
2147 {
2148     return cmp_func(a, b, aux) < 0 ?
2149       (cmp_func(b, c, aux) < 0 ? b : cmp_func(a, c, aux) < 0 ? c : a)
2150      :(cmp_func(b, c, aux) > 0 ? b : cmp_func(a, c, aux) < 0 ? a : c);
2151 }
2152
2153 CV_IMPL void
2154 cvSeqSort( CvSeq* seq, CvCmpFunc cmp_func, void* aux )
2155 {
2156     int elem_size;
2157     int isort_thresh = 7;
2158     CvSeqReader left, right;
2159     int sp = 0;
2160
2161     struct
2162     {
2163         CvSeqReaderPos lb;
2164         CvSeqReaderPos ub;
2165     }
2166     stack[48];
2167
2168     CV_FUNCNAME( "cvSeqSort" );
2169
2170     __BEGIN__;
2171
2172     if( !CV_IS_SEQ(seq) )
2173         CV_ERROR( !seq ? CV_StsNullPtr : CV_StsBadArg, "Bad input sequence" );
2174
2175     if( !cmp_func )
2176         CV_ERROR( CV_StsNullPtr, "Null compare function" );
2177
2178     if( seq->total <= 1 )
2179         EXIT;
2180
2181     elem_size = seq->elem_size;
2182     isort_thresh *= elem_size;
2183
2184     cvStartReadSeq( seq, &left, 0 );
2185     right = left;
2186     CV_SAVE_READER_POS( left, stack[0].lb );
2187     CV_PREV_SEQ_ELEM( elem_size, right );
2188     CV_SAVE_READER_POS( right, stack[0].ub );
2189
2190     while( sp >= 0 )
2191     {
2192         CV_RESTORE_READER_POS( left, stack[sp].lb );
2193         CV_RESTORE_READER_POS( right, stack[sp].ub );
2194         sp--;
2195
2196         for(;;)
2197         {
2198             int i, n, m;
2199             CvSeqReader ptr, ptr2;
2200
2201             if( left.block == right.block )
2202                 n = (int)(right.ptr - left.ptr) + elem_size;
2203             else
2204             {
2205                 n = cvGetSeqReaderPos( &right );
2206                 n = (n - cvGetSeqReaderPos( &left ) + 1)*elem_size;
2207             }
2208
2209             if( n <= isort_thresh )
2210             {
2211             insert_sort:
2212                 ptr = ptr2 = left;
2213                 CV_NEXT_SEQ_ELEM( elem_size, ptr );
2214                 CV_NEXT_SEQ_ELEM( elem_size, right );
2215                 while( ptr.ptr != right.ptr )
2216                 {
2217                     ptr2.ptr = ptr.ptr;
2218                     if( ptr2.block != ptr.block )
2219                     {
2220                         ptr2.block = ptr.block;
2221                         ptr2.block_min = ptr.block_min;
2222                         ptr2.block_max = ptr.block_max;
2223                     }
2224                     while( ptr2.ptr != left.ptr )
2225                     {
2226                         char* cur = ptr2.ptr;
2227                         CV_PREV_SEQ_ELEM( elem_size, ptr2 );
2228                         if( cmp_func( ptr2.ptr, cur, aux ) <= 0 )
2229                             break;
2230                         CV_SWAP_ELEMS( ptr2.ptr, cur, elem_size );
2231                     }
2232                     CV_NEXT_SEQ_ELEM( elem_size, ptr );
2233                 }
2234                 break;
2235             }
2236             else
2237             {
2238                 CvSeqReader left0, left1, right0, right1;
2239                 CvSeqReader tmp0, tmp1;
2240                 char *m1, *m2, *m3, *pivot;
2241                 int swap_cnt = 0;
2242                 int l, l0, l1, r, r0, r1;
2243
2244                 left0 = tmp0 = left;
2245                 right0 = right1 = right;
2246                 n /= elem_size;
2247
2248                 if( n > 40 )
2249                 {
2250                     int d = n / 8;
2251                     char *p1, *p2, *p3;
2252                     p1 = tmp0.ptr;
2253                     cvSetSeqReaderPos( &tmp0, d, 1 );
2254                     p2 = tmp0.ptr;
2255                     cvSetSeqReaderPos( &tmp0, d, 1 );
2256                     p3 = tmp0.ptr;
2257                     m1 = icvMed3( p1, p2, p3, cmp_func, aux );
2258                     cvSetSeqReaderPos( &tmp0, (n/2) - d*3, 1 );
2259                     p1 = tmp0.ptr;
2260                     cvSetSeqReaderPos( &tmp0, d, 1 );
2261                     p2 = tmp0.ptr;
2262                     cvSetSeqReaderPos( &tmp0, d, 1 );
2263                     p3 = tmp0.ptr;
2264                     m2 = icvMed3( p1, p2, p3, cmp_func, aux );
2265                     cvSetSeqReaderPos( &tmp0, n - 1 - d*3 - n/2, 1 );
2266                     p1 = tmp0.ptr;
2267                     cvSetSeqReaderPos( &tmp0, d, 1 );
2268                     p2 = tmp0.ptr;
2269                     cvSetSeqReaderPos( &tmp0, d, 1 );
2270                     p3 = tmp0.ptr;
2271                     m3 = icvMed3( p1, p2, p3, cmp_func, aux );
2272                 }
2273                 else
2274                 {
2275                     m1 = tmp0.ptr;
2276                     cvSetSeqReaderPos( &tmp0, n/2, 1 );
2277                     m2 = tmp0.ptr;
2278                     cvSetSeqReaderPos( &tmp0, n - 1 - n/2, 1 );
2279                     m3 = tmp0.ptr;
2280                 }
2281
2282                 pivot = icvMed3( m1, m2, m3, cmp_func, aux );
2283                 left = left0;
2284                 if( pivot != left.ptr )
2285                 {
2286                     CV_SWAP_ELEMS( pivot, left.ptr, elem_size );
2287                     pivot = left.ptr;
2288                 }
2289                 CV_NEXT_SEQ_ELEM( elem_size, left );
2290                 left1 = left;
2291
2292                 for(;;)
2293                 {
2294                     while( left.ptr != right.ptr && (r = cmp_func(left.ptr, pivot, aux)) <= 0 )
2295                     {
2296                         if( r == 0 )
2297                         {
2298                             if( left1.ptr != left.ptr )
2299                                 CV_SWAP_ELEMS( left1.ptr, left.ptr, elem_size );
2300                             swap_cnt = 1;
2301                             CV_NEXT_SEQ_ELEM( elem_size, left1 );
2302                         }
2303                         CV_NEXT_SEQ_ELEM( elem_size, left );
2304                     }
2305
2306                     while( left.ptr != right.ptr && (r = cmp_func(right.ptr,pivot, aux)) >= 0 )
2307                     {
2308                         if( r == 0 )
2309                         {
2310                             if( right1.ptr != right.ptr )
2311                                 CV_SWAP_ELEMS( right1.ptr, right.ptr, elem_size );
2312                             swap_cnt = 1;
2313                             CV_PREV_SEQ_ELEM( elem_size, right1 );
2314                         }
2315                         CV_PREV_SEQ_ELEM( elem_size, right );
2316                     }
2317
2318                     if( left.ptr == right.ptr )
2319                     {
2320                         r = cmp_func(left.ptr, pivot, aux);
2321                         if( r == 0 )
2322                         {
2323                             if( left1.ptr != left.ptr )
2324                                 CV_SWAP_ELEMS( left1.ptr, left.ptr, elem_size );
2325                             swap_cnt = 1;
2326                             CV_NEXT_SEQ_ELEM( elem_size, left1 );
2327                         }
2328                         if( r <= 0 )
2329                         {
2330                             CV_NEXT_SEQ_ELEM( elem_size, left );
2331                         }
2332                         else
2333                         {
2334                             CV_PREV_SEQ_ELEM( elem_size, right );
2335                         }
2336                         break;
2337                     }
2338
2339                     CV_SWAP_ELEMS( left.ptr, right.ptr, elem_size );
2340                     CV_NEXT_SEQ_ELEM( elem_size, left );
2341                     r = left.ptr == right.ptr;
2342                     CV_PREV_SEQ_ELEM( elem_size, right );
2343                     swap_cnt = 1;
2344                     if( r )
2345                         break;
2346                 }
2347
2348                 if( swap_cnt == 0 )
2349                 {
2350                     left = left0, right = right0;
2351                     goto insert_sort;
2352                 }
2353
2354                 l = cvGetSeqReaderPos( &left );
2355                 if( l == 0 )
2356                     l = seq->total;
2357                 l0 = cvGetSeqReaderPos( &left0 );
2358                 l1 = cvGetSeqReaderPos( &left1 );
2359                 if( l1 == 0 )
2360                     l1 = seq->total;
2361
2362                 n = MIN( l - l1, l1 - l0 );
2363                 if( n > 0 )
2364                 {
2365                     tmp0 = left0;
2366                     tmp1 = left;
2367                     cvSetSeqReaderPos( &tmp1, 0-n, 1 );
2368                     for( i = 0; i < n; i++ )
2369                     {
2370                         CV_SWAP_ELEMS( tmp0.ptr, tmp1.ptr, elem_size );
2371                         CV_NEXT_SEQ_ELEM( elem_size, tmp0 );
2372                         CV_NEXT_SEQ_ELEM( elem_size, tmp1 );
2373                     }
2374                 }
2375
2376                 r = cvGetSeqReaderPos( &right );
2377                 r0 = cvGetSeqReaderPos( &right0 );
2378                 r1 = cvGetSeqReaderPos( &right1 );
2379                 m = MIN( r0 - r1, r1 - r );
2380                 if( m > 0 )
2381                 {
2382                     tmp0 = left;
2383                     tmp1 = right0;
2384                     cvSetSeqReaderPos( &tmp1, 1-m, 1 );
2385                     for( i = 0; i < m; i++ )
2386                     {
2387                         CV_SWAP_ELEMS( tmp0.ptr, tmp1.ptr, elem_size );
2388                         CV_NEXT_SEQ_ELEM( elem_size, tmp0 );
2389                         CV_NEXT_SEQ_ELEM( elem_size, tmp1 );
2390                     }
2391                 }
2392
2393                 n = l - l1;
2394                 m = r1 - r;
2395                 if( n > 1 )
2396                 {
2397                     if( m > 1 )
2398                     {
2399                         if( n > m )
2400                         {
2401                             sp++;
2402                             CV_SAVE_READER_POS( left0, stack[sp].lb );
2403                             cvSetSeqReaderPos( &left0, n - 1, 1 );
2404                             CV_SAVE_READER_POS( left0, stack[sp].ub );
2405                             left = right = right0;
2406                             cvSetSeqReaderPos( &left, 1 - m, 1 );
2407                         }
2408                         else
2409                         {
2410                             sp++;
2411                             CV_SAVE_READER_POS( right0, stack[sp].ub );
2412                             cvSetSeqReaderPos( &right0, 1 - m, 1 );
2413                             CV_SAVE_READER_POS( right0, stack[sp].lb );
2414                             left = right = left0;
2415                             cvSetSeqReaderPos( &right, n - 1, 1 );
2416                         }
2417                     }
2418                     else
2419                     {
2420                         left = right = left0;
2421                         cvSetSeqReaderPos( &right, n - 1, 1 );
2422                     }
2423                 }
2424                 else if( m > 1 )
2425                 {
2426                     left = right = right0;
2427                     cvSetSeqReaderPos( &left, 1 - m, 1 );
2428                 }
2429                 else
2430                     break;
2431             }
2432         }
2433     }
2434
2435     __END__;
2436 }
2437
2438
2439 CV_IMPL char*
2440 cvSeqSearch( CvSeq* seq, const void* _elem, CvCmpFunc cmp_func,
2441              int is_sorted, int* _idx, void* userdata )
2442 {
2443     char* result = 0;
2444     const char* elem = (const char*)_elem;
2445     int idx = -1;
2446
2447     CV_FUNCNAME("cvSeqSort");
2448
2449     __BEGIN__;
2450
2451     int elem_size, i, j, total;
2452
2453     if( !CV_IS_SEQ(seq) )
2454         CV_ERROR( !seq ? CV_StsNullPtr : CV_StsBadArg, "Bad input sequence" );
2455
2456     if( !elem )
2457         CV_ERROR( CV_StsNullPtr, "Null element pointer" );
2458
2459     elem_size = seq->elem_size;
2460     total = seq->total;
2461
2462     if( total == 0 )
2463         EXIT;
2464
2465     if( !is_sorted )
2466     {
2467         CvSeqReader reader;
2468         cvStartReadSeq( seq, &reader, 0 );
2469
2470         if( cmp_func )
2471         {
2472             for( i = 0; i < total; i++ )
2473             {
2474                 if( cmp_func( elem, reader.ptr, userdata ) == 0 )
2475                     break;
2476                 CV_NEXT_SEQ_ELEM( elem_size, reader );
2477             }
2478         }
2479         else if( (elem_size & (sizeof(int)-1)) == 0 )
2480         {
2481             for( i = 0; i < total; i++ )
2482             {
2483                 for( j = 0; j < elem_size; j += sizeof(int) )
2484                 {
2485                     if( *(const int*)(reader.ptr + j) != *(const int*)(elem + j) )
2486                         break;
2487                 }
2488                 if( j == elem_size )
2489                     break;
2490                 CV_NEXT_SEQ_ELEM( elem_size, reader );
2491             }
2492         }
2493         else
2494         {
2495             for( i = 0; i < total; i++ )
2496             {
2497                 for( j = 0; j < elem_size; j++ )
2498                 {
2499                     if( reader.ptr[j] != elem[j] )
2500                         break;
2501                 }
2502                 if( j == elem_size )
2503                     break;
2504                 CV_NEXT_SEQ_ELEM( elem_size, reader );
2505             }
2506         }
2507
2508         idx = i;
2509         if( i < total )
2510             result = reader.ptr;
2511     }
2512     else
2513     {
2514         if( !cmp_func )
2515             CV_ERROR( CV_StsNullPtr, "Null compare function" );
2516
2517         i = 0, j = total;
2518
2519         while( j > i )
2520         {
2521             int k = (i+j)>>1, code;
2522             char* ptr = cvGetSeqElem( seq, k );
2523             code = cmp_func( elem, ptr, userdata );
2524             if( !code )
2525             {
2526                 result = ptr;
2527                 idx = k;
2528                 EXIT;
2529             }
2530             if( code < 0 )
2531                 j = k;
2532             else
2533                 i = k+1;
2534         }
2535         idx = j;
2536     }
2537
2538     __END__;
2539
2540     if( _idx )
2541         *_idx = idx;
2542
2543     return result;
2544 }
2545
2546
2547 CV_IMPL void
2548 cvSeqInvert( CvSeq* seq )
2549 {
2550     CV_FUNCNAME( "cvSeqInvert" );
2551
2552     __BEGIN__;
2553
2554     CvSeqReader left_reader, right_reader;
2555     int elem_size;
2556     int i, count;
2557
2558     CV_CALL( cvStartReadSeq( seq, &left_reader, 0 ));
2559     CV_CALL( cvStartReadSeq( seq, &right_reader, 1 ));
2560     elem_size = seq->elem_size;
2561     count = seq->total >> 1;
2562
2563     for( i = 0; i < count; i++ )
2564     {
2565         CV_SWAP_ELEMS( left_reader.ptr, right_reader.ptr, elem_size );
2566         CV_NEXT_SEQ_ELEM( elem_size, left_reader );
2567         CV_PREV_SEQ_ELEM( elem_size, right_reader );
2568     }
2569
2570     __END__;
2571 }
2572
2573
2574 typedef struct CvPTreeNode
2575 {
2576     struct CvPTreeNode* parent;
2577     char* element;
2578     int rank;
2579 }
2580 CvPTreeNode;
2581
2582
2583 // the function splits the input sequence or set into one or more equivalence classes.
2584 // is_equal(a,b,...) returns non-zero if the two sequence elements
2585 // belong to the same class. the function returns sequence of integers -
2586 // 0-based class indexes for each element.
2587 //
2588 // The algorithm is described in "Introduction to Algorithms"
2589 // by Cormen, Leiserson and Rivest, chapter "Data structures for disjoint sets"
2590 CV_IMPL  int
2591 cvSeqPartition( const CvSeq* seq, CvMemStorage* storage, CvSeq** labels,
2592                 CvCmpFunc is_equal, void* userdata )
2593 {
2594     CvSeq* result = 0;
2595     CvMemStorage* temp_storage = 0;
2596     int class_idx = 0;
2597
2598     CV_FUNCNAME( "cvSeqPartition" );
2599
2600     __BEGIN__;
2601
2602     CvSeqWriter writer;
2603     CvSeqReader reader, reader0;
2604     CvSeq* nodes;
2605     int i, j;
2606     int is_set;
2607
2608     if( !labels )
2609         CV_ERROR( CV_StsNullPtr, "" );
2610
2611     if( !seq || !is_equal )
2612         CV_ERROR( CV_StsNullPtr, "" );
2613
2614     if( !storage )
2615         storage = seq->storage;
2616
2617     if( !storage )
2618         CV_ERROR( CV_StsNullPtr, "" );
2619
2620     is_set = CV_IS_SET(seq);
2621
2622     temp_storage = cvCreateChildMemStorage( storage );
2623
2624     nodes = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvPTreeNode), temp_storage );
2625
2626     cvStartReadSeq( seq, &reader );
2627     memset( &writer, 0, sizeof(writer));
2628     cvStartAppendToSeq( nodes, &writer );
2629
2630     // Initial O(N) pass. Make a forest of single-vertex trees.
2631     for( i = 0; i < seq->total; i++ )
2632     {
2633         CvPTreeNode node = { 0, 0, 0 };
2634         if( !is_set || CV_IS_SET_ELEM( reader.ptr ))
2635             node.element = reader.ptr;
2636         CV_WRITE_SEQ_ELEM( node, writer );
2637         CV_NEXT_SEQ_ELEM( seq->elem_size, reader );
2638     }
2639
2640     cvEndWriteSeq( &writer );
2641
2642     // because in the next loop we will iterate
2643     // through all the sequence nodes each time,
2644     // we do not need to initialize reader every time
2645     cvStartReadSeq( nodes, &reader );
2646     cvStartReadSeq( nodes, &reader0 );
2647
2648     // The main O(N^2) pass. Merge connected components.
2649     for( i = 0; i < nodes->total; i++ )
2650     {
2651         CvPTreeNode* node = (CvPTreeNode*)(reader0.ptr);
2652         CvPTreeNode* root = node;
2653         CV_NEXT_SEQ_ELEM( nodes->elem_size, reader0 );
2654
2655         if( !node->element )
2656             continue;
2657
2658         // find root
2659         while( root->parent )
2660             root = root->parent;
2661
2662         for( j = 0; j < nodes->total; j++ )
2663         {
2664             CvPTreeNode* node2 = (CvPTreeNode*)reader.ptr;
2665
2666             if( node2->element && node2 != node &&
2667                 is_equal( node->element, node2->element, userdata ))
2668             {
2669                 CvPTreeNode* root2 = node2;
2670
2671                 // unite both trees
2672                 while( root2->parent )
2673                     root2 = root2->parent;
2674
2675                 if( root2 != root )
2676                 {
2677                     if( root->rank > root2->rank )
2678                         root2->parent = root;
2679                     else
2680                     {
2681                         root->parent = root2;
2682                         root2->rank += root->rank == root2->rank;
2683                         root = root2;
2684                     }
2685                     assert( root->parent == 0 );
2686
2687                     // compress path from node2 to the root
2688                     while( node2->parent )
2689                     {
2690                         CvPTreeNode* temp = node2;
2691                         node2 = node2->parent;
2692                         temp->parent = root;
2693                     }
2694
2695                     // compress path from node to the root
2696                     node2 = node;
2697                     while( node2->parent )
2698                     {
2699                         CvPTreeNode* temp = node2;
2700                         node2 = node2->parent;
2701                         temp->parent = root;
2702                     }
2703                 }
2704             }
2705
2706             CV_NEXT_SEQ_ELEM( sizeof(*node), reader );
2707         }
2708     }
2709
2710     // Final O(N) pass (Enumerate classes)
2711     // Reuse reader one more time
2712     result = cvCreateSeq( 0, sizeof(CvSeq), sizeof(int), storage );
2713     cvStartAppendToSeq( result, &writer );
2714
2715     for( i = 0; i < nodes->total; i++ )
2716     {
2717         CvPTreeNode* node = (CvPTreeNode*)reader.ptr;
2718         int idx = -1;
2719
2720         if( node->element )
2721         {
2722             while( node->parent )
2723                 node = node->parent;
2724             if( node->rank >= 0 )
2725                 node->rank = ~class_idx++;
2726             idx = ~node->rank;
2727         }
2728
2729         CV_NEXT_SEQ_ELEM( sizeof(*node), reader );
2730         CV_WRITE_SEQ_ELEM( idx, writer );
2731     }
2732
2733     cvEndWriteSeq( &writer );
2734
2735     __END__;
2736
2737     if( labels )
2738         *labels = result;
2739
2740     cvReleaseMemStorage( &temp_storage );
2741     return class_idx;
2742 }
2743
2744
2745 /****************************************************************************************\
2746 *                                      Set implementation                                *
2747 \****************************************************************************************/
2748
2749 /* creates empty set */
2750 CV_IMPL CvSet*
2751 cvCreateSet( int set_flags, int header_size, int elem_size, CvMemStorage * storage )
2752 {
2753     CvSet *set = 0;
2754
2755     CV_FUNCNAME( "cvCreateSet" );
2756
2757     __BEGIN__;
2758
2759     if( !storage )
2760         CV_ERROR( CV_StsNullPtr, "" );
2761     if( header_size < (int)sizeof( CvSet ) ||
2762         elem_size < (int)sizeof(void*)*2 ||
2763         (elem_size & (sizeof(void*)-1)) != 0 )
2764         CV_ERROR( CV_StsBadSize, "" );
2765
2766     set = (CvSet*) cvCreateSeq( set_flags, header_size, elem_size, storage );
2767     set->flags = (set->flags & ~CV_MAGIC_MASK) | CV_SET_MAGIC_VAL;
2768
2769     __END__;
2770
2771     return set;
2772 }
2773
2774
2775 /* adds new element to the set */
2776 CV_IMPL int
2777 cvSetAdd( CvSet* set, CvSetElem* element, CvSetElem** inserted_element )
2778 {
2779     int id = -1;
2780
2781     CV_FUNCNAME( "cvSetAdd" );
2782
2783     __BEGIN__;
2784
2785     CvSetElem *free_elem;
2786
2787     if( !set )
2788         CV_ERROR( CV_StsNullPtr, "" );
2789
2790     if( !(set->free_elems) )
2791     {
2792         int count = set->total;
2793         int elem_size = set->elem_size;
2794         char *ptr;
2795         CV_CALL( icvGrowSeq( (CvSeq *) set, 0 ));
2796
2797         set->free_elems = (CvSetElem*) (ptr = set->ptr);
2798         for( ; ptr + elem_size <= set->block_max; ptr += elem_size, count++ )
2799         {
2800             ((CvSetElem*)ptr)->flags = count | CV_SET_ELEM_FREE_FLAG;
2801             ((CvSetElem*)ptr)->next_free = (CvSetElem*)(ptr + elem_size);
2802         }
2803         assert( count <= CV_SET_ELEM_IDX_MASK+1 );
2804         ((CvSetElem*)(ptr - elem_size))->next_free = 0;
2805         set->first->prev->count += count - set->total;
2806         set->total = count;
2807         set->ptr = set->block_max;
2808     }
2809
2810     free_elem = set->free_elems;
2811     set->free_elems = free_elem->next_free;
2812
2813     id = free_elem->flags & CV_SET_ELEM_IDX_MASK;
2814     if( element )
2815         CV_MEMCPY_INT( free_elem, element, (size_t)set->elem_size/sizeof(int) );
2816
2817     free_elem->flags = id;
2818     set->active_count++;
2819
2820     if( inserted_element )
2821         *inserted_element = free_elem;
2822
2823     __END__;
2824
2825     return id;
2826 }
2827
2828
2829 /* removes element from the set given its index */
2830 CV_IMPL void
2831 cvSetRemove( CvSet* set, int index )
2832 {
2833     CV_FUNCNAME( "cvSetRemove" );
2834
2835     __BEGIN__;
2836
2837     CvSetElem* elem = cvGetSetElem( set, index );
2838     if( elem )
2839         cvSetRemoveByPtr( set, elem );
2840     else if( !set )
2841         CV_ERROR( CV_StsNullPtr, "" );
2842
2843     __END__;
2844 }
2845
2846
2847 /* removes all elements from the set */
2848 CV_IMPL void
2849 cvClearSet( CvSet* set )
2850 {
2851     CV_FUNCNAME( "cvClearSet" );
2852
2853     __BEGIN__;
2854
2855     CV_CALL( cvClearSeq( (CvSeq*)set ));
2856     set->free_elems = 0;
2857     set->active_count = 0;
2858
2859     __END__;
2860 }
2861
2862
2863 /****************************************************************************************\
2864 *                                 Graph  implementation                                  *
2865 \****************************************************************************************/
2866
2867 /* creates new graph */
2868 CV_IMPL CvGraph *
2869 cvCreateGraph( int graph_type, int header_size,
2870                int vtx_size, int edge_size, CvMemStorage * storage )
2871 {
2872     CvGraph *graph = 0;
2873     CvSet *edges = 0;
2874
2875     CV_FUNCNAME( "cvCleateGraph" );
2876
2877     __BEGIN__;
2878
2879     CvSet *vertices = 0;
2880
2881     if( header_size < (int)sizeof( CvGraph ) ||
2882         edge_size < (int)sizeof( CvGraphEdge ) ||
2883         vtx_size < (int)sizeof( CvGraphVtx ))
2884         CV_ERROR( CV_StsBadSize, "" );
2885
2886     CV_CALL( vertices = cvCreateSet( graph_type, header_size, vtx_size, storage ));
2887     CV_CALL( edges = cvCreateSet( CV_SEQ_KIND_GENERIC | CV_SEQ_ELTYPE_GRAPH_EDGE,
2888                                   sizeof( CvSet ), edge_size, storage ));
2889
2890     graph = (CvGraph*)vertices;
2891     graph->edges = edges;
2892
2893     __END__;
2894
2895     return graph;
2896 }
2897
2898
2899 /* Removes all the vertices and edges from the graph */
2900 CV_IMPL void
2901 cvClearGraph( CvGraph * graph )
2902 {
2903     CV_FUNCNAME( "cvClearGraph" );
2904
2905     __BEGIN__;
2906
2907     if( !graph )
2908         CV_ERROR( CV_StsNullPtr, "" );
2909
2910     cvClearSet( graph->edges );
2911     cvClearSet( (CvSet*)graph );
2912
2913     __END__;
2914 }
2915
2916
2917 /* Adds vertex to the graph */
2918 CV_IMPL int
2919 cvGraphAddVtx( CvGraph* graph, const CvGraphVtx* _vertex, CvGraphVtx** _inserted_vertex )
2920 {
2921     CvGraphVtx *vertex = 0;
2922     int index = -1;
2923
2924     CV_FUNCNAME( "cvGraphAddVtx" );
2925
2926     __BEGIN__;
2927
2928     if( !graph )
2929         CV_ERROR( CV_StsNullPtr, "" );
2930
2931     vertex = (CvGraphVtx*)cvSetNew((CvSet*)graph);
2932     if( vertex )
2933     {
2934         if( _vertex )
2935             CV_MEMCPY_INT( vertex + 1, _vertex + 1,
2936                 (size_t)(graph->elem_size - sizeof(CvGraphVtx))/sizeof(int) );
2937         vertex->first = 0;
2938         index = vertex->flags;
2939     }
2940
2941     if( _inserted_vertex )
2942         *_inserted_vertex = vertex;
2943
2944     __END__;
2945
2946     return index;
2947 }
2948
2949
2950 /* Removes vertex from the graph together with incident edges */
2951 CV_IMPL int
2952 cvGraphRemoveVtxByPtr( CvGraph* graph, CvGraphVtx* vtx )
2953 {
2954     int count = -1;
2955
2956     CV_FUNCNAME( "cvGraphRemoveVtxByPtr" );
2957
2958     __BEGIN__;
2959
2960     if( !graph || !vtx )
2961         CV_ERROR( CV_StsNullPtr, "" );
2962
2963     if( !CV_IS_SET_ELEM(vtx))
2964         CV_ERROR( CV_StsBadArg, "The vertex does not belong to the graph" );
2965
2966     count = graph->edges->active_count;
2967     for( ;; )
2968     {
2969         CvGraphEdge *edge = vtx->first;
2970         if( !edge )
2971             break;
2972         cvGraphRemoveEdgeByPtr( graph, edge->vtx[0], edge->vtx[1] );
2973     }
2974     count -= graph->edges->active_count;
2975     cvSetRemoveByPtr( (CvSet*)graph, vtx );
2976
2977     __END__;
2978
2979     return count;
2980 }
2981
2982
2983 /* Removes vertex from the graph together with incident edges */
2984 CV_IMPL int
2985 cvGraphRemoveVtx( CvGraph* graph, int index )
2986 {
2987     int count = -1;
2988     CvGraphVtx *vtx = 0;
2989
2990     CV_FUNCNAME( "cvGraphRemoveVtx" );
2991
2992     __BEGIN__;
2993
2994     if( !graph )
2995         CV_ERROR( CV_StsNullPtr, "" );
2996
2997     vtx = cvGetGraphVtx( graph, index );
2998     if( !vtx )
2999         CV_ERROR( CV_StsBadArg, "The vertex is not found" );
3000
3001     count = graph->edges->active_count;
3002     for( ;; )
3003     {
3004         CvGraphEdge *edge = vtx->first;
3005         count++;
3006
3007         if( !edge )
3008             break;
3009         cvGraphRemoveEdgeByPtr( graph, edge->vtx[0], edge->vtx[1] );
3010     }
3011     count -= graph->edges->active_count;
3012     cvSetRemoveByPtr( (CvSet*)graph, vtx );
3013
3014     __END__;
3015
3016     return count;
3017 }
3018
3019
3020 /* Finds graph edge given pointers to the ending vertices */
3021 CV_IMPL CvGraphEdge*
3022 cvFindGraphEdgeByPtr( const CvGraph* graph,
3023                       const CvGraphVtx* start_vtx,
3024                       const CvGraphVtx* end_vtx )
3025 {
3026     CvGraphEdge *edge = 0;
3027     CV_FUNCNAME( "cvFindGraphEdgeByPtr" );
3028
3029     __BEGIN__;
3030
3031     int ofs = 0;
3032
3033     if( !graph || !start_vtx || !end_vtx )
3034         CV_ERROR( CV_StsNullPtr, "" );
3035
3036     if( start_vtx == end_vtx )
3037         EXIT;
3038
3039     if( !CV_IS_GRAPH_ORIENTED( graph ) &&
3040         (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) )
3041     {
3042         const CvGraphVtx* t;
3043         CV_SWAP( start_vtx, end_vtx, t );
3044     }
3045
3046     edge = start_vtx->first;
3047     for( ; edge; edge = edge->next[ofs] )
3048     {
3049         ofs = start_vtx == edge->vtx[1];
3050         assert( ofs == 1 || start_vtx == edge->vtx[0] );
3051         if( edge->vtx[1] == end_vtx )
3052             break;
3053     }
3054
3055     __END__;
3056
3057     return edge;
3058 }
3059
3060
3061 /* Finds edge in the graph given indices of the ending vertices */
3062 CV_IMPL CvGraphEdge *
3063 cvFindGraphEdge( const CvGraph* graph, int start_idx, int end_idx )
3064 {
3065     CvGraphEdge *edge = 0;
3066     CvGraphVtx *start_vtx;
3067     CvGraphVtx *end_vtx;
3068
3069     CV_FUNCNAME( "cvFindGraphEdge" );
3070
3071     __BEGIN__;
3072
3073     if( !graph )
3074         CV_ERROR( CV_StsNullPtr, "graph pointer is NULL" );
3075
3076     start_vtx = cvGetGraphVtx( graph, start_idx );
3077     end_vtx = cvGetGraphVtx( graph, end_idx );
3078
3079     edge = cvFindGraphEdgeByPtr( graph, start_vtx, end_vtx );
3080
3081     __END__;
3082
3083     return edge;
3084 }
3085
3086
3087 /* Connects the two given vertices if they were not connected before,
3088    otherwise it returns the existing edge */
3089 CV_IMPL int
3090 cvGraphAddEdgeByPtr( CvGraph* graph,
3091                      CvGraphVtx* start_vtx, CvGraphVtx* end_vtx,
3092                      const CvGraphEdge* _edge,
3093                      CvGraphEdge ** _inserted_edge )
3094 {
3095     CvGraphEdge *edge = 0;
3096     int result = -1;
3097
3098     CV_FUNCNAME( "cvGraphAddEdgeByPtr" );
3099
3100     __BEGIN__;
3101
3102     int delta;
3103
3104     if( !graph )
3105         CV_ERROR( CV_StsNullPtr, "graph pointer is NULL" );
3106
3107     if( !CV_IS_GRAPH_ORIENTED( graph ) &&
3108         (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) )
3109     {
3110         CvGraphVtx* t;
3111         CV_SWAP( start_vtx, end_vtx, t );
3112     }
3113
3114     CV_CALL( edge = cvFindGraphEdgeByPtr( graph, start_vtx, end_vtx ));
3115     if( edge )
3116     {
3117         result = 0;
3118         EXIT;
3119     }
3120
3121     if( start_vtx == end_vtx )
3122         CV_ERROR( start_vtx ? CV_StsBadArg : CV_StsNullPtr,
3123         "vertex pointers coinside (or set to NULL)" );
3124
3125     CV_CALL( edge = (CvGraphEdge*)cvSetNew( (CvSet*)(graph->edges) ));
3126     assert( edge->flags >= 0 );
3127
3128     edge->vtx[0] = start_vtx;
3129     edge->vtx[1] = end_vtx;
3130     edge->next[0] = start_vtx->first;
3131     edge->next[1] = end_vtx->first;
3132     start_vtx->first = end_vtx->first = edge;
3133
3134     delta = (graph->edges->elem_size - sizeof(*edge))/sizeof(int);
3135     if( _edge )
3136     {
3137         if( delta > 0 )
3138             CV_MEMCPY_INT( edge + 1, _edge + 1, delta );
3139         edge->weight = _edge->weight;
3140     }
3141     else
3142     {
3143         if( delta > 0 )
3144             CV_ZERO_INT( edge + 1, delta );
3145         edge->weight = 1.f;
3146     }
3147
3148     result = 1;
3149
3150     __END__;
3151
3152     if( _inserted_edge )
3153         *_inserted_edge = edge;
3154
3155     return result;
3156 }
3157
3158 /* Connects the two given vertices if they were not connected before,
3159    otherwise it returns the existing edge */
3160 CV_IMPL int
3161 cvGraphAddEdge( CvGraph* graph,
3162                 int start_idx, int end_idx,
3163                 const CvGraphEdge* _edge,
3164                 CvGraphEdge ** _inserted_edge )
3165 {
3166     CvGraphVtx *start_vtx;
3167     CvGraphVtx *end_vtx;
3168     int result = -1;
3169
3170     CV_FUNCNAME( "cvGraphAddEdge" );
3171
3172     __BEGIN__;
3173
3174     if( !graph )
3175         CV_ERROR( CV_StsNullPtr, "" );
3176
3177     start_vtx = cvGetGraphVtx( graph, start_idx );
3178     end_vtx = cvGetGraphVtx( graph, end_idx );
3179
3180     result = cvGraphAddEdgeByPtr( graph, start_vtx, end_vtx, _edge, _inserted_edge );
3181
3182     __END__;
3183
3184     return result;
3185 }
3186
3187
3188 /* Removes the graph edge connecting two given vertices */
3189 CV_IMPL void
3190 cvGraphRemoveEdgeByPtr( CvGraph* graph, CvGraphVtx* start_vtx, CvGraphVtx* end_vtx )
3191 {
3192     CV_FUNCNAME( "cvGraphRemoveEdgeByPtr" );
3193
3194     __BEGIN__;
3195
3196     int ofs, prev_ofs;
3197     CvGraphEdge *edge, *next_edge, *prev_edge;
3198
3199     if( !graph || !start_vtx || !end_vtx )
3200         CV_ERROR( CV_StsNullPtr, "" );
3201
3202     if( start_vtx == end_vtx )
3203         EXIT;
3204
3205     if( !CV_IS_GRAPH_ORIENTED( graph ) &&
3206         (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) )
3207     {
3208         CvGraphVtx* t;
3209         CV_SWAP( start_vtx, end_vtx, t );
3210     }
3211
3212     for( ofs = prev_ofs = 0, prev_edge = 0, edge = start_vtx->first; edge != 0;
3213          prev_ofs = ofs, prev_edge = edge, edge = edge->next[ofs] )
3214     {
3215         ofs = start_vtx == edge->vtx[1];
3216         assert( ofs == 1 || start_vtx == edge->vtx[0] );
3217         if( edge->vtx[1] == end_vtx )
3218             break;
3219     }
3220
3221     if( !edge )
3222         EXIT;
3223
3224     next_edge = edge->next[ofs];
3225     if( prev_edge )
3226         prev_edge->next[prev_ofs] = next_edge;
3227     else
3228         start_vtx->first = next_edge;
3229
3230     for( ofs = prev_ofs = 0, prev_edge = 0, edge = end_vtx->first; edge != 0;
3231          prev_ofs = ofs, prev_edge = edge, edge = edge->next[ofs] )
3232     {
3233         ofs = end_vtx == edge->vtx[1];
3234         assert( ofs == 1 || end_vtx == edge->vtx[0] );
3235         if( edge->vtx[0] == start_vtx )
3236             break;
3237     }
3238
3239     assert( edge != 0 );
3240
3241     next_edge = edge->next[ofs];
3242     if( prev_edge )
3243         prev_edge->next[prev_ofs] = next_edge;
3244     else
3245         end_vtx->first = next_edge;
3246
3247     cvSetRemoveByPtr( graph->edges, edge );
3248
3249     __END__;
3250 }
3251
3252
3253 /* Removes the graph edge connecting two given vertices */
3254 CV_IMPL void
3255 cvGraphRemoveEdge( CvGraph* graph, int start_idx, int end_idx )
3256 {
3257     CvGraphVtx *start_vtx;
3258     CvGraphVtx *end_vtx;
3259
3260     CV_FUNCNAME( "cvGraphRemoveEdge" );
3261
3262     __BEGIN__;
3263
3264     if( !graph )
3265         CV_ERROR( CV_StsNullPtr, "" );
3266
3267     start_vtx = cvGetGraphVtx( graph, start_idx );
3268     end_vtx = cvGetGraphVtx( graph, end_idx );
3269
3270     cvGraphRemoveEdgeByPtr( graph, start_vtx, end_vtx );
3271
3272     __END__;
3273 }
3274
3275
3276 /* Counts number of edges incident to the vertex */
3277 CV_IMPL int
3278 cvGraphVtxDegreeByPtr( const CvGraph* graph, const CvGraphVtx* vertex )
3279 {
3280     CvGraphEdge *edge;
3281     int count = -1;
3282
3283     CV_FUNCNAME( "cvGraphVtxDegreeByPtr" );
3284
3285     __BEGIN__;
3286
3287     if( !graph || !vertex )
3288         CV_ERROR( CV_StsNullPtr, "" );
3289
3290     for( edge = vertex->first, count = 0; edge; )
3291     {
3292         count++;
3293         edge = CV_NEXT_GRAPH_EDGE( edge, vertex );
3294     }
3295
3296     __END__;
3297
3298     return count;
3299 }
3300
3301
3302 /* Counts number of edges incident to the vertex */
3303 CV_IMPL int
3304 cvGraphVtxDegree( const CvGraph* graph, int vtx_idx )
3305 {
3306     CvGraphVtx *vertex;
3307     CvGraphEdge *edge;
3308     int count = -1;
3309
3310     CV_FUNCNAME( "cvGraphVtxDegree" );
3311
3312     __BEGIN__;
3313
3314     if( !graph )
3315         CV_ERROR( CV_StsNullPtr, "" );
3316
3317     vertex = cvGetGraphVtx( graph, vtx_idx );
3318     if( !vertex )
3319         CV_ERROR( CV_StsObjectNotFound, "" );
3320
3321     for( edge = vertex->first, count = 0; edge; )
3322     {
3323         count++;
3324         edge = CV_NEXT_GRAPH_EDGE( edge, vertex );
3325     }
3326
3327     __END__;
3328
3329     return count;
3330 }
3331
3332
3333 typedef struct CvGraphItem
3334 {
3335     CvGraphVtx* vtx;
3336     CvGraphEdge* edge;
3337 }
3338 CvGraphItem;
3339
3340
3341 static  void
3342 icvSeqElemsClearFlags( CvSeq* seq, int offset, int clear_mask )
3343 {
3344     CV_FUNCNAME("icvStartScanGraph");
3345
3346     __BEGIN__;
3347
3348     CvSeqReader reader;
3349     int i, total, elem_size;
3350
3351     if( !seq )
3352         CV_ERROR( CV_StsNullPtr, "" );
3353
3354     elem_size = seq->elem_size;
3355     total = seq->total;
3356
3357     if( (unsigned)offset > (unsigned)elem_size )
3358         CV_ERROR( CV_StsBadArg, "" );
3359
3360     CV_CALL( cvStartReadSeq( seq, &reader ));
3361
3362     for( i = 0; i < total; i++ )
3363     {
3364         int* flag_ptr = (int*)(reader.ptr + offset);
3365         *flag_ptr &= ~clear_mask;
3366
3367         CV_NEXT_SEQ_ELEM( elem_size, reader );
3368     }
3369
3370     __END__;
3371 }
3372
3373
3374 static  char*
3375 icvSeqFindNextElem( CvSeq* seq, int offset, int mask,
3376                     int value, int* start_index )
3377 {
3378     char* elem_ptr = 0;
3379
3380     CV_FUNCNAME("icvStartScanGraph");
3381
3382     __BEGIN__;
3383
3384     CvSeqReader reader;
3385     int total, elem_size, index;
3386
3387     if( !seq || !start_index )
3388         CV_ERROR( CV_StsNullPtr, "" );
3389
3390     elem_size = seq->elem_size;
3391     total = seq->total;
3392     index = *start_index;
3393
3394     if( (unsigned)offset > (unsigned)elem_size )
3395         CV_ERROR( CV_StsBadArg, "" );
3396
3397     if( total == 0 )
3398         EXIT;
3399
3400     if( (unsigned)index >= (unsigned)total )
3401     {
3402         index %= total;
3403         index += index < 0 ? total : 0;
3404     }
3405
3406     CV_CALL( cvStartReadSeq( seq, &reader ));
3407
3408     if( index != 0 )
3409         CV_CALL( cvSetSeqReaderPos( &reader, index ));
3410
3411     for( index = 0; index < total; index++ )
3412     {
3413         int* flag_ptr = (int*)(reader.ptr + offset);
3414         if( (*flag_ptr & mask) == value )
3415             break;
3416
3417         CV_NEXT_SEQ_ELEM( elem_size, reader );
3418     }
3419
3420     if( index < total )
3421     {
3422         elem_ptr = reader.ptr;
3423         *start_index = index;
3424     }
3425
3426     __END__;
3427
3428     return  elem_ptr;
3429 }
3430
3431 #define CV_FIELD_OFFSET( field, structtype ) ((int)(size_t)&((structtype*)0)->field)
3432
3433 CV_IMPL CvGraphScanner*
3434 cvCreateGraphScanner( CvGraph* graph, CvGraphVtx* vtx, int mask )
3435 {
3436     CvGraphScanner* scanner = 0;
3437     CvMemStorage* child_storage = 0;
3438
3439     CV_FUNCNAME("cvCreateGraphScanner");
3440
3441     __BEGIN__;
3442
3443     if( !graph )
3444         CV_ERROR( CV_StsNullPtr, "Null graph pointer" );
3445
3446     CV_ASSERT( graph->storage != 0 );
3447
3448     CV_CALL( scanner = (CvGraphScanner*)cvAlloc( sizeof(*scanner) ));
3449     memset( scanner, 0, sizeof(*scanner));
3450
3451     scanner->graph = graph;
3452     scanner->mask = mask;
3453     scanner->vtx = vtx;
3454     scanner->index = vtx == 0 ? 0 : -1;
3455
3456     CV_CALL( child_storage = cvCreateChildMemStorage( graph->storage ));
3457
3458     CV_CALL( scanner->stack = cvCreateSeq( 0, sizeof(CvSet),
3459                        sizeof(CvGraphItem), child_storage ));
3460
3461     CV_CALL( icvSeqElemsClearFlags( (CvSeq*)graph,
3462                                     CV_FIELD_OFFSET( flags, CvGraphVtx),
3463                                     CV_GRAPH_ITEM_VISITED_FLAG|
3464                                     CV_GRAPH_SEARCH_TREE_NODE_FLAG ));
3465
3466     CV_CALL( icvSeqElemsClearFlags( (CvSeq*)(graph->edges),
3467                                     CV_FIELD_OFFSET( flags, CvGraphEdge),
3468                                     CV_GRAPH_ITEM_VISITED_FLAG ));
3469
3470     __END__;
3471
3472     if( cvGetErrStatus() < 0 )
3473     {
3474         cvReleaseMemStorage( &child_storage );
3475         cvFree( &scanner );
3476     }
3477
3478     return scanner;
3479 }
3480
3481
3482 CV_IMPL void
3483 cvReleaseGraphScanner( CvGraphScanner** scanner )
3484 {
3485     CV_FUNCNAME("cvReleaseGraphScanner");
3486
3487     __BEGIN__;
3488
3489     if( !scanner )
3490         CV_ERROR( CV_StsNullPtr, "Null double pointer to graph scanner" );
3491
3492     if( *scanner )
3493     {
3494         if( (*scanner)->stack )
3495             CV_CALL( cvReleaseMemStorage( &((*scanner)->stack->storage)));
3496         cvFree( scanner );
3497     }
3498
3499     __END__;
3500 }
3501
3502
3503 CV_IMPL int
3504 cvNextGraphItem( CvGraphScanner* scanner )
3505 {
3506     int code = -1;
3507
3508     CV_FUNCNAME("cvNextGraphItem");
3509
3510     __BEGIN__;
3511
3512     CvGraphVtx* vtx;
3513     CvGraphVtx* dst;
3514     CvGraphEdge* edge;
3515     CvGraphItem item;
3516
3517     if( !scanner || !(scanner->stack))
3518         CV_ERROR( CV_StsNullPtr, "Null graph scanner" );
3519
3520     dst = scanner->dst;
3521     vtx = scanner->vtx;
3522     edge = scanner->edge;
3523
3524     for(;;)
3525     {
3526         for(;;)
3527         {
3528             if( dst && !CV_IS_GRAPH_VERTEX_VISITED(dst) )
3529             {
3530                 scanner->vtx = vtx = dst;
3531                 edge = vtx->first;
3532                 dst->flags |= CV_GRAPH_ITEM_VISITED_FLAG;
3533
3534                 if((scanner->mask & CV_GRAPH_VERTEX))
3535                 {
3536                     scanner->vtx = vtx;
3537                     scanner->edge = vtx->first;
3538                     scanner->dst = 0;
3539                     code = CV_GRAPH_VERTEX;
3540                     EXIT;
3541                 }
3542             }
3543
3544             while( edge )
3545             {
3546                 dst = edge->vtx[vtx == edge->vtx[0]];
3547
3548                 if( !CV_IS_GRAPH_EDGE_VISITED(edge) )
3549                 {
3550                     // check that the edge is outcoming
3551                     if( !CV_IS_GRAPH_ORIENTED( scanner->graph ) || dst != edge->vtx[0] )
3552                     {
3553                         edge->flags |= CV_GRAPH_ITEM_VISITED_FLAG;
3554
3555                         if( !CV_IS_GRAPH_VERTEX_VISITED(dst) )
3556                         {
3557                             item.vtx = vtx;
3558                             item.edge = edge;
3559
3560                             vtx->flags |= CV_GRAPH_SEARCH_TREE_NODE_FLAG;
3561
3562                             cvSeqPush( scanner->stack, &item );
3563
3564                             if( scanner->mask & CV_GRAPH_TREE_EDGE )
3565                             {
3566                                 code = CV_GRAPH_TREE_EDGE;
3567                                 scanner->vtx = vtx;
3568                                 scanner->dst = dst;
3569                                 scanner->edge = edge;
3570                                 EXIT;
3571                             }
3572                             break;
3573                         }
3574                         else
3575                         {
3576                             if( scanner->mask & (CV_GRAPH_BACK_EDGE|
3577                                                  CV_GRAPH_CROSS_EDGE|
3578                                                  CV_GRAPH_FORWARD_EDGE) )
3579                             {
3580                                 code = (dst->flags & CV_GRAPH_SEARCH_TREE_NODE_FLAG) ?
3581                                        CV_GRAPH_BACK_EDGE :
3582                                        (edge->flags & CV_GRAPH_FORWARD_EDGE_FLAG) ?
3583                                        CV_GRAPH_FORWARD_EDGE : CV_GRAPH_CROSS_EDGE;
3584                                 edge->flags &= ~CV_GRAPH_FORWARD_EDGE_FLAG;
3585                                 if( scanner->mask & code )
3586                                 {
3587                                     scanner->vtx = vtx;
3588                                     scanner->dst = dst;
3589                                     scanner->edge = edge;
3590                                     EXIT;
3591                                 }
3592                             }
3593                         }
3594                     }
3595                     else if( (dst->flags & (CV_GRAPH_ITEM_VISITED_FLAG|
3596                              CV_GRAPH_SEARCH_TREE_NODE_FLAG)) ==
3597                              (CV_GRAPH_ITEM_VISITED_FLAG|
3598                              CV_GRAPH_SEARCH_TREE_NODE_FLAG))
3599                     {
3600                         edge->flags |= CV_GRAPH_FORWARD_EDGE_FLAG;
3601                     }
3602                 }
3603
3604                 edge = CV_NEXT_GRAPH_EDGE( edge, vtx );
3605             }
3606
3607             if( !edge ) /* need to backtrack */
3608             {
3609                 if( scanner->stack->total == 0 )
3610                 {
3611                     if( scanner->index >= 0 )
3612                         vtx = 0;
3613                     else
3614                         scanner->index = 0;
3615                     break;
3616                 }
3617                 cvSeqPop( scanner->stack, &item );
3618                 vtx = item.vtx;
3619                 vtx->flags &= ~CV_GRAPH_SEARCH_TREE_NODE_FLAG;
3620                 edge = item.edge;
3621                 dst = 0;
3622
3623                 if( scanner->mask & CV_GRAPH_BACKTRACKING )
3624                 {
3625                     scanner->vtx = vtx;
3626                     scanner->edge = edge;
3627                     scanner->dst = edge->vtx[vtx == edge->vtx[0]];
3628                     code = CV_GRAPH_BACKTRACKING;
3629                     EXIT;
3630                 }
3631             }
3632         }
3633
3634         if( !vtx )
3635         {
3636             vtx = (CvGraphVtx*)icvSeqFindNextElem( (CvSeq*)(scanner->graph),
3637                   CV_FIELD_OFFSET( flags, CvGraphVtx ), CV_GRAPH_ITEM_VISITED_FLAG|INT_MIN,
3638                   0, &(scanner->index) );
3639
3640             if( !vtx )
3641             {
3642                 code = CV_GRAPH_OVER;
3643                 break;
3644             }
3645         }
3646
3647         dst = vtx;
3648         if( scanner->mask & CV_GRAPH_NEW_TREE )
3649         {
3650             scanner->dst = dst;
3651             scanner->edge = 0;
3652             scanner->vtx = 0;
3653             code = CV_GRAPH_NEW_TREE;
3654             break;
3655         }
3656     }
3657
3658     __END__;
3659
3660     return code;
3661 }
3662
3663
3664 CV_IMPL CvGraph*
3665 cvCloneGraph( const CvGraph* graph, CvMemStorage* storage )
3666 {
3667     int* flag_buffer = 0;
3668     CvGraphVtx** ptr_buffer = 0;
3669     CvGraph* result = 0;
3670
3671     CV_FUNCNAME( "cvCloneGraph" );
3672
3673     __BEGIN__;
3674
3675     int i, k;
3676     int vtx_size, edge_size;
3677     CvSeqReader reader;
3678
3679     if( !CV_IS_GRAPH(graph))
3680         CV_ERROR( CV_StsBadArg, "Invalid graph pointer" );
3681
3682     if( !storage )
3683         storage = graph->storage;
3684
3685     if( !storage )
3686         CV_ERROR( CV_StsNullPtr, "NULL storage pointer" );
3687
3688     vtx_size = graph->elem_size;
3689     edge_size = graph->edges->elem_size;
3690
3691     CV_CALL( flag_buffer = (int*)cvAlloc( graph->total*sizeof(flag_buffer[0])));
3692     CV_CALL( ptr_buffer = (CvGraphVtx**)cvAlloc( graph->total*sizeof(ptr_buffer[0])));
3693     CV_CALL( result = cvCreateGraph( graph->flags, graph->header_size,
3694                                      vtx_size, edge_size, storage ));
3695     memcpy( result + sizeof(CvGraph), graph + sizeof(CvGraph),
3696             graph->header_size - sizeof(CvGraph));
3697
3698     // pass 1. save flags, copy vertices
3699     cvStartReadSeq( (CvSeq*)graph, &reader );
3700     for( i = 0, k = 0; i < graph->total; i++ )
3701     {
3702         if( CV_IS_SET_ELEM( reader.ptr ))
3703         {
3704             CvGraphVtx* vtx = (CvGraphVtx*)reader.ptr;
3705             CvGraphVtx* dstvtx = 0;
3706             CV_CALL( cvGraphAddVtx( result, vtx, &dstvtx ));
3707             flag_buffer[k] = dstvtx->flags = vtx->flags;
3708             vtx->flags = k;
3709             ptr_buffer[k++] = dstvtx;
3710         }
3711         CV_NEXT_SEQ_ELEM( vtx_size, reader );
3712     }
3713
3714     // pass 2. copy edges
3715     cvStartReadSeq( (CvSeq*)graph->edges, &reader );
3716     for( i = 0; i < graph->edges->total; i++ )
3717     {
3718         if( CV_IS_SET_ELEM( reader.ptr ))
3719         {
3720             CvGraphEdge* edge = (CvGraphEdge*)reader.ptr;
3721             CvGraphEdge* dstedge = 0;
3722             CvGraphVtx* new_org = ptr_buffer[edge->vtx[0]->flags];
3723             CvGraphVtx* new_dst = ptr_buffer[edge->vtx[1]->flags];
3724             CV_CALL( cvGraphAddEdgeByPtr( result, new_org, new_dst, edge, &dstedge ));
3725             dstedge->flags = edge->flags;
3726         }
3727         CV_NEXT_SEQ_ELEM( edge_size, reader );
3728     }
3729
3730     // pass 3. restore flags
3731     cvStartReadSeq( (CvSeq*)graph, &reader );
3732     for( i = 0, k = 0; i < graph->edges->total; i++ )
3733     {
3734         if( CV_IS_SET_ELEM( reader.ptr ))
3735         {
3736             CvGraphVtx* vtx = (CvGraphVtx*)reader.ptr;
3737             vtx->flags = flag_buffer[k++];
3738         }
3739         CV_NEXT_SEQ_ELEM( vtx_size, reader );
3740     }
3741
3742     __END__;
3743
3744     cvFree( &flag_buffer );
3745     cvFree( &ptr_buffer );
3746
3747     if( cvGetErrStatus() < 0 )
3748         result = 0;
3749
3750     return result;
3751 }
3752
3753
3754 /****************************************************************************************\
3755 *                                 Working with sequence tree                             *
3756 \****************************************************************************************/
3757
3758 // Gathers pointers to all the sequences, accessible from the <first>, to the single sequence.
3759 CV_IMPL CvSeq*
3760 cvTreeToNodeSeq( const void* first, int header_size, CvMemStorage* storage )
3761 {
3762     CvSeq* allseq = 0;
3763
3764     CV_FUNCNAME("cvTreeToNodeSeq");
3765
3766     __BEGIN__;
3767
3768     CvTreeNodeIterator iterator;
3769
3770     if( !storage )
3771         CV_ERROR( CV_StsNullPtr, "NULL storage pointer" );
3772
3773     CV_CALL( allseq = cvCreateSeq( 0, header_size, sizeof(first), storage ));
3774
3775     if( first )
3776     {
3777         CV_CALL( cvInitTreeNodeIterator( &iterator, first, INT_MAX ));
3778
3779         for(;;)
3780         {
3781             void* node = cvNextTreeNode( &iterator );
3782             if( !node )
3783                 break;
3784             cvSeqPush( allseq, &node );
3785         }
3786     }
3787
3788     __END__;
3789
3790     return allseq;
3791 }
3792
3793
3794 typedef struct CvTreeNode
3795 {
3796     int       flags;         /* micsellaneous flags */
3797     int       header_size;   /* size of sequence header */
3798     struct    CvTreeNode* h_prev; /* previous sequence */
3799     struct    CvTreeNode* h_next; /* next sequence */
3800     struct    CvTreeNode* v_prev; /* 2nd previous sequence */
3801     struct    CvTreeNode* v_next; /* 2nd next sequence */
3802 }
3803 CvTreeNode;
3804
3805
3806
3807 // Insert contour into tree given certain parent sequence.
3808 // If parent is equal to frame (the most external contour),
3809 // then added contour will have null pointer to parent.
3810 CV_IMPL void
3811 cvInsertNodeIntoTree( void* _node, void* _parent, void* _frame )
3812 {
3813     CV_FUNCNAME( "cvInsertNodeIntoTree" );
3814
3815     __BEGIN__;
3816
3817     CvTreeNode* node = (CvTreeNode*)_node;
3818     CvTreeNode* parent = (CvTreeNode*)_parent;
3819
3820     if( !node || !parent )
3821         CV_ERROR( CV_StsNullPtr, "" );
3822
3823     node->v_prev = _parent != _frame ? parent : 0;
3824     node->h_next = parent->v_next;
3825
3826     assert( parent->v_next != node );
3827
3828     if( parent->v_next )
3829         parent->v_next->h_prev = node;
3830     parent->v_next = node;
3831
3832     __END__;
3833 }
3834
3835
3836 // Removes contour from tree (together with the contour children).
3837 CV_IMPL void
3838 cvRemoveNodeFromTree( void* _node, void* _frame )
3839 {
3840     CV_FUNCNAME( "cvRemoveNodeFromTree" );
3841
3842     __BEGIN__;
3843
3844     CvTreeNode* node = (CvTreeNode*)_node;
3845     CvTreeNode* frame = (CvTreeNode*)_frame;
3846
3847     if( !node )
3848         CV_ERROR_FROM_CODE( CV_StsNullPtr );
3849
3850     if( node == frame )
3851         CV_ERROR( CV_StsBadArg, "frame node could not be deleted" );
3852
3853     if( node->h_next )
3854         node->h_next->h_prev = node->h_prev;
3855
3856     if( node->h_prev )
3857         node->h_prev->h_next = node->h_next;
3858     else
3859     {
3860         CvTreeNode* parent = node->v_prev;
3861         if( !parent )
3862             parent = frame;
3863
3864         if( parent )
3865         {
3866             assert( parent->v_next == node );
3867             parent->v_next = node->h_next;
3868         }
3869     }
3870
3871     __END__;
3872 }
3873
3874
3875 CV_IMPL void
3876 cvInitTreeNodeIterator( CvTreeNodeIterator* treeIterator,
3877                         const void* first, int max_level )
3878 {
3879     CV_FUNCNAME("icvInitTreeNodeIterator");
3880
3881     __BEGIN__;
3882
3883     if( !treeIterator || !first )
3884         CV_ERROR( CV_StsNullPtr, "" );
3885
3886     if( max_level < 0 )
3887         CV_ERROR( CV_StsOutOfRange, "" );
3888
3889     treeIterator->node = (void*)first;
3890     treeIterator->level = 0;
3891     treeIterator->max_level = max_level;
3892
3893     __END__;
3894 }
3895
3896
3897 CV_IMPL void*
3898 cvNextTreeNode( CvTreeNodeIterator* treeIterator )
3899 {
3900     CvTreeNode* prevNode = 0;
3901
3902     CV_FUNCNAME("cvNextTreeNode");
3903
3904     __BEGIN__;
3905
3906     CvTreeNode* node;
3907     int level;
3908
3909     if( !treeIterator )
3910         CV_ERROR( CV_StsNullPtr, "NULL iterator pointer" );
3911
3912     prevNode = node = (CvTreeNode*)treeIterator->node;
3913     level = treeIterator->level;
3914
3915     if( node )
3916     {
3917         if( node->v_next && level+1 < treeIterator->max_level )
3918         {
3919             node = node->v_next;
3920             level++;
3921         }
3922         else
3923         {
3924             while( node->h_next == 0 )
3925             {
3926                 node = node->v_prev;
3927                 if( --level < 0 )
3928                 {
3929                     node = 0;
3930                     break;
3931                 }
3932             }
3933             node = node && treeIterator->max_level != 0 ? node->h_next : 0;
3934         }
3935     }
3936
3937     treeIterator->node = node;
3938     treeIterator->level = level;
3939
3940     __END__;
3941
3942     return prevNode;
3943 }
3944
3945
3946 CV_IMPL void*
3947 cvPrevTreeNode( CvTreeNodeIterator* treeIterator )
3948 {
3949     CvTreeNode* prevNode = 0;
3950
3951     CV_FUNCNAME("cvPrevTreeNode");
3952
3953     __BEGIN__;
3954
3955     CvTreeNode* node;
3956     int level;
3957
3958     if( !treeIterator )
3959         CV_ERROR( CV_StsNullPtr, "" );
3960
3961     prevNode = node = (CvTreeNode*)treeIterator->node;
3962     level = treeIterator->level;
3963
3964     if( node )
3965     {
3966         if( !node->h_prev )
3967         {
3968             node = node->v_prev;
3969             if( --level < 0 )
3970                 node = 0;
3971         }
3972         else
3973         {
3974             node = node->h_prev;
3975
3976             while( node->v_next && level < treeIterator->max_level )
3977             {
3978                 node = node->v_next;
3979                 level++;
3980
3981                 while( node->h_next )
3982                     node = node->h_next;
3983             }
3984         }
3985     }
3986
3987     treeIterator->node = node;
3988     treeIterator->level = level;
3989
3990     __END__;
3991
3992     return prevNode;
3993 }
3994
3995 /* End of file. */