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