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