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