imagecore 0.0.1

data/ext/opencv/core/datastructs.cpp

@@ -0,0 +1,4064 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                        Intel License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000, Intel Corporation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of Intel Corporation may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+#include "precomp.hpp"
+
+#define ICV_FREE_PTR(storage)  \
+    ((schar*)(storage)->top + (storage)->block_size - (storage)->free_space)
+
+#define ICV_ALIGNED_SEQ_BLOCK_SIZE  \
+    (int)cvAlign(sizeof(CvSeqBlock), CV_STRUCT_ALIGN)
+
+CV_INLINE int
+cvAlignLeft( int size, int align )
+{
+    return size & -align;
+}
+
+#define CV_GET_LAST_ELEM( seq, block ) \
+    ((block)->data + ((block)->count - 1)*((seq)->elem_size))
+
+#define CV_SWAP_ELEMS(a,b,elem_size)  \
+{                                     \
+    int k;                            \
+    for( k = 0; k < elem_size; k++ )  \
+    {                                 \
+        char t0 = (a)[k];             \
+        char t1 = (b)[k];             \
+        (a)[k] = t1;                  \
+        (b)[k] = t0;                  \
+    }                                 \
+}
+
+#define ICV_SHIFT_TAB_MAX 32
+static const schar icvPower2ShiftTab[] =
+{
+    0, 1, -1, 2, -1, -1, -1, 3, -1, -1, -1, -1, -1, -1, -1, 4,
+    -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 5
+};
+
+/****************************************************************************************\
+*            Functions for manipulating memory storage - list of memory blocks           *
+\****************************************************************************************/
+
+/* Initialize allocated storage: */
+static void
+icvInitMemStorage( CvMemStorage* storage, int block_size )
+{
+    if( !storage )
+        CV_Error( CV_StsNullPtr, "" );
+
+    if( block_size <= 0 )
+        block_size = CV_STORAGE_BLOCK_SIZE;
+
+    block_size = cvAlign( block_size, CV_STRUCT_ALIGN );
+    assert( sizeof(CvMemBlock) % CV_STRUCT_ALIGN == 0 );
+
+    memset( storage, 0, sizeof( *storage ));
+    storage->signature = CV_STORAGE_MAGIC_VAL;
+    storage->block_size = block_size;
+}
+
+
+/* Create root memory storage: */
+CV_IMPL CvMemStorage*
+cvCreateMemStorage( int block_size )
+{
+    CvMemStorage* storage = (CvMemStorage *)cvAlloc( sizeof( CvMemStorage ));
+    icvInitMemStorage( storage, block_size );
+    return storage;
+}
+
+
+/* Create child memory storage: */
+CV_IMPL CvMemStorage *
+cvCreateChildMemStorage( CvMemStorage * parent )
+{
+    if( !parent )
+        CV_Error( CV_StsNullPtr, "" );
+
+    CvMemStorage* storage = cvCreateMemStorage(parent->block_size);
+    storage->parent = parent;
+
+    return storage;
+}
+
+
+/* Release all blocks of the storage (or return them to parent, if any): */
+static void
+icvDestroyMemStorage( CvMemStorage* storage )
+{
+    int k = 0;
+
+    CvMemBlock *block;
+    CvMemBlock *dst_top = 0;
+
+    if( !storage )
+        CV_Error( CV_StsNullPtr, "" );
+
+    if( storage->parent )
+        dst_top = storage->parent->top;
+
+    for( block = storage->bottom; block != 0; k++ )
+    {
+        CvMemBlock *temp = block;
+
+        block = block->next;
+        if( storage->parent )
+        {
+            if( dst_top )
+            {
+                temp->prev = dst_top;
+                temp->next = dst_top->next;
+                if( temp->next )
+                    temp->next->prev = temp;
+                dst_top = dst_top->next = temp;
+            }
+            else
+            {
+                dst_top = storage->parent->bottom = storage->parent->top = temp;
+                temp->prev = temp->next = 0;
+                storage->free_space = storage->block_size - sizeof( *temp );
+            }
+        }
+        else
+        {
+            cvFree( &temp );
+        }
+    }
+
+    storage->top = storage->bottom = 0;
+    storage->free_space = 0;
+}
+
+
+/* Release memory storage: */
+CV_IMPL void
+cvReleaseMemStorage( CvMemStorage** storage )
+{
+    if( !storage )
+        CV_Error( CV_StsNullPtr, "" );
+
+    CvMemStorage* st = *storage;
+    *storage = 0;
+    if( st )
+    {
+        icvDestroyMemStorage( st );
+        cvFree( &st );
+    }
+}
+
+
+/* Clears memory storage (return blocks to the parent, if any): */
+CV_IMPL void
+cvClearMemStorage( CvMemStorage * storage )
+{
+    if( !storage )
+        CV_Error( CV_StsNullPtr, "" );
+
+    if( storage->parent )
+        icvDestroyMemStorage( storage );
+    else
+    {
+        storage->top = storage->bottom;
+        storage->free_space = storage->bottom ? storage->block_size - sizeof(CvMemBlock) : 0;
+    }
+}
+
+
+/* Moves stack pointer to next block.
+   If no blocks, allocate new one and link it to the storage: */
+static void
+icvGoNextMemBlock( CvMemStorage * storage )
+{
+    if( !storage )
+        CV_Error( CV_StsNullPtr, "" );
+
+    if( !storage->top || !storage->top->next )
+    {
+        CvMemBlock *block;
+
+        if( !(storage->parent) )
+        {
+            block = (CvMemBlock *)cvAlloc( storage->block_size );
+        }
+        else
+        {
+            CvMemStorage *parent = storage->parent;
+            CvMemStoragePos parent_pos;
+
+            cvSaveMemStoragePos( parent, &parent_pos );
+            icvGoNextMemBlock( parent );
+
+            block = parent->top;
+            cvRestoreMemStoragePos( parent, &parent_pos );
+
+            if( block == parent->top )  /* the single allocated block */
+            {
+                assert( parent->bottom == block );
+                parent->top = parent->bottom = 0;
+                parent->free_space = 0;
+            }
+            else
+            {
+                /* cut the block from the parent's list of blocks */
+                parent->top->next = block->next;
+                if( block->next )
+                    block->next->prev = parent->top;
+            }
+        }
+
+        /* link block */
+        block->next = 0;
+        block->prev = storage->top;
+
+        if( storage->top )
+            storage->top->next = block;
+        else
+            storage->top = storage->bottom = block;
+    }
+
+    if( storage->top->next )
+        storage->top = storage->top->next;
+    storage->free_space = storage->block_size - sizeof(CvMemBlock);
+    assert( storage->free_space % CV_STRUCT_ALIGN == 0 );
+}
+
+
+/* Remember memory storage position: */
+CV_IMPL void
+cvSaveMemStoragePos( const CvMemStorage * storage, CvMemStoragePos * pos )
+{
+    if( !storage || !pos )
+        CV_Error( CV_StsNullPtr, "" );
+
+    pos->top = storage->top;
+    pos->free_space = storage->free_space;
+}
+
+
+/* Restore memory storage position: */
+CV_IMPL void
+cvRestoreMemStoragePos( CvMemStorage * storage, CvMemStoragePos * pos )
+{
+    if( !storage || !pos )
+        CV_Error( CV_StsNullPtr, "" );
+    if( pos->free_space > storage->block_size )
+        CV_Error( CV_StsBadSize, "" );
+
+    /*
+    // this breaks icvGoNextMemBlock, so comment it off for now
+    if( storage->parent && (!pos->top || pos->top->next) )
+    {
+        CvMemBlock* save_bottom;
+        if( !pos->top )
+            save_bottom = 0;
+        else
+        {
+            save_bottom = storage->bottom;
+            storage->bottom = pos->top->next;
+            pos->top->next = 0;
+            storage->bottom->prev = 0;
+        }
+        icvDestroyMemStorage( storage );
+        storage->bottom = save_bottom;
+    }*/
+
+    storage->top = pos->top;
+    storage->free_space = pos->free_space;
+
+    if( !storage->top )
+    {
+        storage->top = storage->bottom;
+        storage->free_space = storage->top ? storage->block_size - sizeof(CvMemBlock) : 0;
+    }
+}
+
+
+/* Allocate continuous buffer of the specified size in the storage: */
+CV_IMPL void*
+cvMemStorageAlloc( CvMemStorage* storage, size_t size )
+{
+    schar *ptr = 0;
+    if( !storage )
+        CV_Error( CV_StsNullPtr, "NULL storage pointer" );
+
+    if( size > INT_MAX )
+        CV_Error( CV_StsOutOfRange, "Too large memory block is requested" );
+
+    assert( storage->free_space % CV_STRUCT_ALIGN == 0 );
+
+    if( (size_t)storage->free_space < size )
+    {
+        size_t max_free_space = cvAlignLeft(storage->block_size - sizeof(CvMemBlock), CV_STRUCT_ALIGN);
+        if( max_free_space < size )
+            CV_Error( CV_StsOutOfRange, "requested size is negative or too big" );
+
+        icvGoNextMemBlock( storage );
+    }
+
+    ptr = ICV_FREE_PTR(storage);
+    assert( (size_t)ptr % CV_STRUCT_ALIGN == 0 );
+    storage->free_space = cvAlignLeft(storage->free_space - (int)size, CV_STRUCT_ALIGN );
+
+    return ptr;
+}
+
+
+CV_IMPL CvString
+cvMemStorageAllocString( CvMemStorage* storage, const char* ptr, int len )
+{
+    CvString str;
+
+    str.len = len >= 0 ? len : (int)strlen(ptr);
+    str.ptr = (char*)cvMemStorageAlloc( storage, str.len + 1 );
+    memcpy( str.ptr, ptr, str.len );
+    str.ptr[str.len] = '\0';
+
+    return str;
+}
+
+
+/****************************************************************************************\
+*                               Sequence implementation                                  *
+\****************************************************************************************/
+
+/* Create empty sequence: */
+CV_IMPL CvSeq *
+cvCreateSeq( int seq_flags, int header_size, int elem_size, CvMemStorage * storage )
+{
+    CvSeq *seq = 0;
+
+    if( !storage )
+        CV_Error( CV_StsNullPtr, "" );
+    if( header_size < (int)sizeof( CvSeq ) || elem_size <= 0 )
+        CV_Error( CV_StsBadSize, "" );
+
+    /* allocate sequence header */
+    seq = (CvSeq*)cvMemStorageAlloc( storage, header_size );
+    memset( seq, 0, header_size );
+
+    seq->header_size = header_size;
+    seq->flags = (seq_flags & ~CV_MAGIC_MASK) | CV_SEQ_MAGIC_VAL;
+    {
+        int elemtype = CV_MAT_TYPE(seq_flags);
+        int typesize = CV_ELEM_SIZE(elemtype);
+
+        if( elemtype != CV_SEQ_ELTYPE_GENERIC && elemtype != CV_USRTYPE1 &&
+            typesize != 0 && typesize != elem_size )
+            CV_Error( CV_StsBadSize,
+            "Specified element size doesn't match to the size of the specified element type "
+            "(try to use 0 for element type)" );
+    }
+    seq->elem_size = elem_size;
+    seq->storage = storage;
+
+    cvSetSeqBlockSize( seq, (1 << 10)/elem_size );
+
+    return seq;
+}
+
+
+/* adjusts <delta_elems> field of sequence. It determines how much the sequence
+   grows if there are no free space inside the sequence buffers */
+CV_IMPL void
+cvSetSeqBlockSize( CvSeq *seq, int delta_elements )
+{
+    int elem_size;
+    int useful_block_size;
+
+    if( !seq || !seq->storage )
+        CV_Error( CV_StsNullPtr, "" );
+    if( delta_elements < 0 )
+        CV_Error( CV_StsOutOfRange, "" );
+
+    useful_block_size = cvAlignLeft(seq->storage->block_size - sizeof(CvMemBlock) -
+                                    sizeof(CvSeqBlock), CV_STRUCT_ALIGN);
+    elem_size = seq->elem_size;
+
+    if( delta_elements == 0 )
+    {
+        delta_elements = (1 << 10) / elem_size;
+        delta_elements = MAX( delta_elements, 1 );
+    }
+    if( delta_elements * elem_size > useful_block_size )
+    {
+        delta_elements = useful_block_size / elem_size;
+        if( delta_elements == 0 )
+            CV_Error( CV_StsOutOfRange, "Storage block size is too small "
+                                        "to fit the sequence elements" );
+    }
+
+    seq->delta_elems = delta_elements;
+}
+
+
+/* Find a sequence element by its index: */
+CV_IMPL schar*
+cvGetSeqElem( const CvSeq *seq, int index )
+{
+    CvSeqBlock *block;
+    int count, total = seq->total;
+
+    if( (unsigned)index >= (unsigned)total )
+    {
+        index += index < 0 ? total : 0;
+        index -= index >= total ? total : 0;
+        if( (unsigned)index >= (unsigned)total )
+            return 0;
+    }
+
+    block = seq->first;
+    if( index + index <= total )
+    {
+        while( index >= (count = block->count) )
+        {
+            block = block->next;
+            index -= count;
+        }
+    }
+    else
+    {
+        do
+        {
+            block = block->prev;
+            total -= block->count;
+        }
+        while( index < total );
+        index -= total;
+    }
+
+    return block->data + index * seq->elem_size;
+}
+
+
+/* Calculate index of a sequence element: */
+CV_IMPL int
+cvSeqElemIdx( const CvSeq* seq, const void* _element, CvSeqBlock** _block )
+{
+    const schar *element = (const schar *)_element;
+    int elem_size;
+    int id = -1;
+    CvSeqBlock *first_block;
+    CvSeqBlock *block;
+
+    if( !seq || !element )
+        CV_Error( CV_StsNullPtr, "" );
+
+    block = first_block = seq->first;
+    elem_size = seq->elem_size;
+
+    for( ;; )
+    {
+        if( (unsigned)(element - block->data) < (unsigned) (block->count * elem_size) )
+        {
+            if( _block )
+                *_block = block;
+            if( elem_size <= ICV_SHIFT_TAB_MAX && (id = icvPower2ShiftTab[elem_size - 1]) >= 0 )
+                id = (int)((size_t)(element - block->data) >> id);
+            else
+                id = (int)((size_t)(element - block->data) / elem_size);
+            id += block->start_index - seq->first->start_index;
+            break;
+        }
+        block = block->next;
+        if( block == first_block )
+            break;
+    }
+
+    return id;
+}
+
+
+CV_IMPL int
+cvSliceLength( CvSlice slice, const CvSeq* seq )
+{
+    int total = seq->total;
+    int length = slice.end_index - slice.start_index;
+
+    if( length != 0 )
+    {
+        if( slice.start_index < 0 )
+            slice.start_index += total;
+        if( slice.end_index <= 0 )
+            slice.end_index += total;
+
+        length = slice.end_index - slice.start_index;
+    }
+
+    if( length < 0 )
+    {
+        length += total;
+        /*if( length < 0 )
+            length += total;*/
+    }
+    else if( length > total )
+        length = total;
+
+    return length;
+}
+
+
+/* Copy all sequence elements into single continuous array: */
+CV_IMPL void*
+cvCvtSeqToArray( const CvSeq *seq, void *array, CvSlice slice )
+{
+    int elem_size, total;
+    CvSeqReader reader;
+    char *dst = (char*)array;
+
+    if( !seq || !array )
+        CV_Error( CV_StsNullPtr, "" );
+
+    elem_size = seq->elem_size;
+    total = cvSliceLength( slice, seq )*elem_size;
+
+    if( total == 0 )
+        return 0;
+
+    cvStartReadSeq( seq, &reader, 0 );
+    cvSetSeqReaderPos( &reader, slice.start_index, 0 );
+
+    do
+    {
+        int count = (int)(reader.block_max - reader.ptr);
+        if( count > total )
+            count = total;
+
+        memcpy( dst, reader.ptr, count );
+        dst += count;
+        reader.block = reader.block->next;
+        reader.ptr = reader.block->data;
+        reader.block_max = reader.ptr + reader.block->count*elem_size;
+        total -= count;
+    }
+    while( total > 0 );
+
+    return array;
+}
+
+
+/* Construct a sequence from an array without copying any data.
+   NB: The resultant sequence cannot grow beyond its initial size: */
+CV_IMPL CvSeq*
+cvMakeSeqHeaderForArray( int seq_flags, int header_size, int elem_size,
+                         void *array, int total, CvSeq *seq, CvSeqBlock * block )
+{
+    CvSeq* result = 0;
+
+    if( elem_size <= 0 || header_size < (int)sizeof( CvSeq ) || total < 0 )
+        CV_Error( CV_StsBadSize, "" );
+
+    if( !seq || ((!array || !block) && total > 0) )
+        CV_Error( CV_StsNullPtr, "" );
+
+    memset( seq, 0, header_size );
+
+    seq->header_size = header_size;
+    seq->flags = (seq_flags & ~CV_MAGIC_MASK) | CV_SEQ_MAGIC_VAL;
+    {
+        int elemtype = CV_MAT_TYPE(seq_flags);
+        int typesize = CV_ELEM_SIZE(elemtype);
+
+        if( elemtype != CV_SEQ_ELTYPE_GENERIC &&
+            typesize != 0 && typesize != elem_size )
+            CV_Error( CV_StsBadSize,
+            "Element size doesn't match to the size of predefined element type "
+            "(try to use 0 for sequence element type)" );
+    }
+    seq->elem_size = elem_size;
+    seq->total = total;
+    seq->block_max = seq->ptr = (schar *) array + total * elem_size;
+
+    if( total > 0 )
+    {
+        seq->first = block;
+        block->prev = block->next = block;
+        block->start_index = 0;
+        block->count = total;
+        block->data = (schar *) array;
+    }
+
+    result = seq;
+
+    return result;
+}
+
+
+/* The function allocates space for at least one more sequence element.
+   If there are free sequence blocks (seq->free_blocks != 0)
+   they are reused, otherwise the space is allocated in the storage: */
+static void
+icvGrowSeq( CvSeq *seq, int in_front_of )
+{
+    CvSeqBlock *block;
+
+    if( !seq )
+        CV_Error( CV_StsNullPtr, "" );
+    block = seq->free_blocks;
+
+    if( !block )
+    {
+        int elem_size = seq->elem_size;
+        int delta_elems = seq->delta_elems;
+        CvMemStorage *storage = seq->storage;
+
+        if( seq->total >= delta_elems*4 )
+            cvSetSeqBlockSize( seq, delta_elems*2 );
+
+        if( !storage )
+            CV_Error( CV_StsNullPtr, "The sequence has NULL storage pointer" );
+
+        /* If there is a free space just after last allocated block
+           and it is big enough then enlarge the last block.
+           This can happen only if the new block is added to the end of sequence: */
+        if( (unsigned)(ICV_FREE_PTR(storage) - seq->block_max) < CV_STRUCT_ALIGN &&
+            storage->free_space >= seq->elem_size && !in_front_of )
+        {
+            int delta = storage->free_space / elem_size;
+
+            delta = MIN( delta, delta_elems ) * elem_size;
+            seq->block_max += delta;
+            storage->free_space = cvAlignLeft((int)(((schar*)storage->top + storage->block_size) -
+                                              seq->block_max), CV_STRUCT_ALIGN );
+            return;
+        }
+        else
+        {
+            int delta = elem_size * delta_elems + ICV_ALIGNED_SEQ_BLOCK_SIZE;
+
+            /* Try to allocate <delta_elements> elements: */
+            if( storage->free_space < delta )
+            {
+                int small_block_size = MAX(1, delta_elems/3)*elem_size +
+                                       ICV_ALIGNED_SEQ_BLOCK_SIZE;
+                /* try to allocate smaller part */
+                if( storage->free_space >= small_block_size + CV_STRUCT_ALIGN )
+                {
+                    delta = (storage->free_space - ICV_ALIGNED_SEQ_BLOCK_SIZE)/seq->elem_size;
+                    delta = delta*seq->elem_size + ICV_ALIGNED_SEQ_BLOCK_SIZE;
+                }
+                else
+                {
+                    icvGoNextMemBlock( storage );
+                    assert( storage->free_space >= delta );
+                }
+            }
+
+            block = (CvSeqBlock*)cvMemStorageAlloc( storage, delta );
+            block->data = (schar*)cvAlignPtr( block + 1, CV_STRUCT_ALIGN );
+            block->count = delta - ICV_ALIGNED_SEQ_BLOCK_SIZE;
+            block->prev = block->next = 0;
+        }
+    }
+    else
+    {
+        seq->free_blocks = block->next;
+    }
+
+    if( !(seq->first) )
+    {
+        seq->first = block;
+        block->prev = block->next = block;
+    }
+    else
+    {
+        block->prev = seq->first->prev;
+        block->next = seq->first;
+        block->prev->next = block->next->prev = block;
+    }
+
+    /* For free blocks the <count> field means
+     * total number of bytes in the block.
+     *
+     * For used blocks it means current number
+     * of sequence elements in the block:
+     */
+    assert( block->count % seq->elem_size == 0 && block->count > 0 );
+
+    if( !in_front_of )
+    {
+        seq->ptr = block->data;
+        seq->block_max = block->data + block->count;
+        block->start_index = block == block->prev ? 0 :
+            block->prev->start_index + block->prev->count;
+    }
+    else
+    {
+        int delta = block->count / seq->elem_size;
+        block->data += block->count;
+
+        if( block != block->prev )
+        {
+            assert( seq->first->start_index == 0 );
+            seq->first = block;
+        }
+        else
+        {
+            seq->block_max = seq->ptr = block->data;
+        }
+
+        block->start_index = 0;
+
+        for( ;; )
+        {
+            block->start_index += delta;
+            block = block->next;
+            if( block == seq->first )
+                break;
+        }
+    }
+
+    block->count = 0;
+}
+
+/* Recycle a sequence block: */
+static void
+icvFreeSeqBlock( CvSeq *seq, int in_front_of )
+{
+    CvSeqBlock *block = seq->first;
+
+    assert( (in_front_of ? block : block->prev)->count == 0 );
+
+    if( block == block->prev )  /* single block case */
+    {
+        block->count = (int)(seq->block_max - block->data) + block->start_index * seq->elem_size;
+        block->data = seq->block_max - block->count;
+        seq->first = 0;
+        seq->ptr = seq->block_max = 0;
+        seq->total = 0;
+    }
+    else
+    {
+        if( !in_front_of )
+        {
+            block = block->prev;
+            assert( seq->ptr == block->data );
+
+            block->count = (int)(seq->block_max - seq->ptr);
+            seq->block_max = seq->ptr = block->prev->data +
+                block->prev->count * seq->elem_size;
+        }
+        else
+        {
+            int delta = block->start_index;
+
+            block->count = delta * seq->elem_size;
+            block->data -= block->count;
+
+            /* Update start indices of sequence blocks: */
+            for( ;; )
+            {
+                block->start_index -= delta;
+                block = block->next;
+                if( block == seq->first )
+                    break;
+            }
+
+            seq->first = block->next;
+        }
+
+        block->prev->next = block->next;
+        block->next->prev = block->prev;
+    }
+
+    assert( block->count > 0 && block->count % seq->elem_size == 0 );
+    block->next = seq->free_blocks;
+    seq->free_blocks = block;
+}
+
+
+/****************************************************************************************\
+*                             Sequence Writer implementation                             *
+\****************************************************************************************/
+
+/* Initialize sequence writer: */
+CV_IMPL void
+cvStartAppendToSeq( CvSeq *seq, CvSeqWriter * writer )
+{
+    if( !seq || !writer )
+        CV_Error( CV_StsNullPtr, "" );
+
+    memset( writer, 0, sizeof( *writer ));
+    writer->header_size = sizeof( CvSeqWriter );
+
+    writer->seq = seq;
+    writer->block = seq->first ? seq->first->prev : 0;
+    writer->ptr = seq->ptr;
+    writer->block_max = seq->block_max;
+}
+
+
+/* Initialize sequence writer: */
+CV_IMPL void
+cvStartWriteSeq( int seq_flags, int header_size,
+                 int elem_size, CvMemStorage * storage, CvSeqWriter * writer )
+{
+    if( !storage || !writer )
+        CV_Error( CV_StsNullPtr, "" );
+
+    CvSeq* seq = cvCreateSeq( seq_flags, header_size, elem_size, storage );
+    cvStartAppendToSeq( seq, writer );
+}
+
+
+/* Update sequence header: */
+CV_IMPL void
+cvFlushSeqWriter( CvSeqWriter * writer )
+{
+    if( !writer )
+        CV_Error( CV_StsNullPtr, "" );
+
+    CvSeq* seq = writer->seq;
+    seq->ptr = writer->ptr;
+
+    if( writer->block )
+    {
+        int total = 0;
+        CvSeqBlock *first_block = writer->seq->first;
+        CvSeqBlock *block = first_block;
+
+        writer->block->count = (int)((writer->ptr - writer->block->data) / seq->elem_size);
+        assert( writer->block->count > 0 );
+
+        do
+        {
+            total += block->count;
+            block = block->next;
+        }
+        while( block != first_block );
+
+        writer->seq->total = total;
+    }
+}
+
+
+/* Calls icvFlushSeqWriter and finishes writing process: */
+CV_IMPL CvSeq *
+cvEndWriteSeq( CvSeqWriter * writer )
+{
+    if( !writer )
+        CV_Error( CV_StsNullPtr, "" );
+
+    cvFlushSeqWriter( writer );
+    CvSeq* seq = writer->seq;
+
+    /* Truncate the last block: */
+    if( writer->block && writer->seq->storage )
+    {
+        CvMemStorage *storage = seq->storage;
+        schar *storage_block_max = (schar *) storage->top + storage->block_size;
+
+        assert( writer->block->count > 0 );
+
+        if( (unsigned)((storage_block_max - storage->free_space)
+            - seq->block_max) < CV_STRUCT_ALIGN )
+        {
+            storage->free_space = cvAlignLeft((int)(storage_block_max - seq->ptr), CV_STRUCT_ALIGN);
+            seq->block_max = seq->ptr;
+        }
+    }
+
+    writer->ptr = 0;
+    return seq;
+}
+
+
+/* Create new sequence block: */
+CV_IMPL void
+cvCreateSeqBlock( CvSeqWriter * writer )
+{
+    if( !writer || !writer->seq )
+        CV_Error( CV_StsNullPtr, "" );
+
+    CvSeq* seq = writer->seq;
+
+    cvFlushSeqWriter( writer );
+
+    icvGrowSeq( seq, 0 );
+
+    writer->block = seq->first->prev;
+    writer->ptr = seq->ptr;
+    writer->block_max = seq->block_max;
+}
+
+
+/****************************************************************************************\
+*                               Sequence Reader implementation                           *
+\****************************************************************************************/
+
+/* Initialize sequence reader: */
+CV_IMPL void
+cvStartReadSeq( const CvSeq *seq, CvSeqReader * reader, int reverse )
+{
+    CvSeqBlock *first_block;
+    CvSeqBlock *last_block;
+
+    if( reader )
+    {
+        reader->seq = 0;
+        reader->block = 0;
+        reader->ptr = reader->block_max = reader->block_min = 0;
+    }
+
+    if( !seq || !reader )
+        CV_Error( CV_StsNullPtr, "" );
+
+    reader->header_size = sizeof( CvSeqReader );
+    reader->seq = (CvSeq*)seq;
+
+    first_block = seq->first;
+
+    if( first_block )
+    {
+        last_block = first_block->prev;
+        reader->ptr = first_block->data;
+        reader->prev_elem = CV_GET_LAST_ELEM( seq, last_block );
+        reader->delta_index = seq->first->start_index;
+
+        if( reverse )
+        {
+            schar *temp = reader->ptr;
+
+            reader->ptr = reader->prev_elem;
+            reader->prev_elem = temp;
+
+            reader->block = last_block;
+        }
+        else
+        {
+            reader->block = first_block;
+        }
+
+        reader->block_min = reader->block->data;
+        reader->block_max = reader->block_min + reader->block->count * seq->elem_size;
+    }
+    else
+    {
+        reader->delta_index = 0;
+        reader->block = 0;
+
+        reader->ptr = reader->prev_elem = reader->block_min = reader->block_max = 0;
+    }
+}
+
+
+/* Change the current reading block
+ * to the previous or to the next:
+ */
+CV_IMPL void
+cvChangeSeqBlock( void* _reader, int direction )
+{
+    CvSeqReader* reader = (CvSeqReader*)_reader;
+
+    if( !reader )
+        CV_Error( CV_StsNullPtr, "" );
+
+    if( direction > 0 )
+    {
+        reader->block = reader->block->next;
+        reader->ptr = reader->block->data;
+    }
+    else
+    {
+        reader->block = reader->block->prev;
+        reader->ptr = CV_GET_LAST_ELEM( reader->seq, reader->block );
+    }
+    reader->block_min = reader->block->data;
+    reader->block_max = reader->block_min + reader->block->count * reader->seq->elem_size;
+}
+
+
+/* Return the current reader position: */
+CV_IMPL int
+cvGetSeqReaderPos( CvSeqReader* reader )
+{
+    int elem_size;
+    int index = -1;
+
+    if( !reader || !reader->ptr )
+        CV_Error( CV_StsNullPtr, "" );
+
+    elem_size = reader->seq->elem_size;
+    if( elem_size <= ICV_SHIFT_TAB_MAX && (index = icvPower2ShiftTab[elem_size - 1]) >= 0 )
+        index = (int)((reader->ptr - reader->block_min) >> index);
+    else
+        index = (int)((reader->ptr - reader->block_min) / elem_size);
+
+    index += reader->block->start_index - reader->delta_index;
+
+    return index;
+}
+
+
+/* Set reader position to given position,
+ * either absolute or relative to the
+ *  current one:
+ */
+CV_IMPL void
+cvSetSeqReaderPos( CvSeqReader* reader, int index, int is_relative )
+{
+    CvSeqBlock *block;
+    int elem_size, count, total;
+
+    if( !reader || !reader->seq )
+        CV_Error( CV_StsNullPtr, "" );
+
+    total = reader->seq->total;
+    elem_size = reader->seq->elem_size;
+
+    if( !is_relative )
+    {
+        if( index < 0 )
+        {
+            if( index < -total )
+                CV_Error( CV_StsOutOfRange, "" );
+            index += total;
+        }
+        else if( index >= total )
+        {
+            index -= total;
+            if( index >= total )
+                CV_Error( CV_StsOutOfRange, "" );
+        }
+
+        block = reader->seq->first;
+        if( index >= (count = block->count) )
+        {
+            if( index + index <= total )
+            {
+                do
+                {
+                    block = block->next;
+                    index -= count;
+                }
+                while( index >= (count = block->count) );
+            }
+            else
+            {
+                do
+                {
+                    block = block->prev;
+                    total -= block->count;
+                }
+                while( index < total );
+                index -= total;
+            }
+        }
+        reader->ptr = block->data + index * elem_size;
+        if( reader->block != block )
+        {
+            reader->block = block;
+            reader->block_min = block->data;
+            reader->block_max = block->data + block->count * elem_size;
+        }
+    }
+    else
+    {
+        schar* ptr = reader->ptr;
+        index *= elem_size;
+        block = reader->block;
+
+        if( index > 0 )
+        {
+            while( ptr + index >= reader->block_max )
+            {
+                int delta = (int)(reader->block_max - ptr);
+                index -= delta;
+                reader->block = block = block->next;
+                reader->block_min = ptr = block->data;
+                reader->block_max = block->data + block->count*elem_size;
+            }
+            reader->ptr = ptr + index;
+        }
+        else
+        {
+            while( ptr + index < reader->block_min )
+            {
+                int delta = (int)(ptr - reader->block_min);
+                index += delta;
+                reader->block = block = block->prev;
+                reader->block_min = block->data;
+                reader->block_max = ptr = block->data + block->count*elem_size;
+            }
+            reader->ptr = ptr + index;
+        }
+    }
+}
+
+
+/* Push element onto the sequence: */
+CV_IMPL schar*
+cvSeqPush( CvSeq *seq, const void *element )
+{
+    schar *ptr = 0;
+    size_t elem_size;
+
+    if( !seq )
+        CV_Error( CV_StsNullPtr, "" );
+
+    elem_size = seq->elem_size;
+    ptr = seq->ptr;
+
+    if( ptr >= seq->block_max )
+    {
+        icvGrowSeq( seq, 0 );
+
+        ptr = seq->ptr;
+        assert( ptr + elem_size <= seq->block_max /*&& ptr == seq->block_min */  );
+    }
+
+    if( element )
+        memcpy( ptr, element, elem_size );
+    seq->first->prev->count++;
+    seq->total++;
+    seq->ptr = ptr + elem_size;
+
+    return ptr;
+}
+
+
+/* Pop last element off of the sequence: */
+CV_IMPL void
+cvSeqPop( CvSeq *seq, void *element )
+{
+    schar *ptr;
+    int elem_size;
+
+    if( !seq )
+        CV_Error( CV_StsNullPtr, "" );
+    if( seq->total <= 0 )
+        CV_Error( CV_StsBadSize, "" );
+
+    elem_size = seq->elem_size;
+    seq->ptr = ptr = seq->ptr - elem_size;
+
+    if( element )
+        memcpy( element, ptr, elem_size );
+    seq->ptr = ptr;
+    seq->total--;
+
+    if( --(seq->first->prev->count) == 0 )
+    {
+        icvFreeSeqBlock( seq, 0 );
+        assert( seq->ptr == seq->block_max );
+    }
+}
+
+
+/* Push element onto the front of the sequence: */
+CV_IMPL schar*
+cvSeqPushFront( CvSeq *seq, const void *element )
+{
+    schar* ptr = 0;
+    int elem_size;
+    CvSeqBlock *block;
+
+    if( !seq )
+        CV_Error( CV_StsNullPtr, "" );
+
+    elem_size = seq->elem_size;
+    block = seq->first;
+
+    if( !block || block->start_index == 0 )
+    {
+        icvGrowSeq( seq, 1 );
+
+        block = seq->first;
+        assert( block->start_index > 0 );
+    }
+
+    ptr = block->data -= elem_size;
+
+    if( element )
+        memcpy( ptr, element, elem_size );
+    block->count++;
+    block->start_index--;
+    seq->total++;
+
+    return ptr;
+}
+
+
+/* Shift out first element of the sequence: */
+CV_IMPL void
+cvSeqPopFront( CvSeq *seq, void *element )
+{
+    int elem_size;
+    CvSeqBlock *block;
+
+    if( !seq )
+        CV_Error( CV_StsNullPtr, "" );
+    if( seq->total <= 0 )
+        CV_Error( CV_StsBadSize, "" );
+
+    elem_size = seq->elem_size;
+    block = seq->first;
+
+    if( element )
+        memcpy( element, block->data, elem_size );
+    block->data += elem_size;
+    block->start_index++;
+    seq->total--;
+
+    if( --(block->count) == 0 )
+        icvFreeSeqBlock( seq, 1 );
+}
+
+/* Insert new element in middle of sequence: */
+CV_IMPL schar*
+cvSeqInsert( CvSeq *seq, int before_index, const void *element )
+{
+    int elem_size;
+    int block_size;
+    CvSeqBlock *block;
+    int delta_index;
+    int total;
+    schar* ret_ptr = 0;
+
+    if( !seq )
+        CV_Error( CV_StsNullPtr, "" );
+
+    total = seq->total;
+    before_index += before_index < 0 ? total : 0;
+    before_index -= before_index > total ? total : 0;
+
+    if( (unsigned)before_index > (unsigned)total )
+        CV_Error( CV_StsOutOfRange, "" );
+
+    if( before_index == total )
+    {
+        ret_ptr = cvSeqPush( seq, element );
+    }
+    else if( before_index == 0 )
+    {
+        ret_ptr = cvSeqPushFront( seq, element );
+    }
+    else
+    {
+        elem_size = seq->elem_size;
+
+        if( before_index >= total >> 1 )
+        {
+            schar *ptr = seq->ptr + elem_size;
+
+            if( ptr > seq->block_max )
+            {
+                icvGrowSeq( seq, 0 );
+
+                ptr = seq->ptr + elem_size;
+                assert( ptr <= seq->block_max );
+            }
+
+            delta_index = seq->first->start_index;
+            block = seq->first->prev;
+            block->count++;
+            block_size = (int)(ptr - block->data);
+
+            while( before_index < block->start_index - delta_index )
+            {
+                CvSeqBlock *prev_block = block->prev;
+
+                memmove( block->data + elem_size, block->data, block_size - elem_size );
+                block_size = prev_block->count * elem_size;
+                memcpy( block->data, prev_block->data + block_size - elem_size, elem_size );
+                block = prev_block;
+
+                /* Check that we don't fall into an infinite loop: */
+                assert( block != seq->first->prev );
+            }
+
+            before_index = (before_index - block->start_index + delta_index) * elem_size;
+            memmove( block->data + before_index + elem_size, block->data + before_index,
+                     block_size - before_index - elem_size );
+
+            ret_ptr = block->data + before_index;
+
+            if( element )
+                memcpy( ret_ptr, element, elem_size );
+            seq->ptr = ptr;
+        }
+        else
+        {
+            block = seq->first;
+
+            if( block->start_index == 0 )
+            {
+                icvGrowSeq( seq, 1 );
+
+                block = seq->first;
+            }
+
+            delta_index = block->start_index;
+            block->count++;
+            block->start_index--;
+            block->data -= elem_size;
+
+            while( before_index > block->start_index - delta_index + block->count )
+            {
+                CvSeqBlock *next_block = block->next;
+
+                block_size = block->count * elem_size;
+                memmove( block->data, block->data + elem_size, block_size - elem_size );
+                memcpy( block->data + block_size - elem_size, next_block->data, elem_size );
+                block = next_block;
+
+                /* Check that we don't fall into an infinite loop: */
+                assert( block != seq->first );
+            }
+
+            before_index = (before_index - block->start_index + delta_index) * elem_size;
+            memmove( block->data, block->data + elem_size, before_index - elem_size );
+
+            ret_ptr = block->data + before_index - elem_size;
+
+            if( element )
+                memcpy( ret_ptr, element, elem_size );
+        }
+
+        seq->total = total + 1;
+    }
+
+    return ret_ptr;
+}
+
+
+/* Removes element from sequence: */
+CV_IMPL void
+cvSeqRemove( CvSeq *seq, int index )
+{
+    schar *ptr;
+    int elem_size;
+    int block_size;
+    CvSeqBlock *block;
+    int delta_index;
+    int total, front = 0;
+
+    if( !seq )
+        CV_Error( CV_StsNullPtr, "" );
+
+    total = seq->total;
+
+    index += index < 0 ? total : 0;
+    index -= index >= total ? total : 0;
+
+    if( (unsigned) index >= (unsigned) total )
+        CV_Error( CV_StsOutOfRange, "Invalid index" );
+
+    if( index == total - 1 )
+    {
+        cvSeqPop( seq, 0 );
+    }
+    else if( index == 0 )
+    {
+        cvSeqPopFront( seq, 0 );
+    }
+    else
+    {
+        block = seq->first;
+        elem_size = seq->elem_size;
+        delta_index = block->start_index;
+        while( block->start_index - delta_index + block->count <= index )
+            block = block->next;
+
+        ptr = block->data + (index - block->start_index + delta_index) * elem_size;
+
+        front = index < total >> 1;
+        if( !front )
+        {
+            block_size = block->count * elem_size - (int)(ptr - block->data);
+
+            while( block != seq->first->prev )  /* while not the last block */
+            {
+                CvSeqBlock *next_block = block->next;
+
+                memmove( ptr, ptr + elem_size, block_size - elem_size );
+                memcpy( ptr + block_size - elem_size, next_block->data, elem_size );
+                block = next_block;
+                ptr = block->data;
+                block_size = block->count * elem_size;
+            }
+
+            memmove( ptr, ptr + elem_size, block_size - elem_size );
+            seq->ptr -= elem_size;
+        }
+        else
+        {
+            ptr += elem_size;
+            block_size = (int)(ptr - block->data);
+
+            while( block != seq->first )
+            {
+                CvSeqBlock *prev_block = block->prev;
+
+                memmove( block->data + elem_size, block->data, block_size - elem_size );
+                block_size = prev_block->count * elem_size;
+                memcpy( block->data, prev_block->data + block_size - elem_size, elem_size );
+                block = prev_block;
+            }
+
+            memmove( block->data + elem_size, block->data, block_size - elem_size );
+            block->data += elem_size;
+            block->start_index++;
+        }
+
+        seq->total = total - 1;
+        if( --block->count == 0 )
+            icvFreeSeqBlock( seq, front );
+    }
+}
+
+
+/* Add several elements to the beginning or end of a sequence: */
+CV_IMPL void
+cvSeqPushMulti( CvSeq *seq, const void *_elements, int count, int front )
+{
+    char *elements = (char *) _elements;
+
+    if( !seq )
+        CV_Error( CV_StsNullPtr, "NULL sequence pointer" );
+    if( count < 0 )
+        CV_Error( CV_StsBadSize, "number of removed elements is negative" );
+
+    int elem_size = seq->elem_size;
+
+    if( !front )
+    {
+        while( count > 0 )
+        {
+            int delta = (int)((seq->block_max - seq->ptr) / elem_size);
+
+            delta = MIN( delta, count );
+            if( delta > 0 )
+            {
+                seq->first->prev->count += delta;
+                seq->total += delta;
+                count -= delta;
+                delta *= elem_size;
+                if( elements )
+                {
+                    memcpy( seq->ptr, elements, delta );
+                    elements += delta;
+                }
+                seq->ptr += delta;
+            }
+
+            if( count > 0 )
+                icvGrowSeq( seq, 0 );
+        }
+    }
+    else
+    {
+        CvSeqBlock* block = seq->first;
+
+        while( count > 0 )
+        {
+            int delta;
+
+            if( !block || block->start_index == 0 )
+            {
+                icvGrowSeq( seq, 1 );
+
+                block = seq->first;
+                assert( block->start_index > 0 );
+            }
+
+            delta = MIN( block->start_index, count );
+            count -= delta;
+            block->start_index -= delta;
+            block->count += delta;
+            seq->total += delta;
+            delta *= elem_size;
+            block->data -= delta;
+
+            if( elements )
+                memcpy( block->data, elements + count*elem_size, delta );
+        }
+    }
+}
+
+
+/* Remove several elements from the end of sequence: */
+CV_IMPL void
+cvSeqPopMulti( CvSeq *seq, void *_elements, int count, int front )
+{
+    char *elements = (char *) _elements;
+
+    if( !seq )
+        CV_Error( CV_StsNullPtr, "NULL sequence pointer" );
+    if( count < 0 )
+        CV_Error( CV_StsBadSize, "number of removed elements is negative" );
+
+    count = MIN( count, seq->total );
+
+    if( !front )
+    {
+        if( elements )
+            elements += count * seq->elem_size;
+
+        while( count > 0 )
+        {
+            int delta = seq->first->prev->count;
+
+            delta = MIN( delta, count );
+            assert( delta > 0 );
+
+            seq->first->prev->count -= delta;
+            seq->total -= delta;
+            count -= delta;
+            delta *= seq->elem_size;
+            seq->ptr -= delta;
+
+            if( elements )
+            {
+                elements -= delta;
+                memcpy( elements, seq->ptr, delta );
+            }
+
+            if( seq->first->prev->count == 0 )
+                icvFreeSeqBlock( seq, 0 );
+        }
+    }
+    else
+    {
+        while( count > 0 )
+        {
+            int delta = seq->first->count;
+
+            delta = MIN( delta, count );
+            assert( delta > 0 );
+
+            seq->first->count -= delta;
+            seq->total -= delta;
+            count -= delta;
+            seq->first->start_index += delta;
+            delta *= seq->elem_size;
+
+            if( elements )
+            {
+                memcpy( elements, seq->first->data, delta );
+                elements += delta;
+            }
+
+            seq->first->data += delta;
+            if( seq->first->count == 0 )
+                icvFreeSeqBlock( seq, 1 );
+        }
+    }
+}
+
+
+/* Remove all elements from a sequence: */
+CV_IMPL void
+cvClearSeq( CvSeq *seq )
+{
+    if( !seq )
+        CV_Error( CV_StsNullPtr, "" );
+    cvSeqPopMulti( seq, 0, seq->total );
+}
+
+
+CV_IMPL CvSeq*
+cvSeqSlice( const CvSeq* seq, CvSlice slice, CvMemStorage* storage, int copy_data )
+{
+    CvSeq* subseq = 0;
+    int elem_size, count, length;
+    CvSeqReader reader;
+    CvSeqBlock *block, *first_block = 0, *last_block = 0;
+
+    if( !CV_IS_SEQ(seq) )
+        CV_Error( CV_StsBadArg, "Invalid sequence header" );
+
+    if( !storage )
+    {
+        storage = seq->storage;
+        if( !storage )
+            CV_Error( CV_StsNullPtr, "NULL storage pointer" );
+    }
+
+    elem_size = seq->elem_size;
+    length = cvSliceLength( slice, seq );
+    if( slice.start_index < 0 )
+        slice.start_index += seq->total;
+    else if( slice.start_index >= seq->total )
+        slice.start_index -= seq->total;
+    if( (unsigned)length > (unsigned)seq->total ||
+        ((unsigned)slice.start_index >= (unsigned)seq->total && length != 0) )
+        CV_Error( CV_StsOutOfRange, "Bad sequence slice" );
+
+    subseq = cvCreateSeq( seq->flags, seq->header_size, elem_size, storage );
+
+    if( length > 0 )
+    {
+        cvStartReadSeq( seq, &reader, 0 );
+        cvSetSeqReaderPos( &reader, slice.start_index, 0 );
+        count = (int)((reader.block_max - reader.ptr)/elem_size);
+
+        do
+        {
+            int bl = MIN( count, length );
+
+            if( !copy_data )
+            {
+                block = (CvSeqBlock*)cvMemStorageAlloc( storage, sizeof(*block) );
+                if( !first_block )
+                {
+                    first_block = subseq->first = block->prev = block->next = block;
+                    block->start_index = 0;
+                }
+                else
+                {
+                    block->prev = last_block;
+                    block->next = first_block;
+                    last_block->next = first_block->prev = block;
+                    block->start_index = last_block->start_index + last_block->count;
+                }
+                last_block = block;
+                block->data = reader.ptr;
+                block->count = bl;
+                subseq->total += bl;
+            }
+            else
+                cvSeqPushMulti( subseq, reader.ptr, bl, 0 );
+            length -= bl;
+            reader.block = reader.block->next;
+            reader.ptr = reader.block->data;
+            count = reader.block->count;
+        }
+        while( length > 0 );
+    }
+
+    return subseq;
+}
+
+
+// Remove slice from the middle of the sequence.
+// !!! TODO !!! Implement more efficient algorithm
+CV_IMPL void
+cvSeqRemoveSlice( CvSeq* seq, CvSlice slice )
+{
+    int total, length;
+
+    if( !CV_IS_SEQ(seq) )
+        CV_Error( CV_StsBadArg, "Invalid sequence header" );
+
+    length = cvSliceLength( slice, seq );
+    total = seq->total;
+
+    if( slice.start_index < 0 )
+        slice.start_index += total;
+    else if( slice.start_index >= total )
+        slice.start_index -= total;
+
+    if( (unsigned)slice.start_index >= (unsigned)total )
+        CV_Error( CV_StsOutOfRange, "start slice index is out of range" );
+
+    slice.end_index = slice.start_index + length;
+
+    if( slice.end_index < total )
+    {
+        CvSeqReader reader_to, reader_from;
+        int elem_size = seq->elem_size;
+
+        cvStartReadSeq( seq, &reader_to );
+        cvStartReadSeq( seq, &reader_from );
+
+        if( slice.start_index > total - slice.end_index )
+        {
+            int i, count = seq->total - slice.end_index;
+            cvSetSeqReaderPos( &reader_to, slice.start_index );
+            cvSetSeqReaderPos( &reader_from, slice.end_index );
+
+            for( i = 0; i < count; i++ )
+            {
+                memcpy( reader_to.ptr, reader_from.ptr, elem_size );
+                CV_NEXT_SEQ_ELEM( elem_size, reader_to );
+                CV_NEXT_SEQ_ELEM( elem_size, reader_from );
+            }
+
+            cvSeqPopMulti( seq, 0, slice.end_index - slice.start_index );
+        }
+        else
+        {
+            int i, count = slice.start_index;
+            cvSetSeqReaderPos( &reader_to, slice.end_index );
+            cvSetSeqReaderPos( &reader_from, slice.start_index );
+
+            for( i = 0; i < count; i++ )
+            {
+                CV_PREV_SEQ_ELEM( elem_size, reader_to );
+                CV_PREV_SEQ_ELEM( elem_size, reader_from );
+
+                memcpy( reader_to.ptr, reader_from.ptr, elem_size );
+            }
+
+            cvSeqPopMulti( seq, 0, slice.end_index - slice.start_index, 1 );
+        }
+    }
+    else
+    {
+        cvSeqPopMulti( seq, 0, total - slice.start_index );
+        cvSeqPopMulti( seq, 0, slice.end_index - total, 1 );
+    }
+}
+
+
+// Insert a sequence into the middle of another sequence:
+// !!! TODO !!! Implement more efficient algorithm
+CV_IMPL void
+cvSeqInsertSlice( CvSeq* seq, int index, const CvArr* from_arr )
+{
+    CvSeqReader reader_to, reader_from;
+    int i, elem_size, total, from_total;
+    CvSeq from_header, *from = (CvSeq*)from_arr;
+    CvSeqBlock block;
+
+    if( !CV_IS_SEQ(seq) )
+        CV_Error( CV_StsBadArg, "Invalid destination sequence header" );
+
+    if( !CV_IS_SEQ(from))
+    {
+        CvMat* mat = (CvMat*)from;
+        if( !CV_IS_MAT(mat))
+            CV_Error( CV_StsBadArg, "Source is not a sequence nor matrix" );
+
+        if( !CV_IS_MAT_CONT(mat->type) || (mat->rows != 1 && mat->cols != 1) )
+            CV_Error( CV_StsBadArg, "The source array must be 1d coninuous vector" );
+
+        from = cvMakeSeqHeaderForArray( CV_SEQ_KIND_GENERIC, sizeof(from_header),
+                                                 CV_ELEM_SIZE(mat->type),
+                                                 mat->data.ptr, mat->cols + mat->rows - 1,
+                                                 &from_header, &block );
+    }
+
+    if( seq->elem_size != from->elem_size )
+        CV_Error( CV_StsUnmatchedSizes,
+        "Source and destination sequence element sizes are different." );
+
+    from_total = from->total;
+
+    if( from_total == 0 )
+        return;
+
+    total = seq->total;
+    index += index < 0 ? total : 0;
+    index -= index > total ? total : 0;
+
+    if( (unsigned)index > (unsigned)total )
+        CV_Error( CV_StsOutOfRange, "" );
+
+    elem_size = seq->elem_size;
+
+    if( index < (total >> 1) )
+    {
+        cvSeqPushMulti( seq, 0, from_total, 1 );
+
+        cvStartReadSeq( seq, &reader_to );
+        cvStartReadSeq( seq, &reader_from );
+        cvSetSeqReaderPos( &reader_from, from_total );
+
+        for( i = 0; i < index; i++ )
+        {
+            memcpy( reader_to.ptr, reader_from.ptr, elem_size );
+            CV_NEXT_SEQ_ELEM( elem_size, reader_to );
+            CV_NEXT_SEQ_ELEM( elem_size, reader_from );
+        }
+    }
+    else
+    {
+        cvSeqPushMulti( seq, 0, from_total );
+
+        cvStartReadSeq( seq, &reader_to );
+        cvStartReadSeq( seq, &reader_from );
+        cvSetSeqReaderPos( &reader_from, total );
+        cvSetSeqReaderPos( &reader_to, seq->total );
+
+        for( i = 0; i < total - index; i++ )
+        {
+            CV_PREV_SEQ_ELEM( elem_size, reader_to );
+            CV_PREV_SEQ_ELEM( elem_size, reader_from );
+            memcpy( reader_to.ptr, reader_from.ptr, elem_size );
+        }
+    }
+
+    cvStartReadSeq( from, &reader_from );
+    cvSetSeqReaderPos( &reader_to, index );
+
+    for( i = 0; i < from_total; i++ )
+    {
+        memcpy( reader_to.ptr, reader_from.ptr, elem_size );
+        CV_NEXT_SEQ_ELEM( elem_size, reader_to );
+        CV_NEXT_SEQ_ELEM( elem_size, reader_from );
+    }
+}
+
+// Sort the sequence using user-specified comparison function.
+// The semantics is similar to qsort() function.
+// The code is based on BSD system qsort():
+//    * Copyright (c) 1992, 1993
+//    *  The Regents of the University of California.  All rights reserved.
+//    *
+//    * Redistribution and use in source and binary forms, with or without
+//    * modification, are permitted provided that the following conditions
+//    * are met:
+//    * 1. Redistributions of source code must retain the above copyright
+//    *    notice, this list of conditions and the following disclaimer.
+//    * 2. Redistributions in binary form must reproduce the above copyright
+//    *    notice, this list of conditions and the following disclaimer in the
+//    *    documentation and/or other materials provided with the distribution.
+//    * 3. All advertising materials mentioning features or use of this software
+//    *    must display the following acknowledgement:
+//    *  This product includes software developed by the University of
+//    *  California, Berkeley and its contributors.
+//    * 4. Neither the name of the University nor the names of its contributors
+//    *    may be used to endorse or promote products derived from this software
+//    *    without specific prior written permission.
+//    *
+//    * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+//    * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+//    * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+//    * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+//    * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+//    * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+//    * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+//    * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+//    * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+//    * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+//    * SUCH DAMAGE.
+
+typedef struct CvSeqReaderPos
+{
+    CvSeqBlock* block;
+    schar* ptr;
+    schar* block_min;
+    schar* block_max;
+}
+CvSeqReaderPos;
+
+#define CV_SAVE_READER_POS( reader, pos )   \
+{                                           \
+    (pos).block = (reader).block;           \
+    (pos).ptr = (reader).ptr;               \
+    (pos).block_min = (reader).block_min;   \
+    (pos).block_max = (reader).block_max;   \
+}
+
+#define CV_RESTORE_READER_POS( reader, pos )\
+{                                           \
+    (reader).block = (pos).block;           \
+    (reader).ptr = (pos).ptr;               \
+    (reader).block_min = (pos).block_min;   \
+    (reader).block_max = (pos).block_max;   \
+}
+
+inline schar*
+icvMed3( schar* a, schar* b, schar* c, CvCmpFunc cmp_func, void* aux )
+{
+    return cmp_func(a, b, aux) < 0 ?
+      (cmp_func(b, c, aux) < 0 ? b : cmp_func(a, c, aux) < 0 ? c : a)
+     :(cmp_func(b, c, aux) > 0 ? b : cmp_func(a, c, aux) < 0 ? a : c);
+}
+
+CV_IMPL void
+cvSeqSort( CvSeq* seq, CvCmpFunc cmp_func, void* aux )
+{
+    int elem_size;
+    int isort_thresh = 7;
+    CvSeqReader left, right;
+    int sp = 0;
+
+    struct
+    {
+        CvSeqReaderPos lb;
+        CvSeqReaderPos ub;
+    }
+    stack[48];
+
+    if( !CV_IS_SEQ(seq) )
+        CV_Error( !seq ? CV_StsNullPtr : CV_StsBadArg, "Bad input sequence" );
+
+    if( !cmp_func )
+        CV_Error( CV_StsNullPtr, "Null compare function" );
+
+    if( seq->total <= 1 )
+        return;
+
+    elem_size = seq->elem_size;
+    isort_thresh *= elem_size;
+
+    cvStartReadSeq( seq, &left, 0 );
+    right = left;
+    CV_SAVE_READER_POS( left, stack[0].lb );
+    CV_PREV_SEQ_ELEM( elem_size, right );
+    CV_SAVE_READER_POS( right, stack[0].ub );
+
+    while( sp >= 0 )
+    {
+        CV_RESTORE_READER_POS( left, stack[sp].lb );
+        CV_RESTORE_READER_POS( right, stack[sp].ub );
+        sp--;
+
+        for(;;)
+        {
+            int i, n, m;
+            CvSeqReader ptr, ptr2;
+
+            if( left.block == right.block )
+                n = (int)(right.ptr - left.ptr) + elem_size;
+            else
+            {
+                n = cvGetSeqReaderPos( &right );
+                n = (n - cvGetSeqReaderPos( &left ) + 1)*elem_size;
+            }
+
+            if( n <= isort_thresh )
+            {
+            insert_sort:
+                ptr = ptr2 = left;
+                CV_NEXT_SEQ_ELEM( elem_size, ptr );
+                CV_NEXT_SEQ_ELEM( elem_size, right );
+                while( ptr.ptr != right.ptr )
+                {
+                    ptr2.ptr = ptr.ptr;
+                    if( ptr2.block != ptr.block )
+                    {
+                        ptr2.block = ptr.block;
+                        ptr2.block_min = ptr.block_min;
+                        ptr2.block_max = ptr.block_max;
+                    }
+                    while( ptr2.ptr != left.ptr )
+                    {
+                        schar* cur = ptr2.ptr;
+                        CV_PREV_SEQ_ELEM( elem_size, ptr2 );
+                        if( cmp_func( ptr2.ptr, cur, aux ) <= 0 )
+                            break;
+                        CV_SWAP_ELEMS( ptr2.ptr, cur, elem_size );
+                    }
+                    CV_NEXT_SEQ_ELEM( elem_size, ptr );
+                }
+                break;
+            }
+            else
+            {
+                CvSeqReader left0, left1, right0, right1;
+                CvSeqReader tmp0, tmp1;
+                schar *m1, *m2, *m3, *pivot;
+                int swap_cnt = 0;
+                int l, l0, l1, r, r0, r1;
+
+                left0 = tmp0 = left;
+                right0 = right1 = right;
+                n /= elem_size;
+
+                if( n > 40 )
+                {
+                    int d = n / 8;
+                    schar *p1, *p2, *p3;
+                    p1 = tmp0.ptr;
+                    cvSetSeqReaderPos( &tmp0, d, 1 );
+                    p2 = tmp0.ptr;
+                    cvSetSeqReaderPos( &tmp0, d, 1 );
+                    p3 = tmp0.ptr;
+                    m1 = icvMed3( p1, p2, p3, cmp_func, aux );
+                    cvSetSeqReaderPos( &tmp0, (n/2) - d*3, 1 );
+                    p1 = tmp0.ptr;
+                    cvSetSeqReaderPos( &tmp0, d, 1 );
+                    p2 = tmp0.ptr;
+                    cvSetSeqReaderPos( &tmp0, d, 1 );
+                    p3 = tmp0.ptr;
+                    m2 = icvMed3( p1, p2, p3, cmp_func, aux );
+                    cvSetSeqReaderPos( &tmp0, n - 1 - d*3 - n/2, 1 );
+                    p1 = tmp0.ptr;
+                    cvSetSeqReaderPos( &tmp0, d, 1 );
+                    p2 = tmp0.ptr;
+                    cvSetSeqReaderPos( &tmp0, d, 1 );
+                    p3 = tmp0.ptr;
+                    m3 = icvMed3( p1, p2, p3, cmp_func, aux );
+                }
+                else
+                {
+                    m1 = tmp0.ptr;
+                    cvSetSeqReaderPos( &tmp0, n/2, 1 );
+                    m2 = tmp0.ptr;
+                    cvSetSeqReaderPos( &tmp0, n - 1 - n/2, 1 );
+                    m3 = tmp0.ptr;
+                }
+
+                pivot = icvMed3( m1, m2, m3, cmp_func, aux );
+                left = left0;
+                if( pivot != left.ptr )
+                {
+                    CV_SWAP_ELEMS( pivot, left.ptr, elem_size );
+                    pivot = left.ptr;
+                }
+                CV_NEXT_SEQ_ELEM( elem_size, left );
+                left1 = left;
+
+                for(;;)
+                {
+                    while( left.ptr != right.ptr && (r = cmp_func(left.ptr, pivot, aux)) <= 0 )
+                    {
+                        if( r == 0 )
+                        {
+                            if( left1.ptr != left.ptr )
+                                CV_SWAP_ELEMS( left1.ptr, left.ptr, elem_size );
+                            swap_cnt = 1;
+                            CV_NEXT_SEQ_ELEM( elem_size, left1 );
+                        }
+                        CV_NEXT_SEQ_ELEM( elem_size, left );
+                    }
+
+                    while( left.ptr != right.ptr && (r = cmp_func(right.ptr,pivot, aux)) >= 0 )
+                    {
+                        if( r == 0 )
+                        {
+                            if( right1.ptr != right.ptr )
+                                CV_SWAP_ELEMS( right1.ptr, right.ptr, elem_size );
+                            swap_cnt = 1;
+                            CV_PREV_SEQ_ELEM( elem_size, right1 );
+                        }
+                        CV_PREV_SEQ_ELEM( elem_size, right );
+                    }
+
+                    if( left.ptr == right.ptr )
+                    {
+                        r = cmp_func(left.ptr, pivot, aux);
+                        if( r == 0 )
+                        {
+                            if( left1.ptr != left.ptr )
+                                CV_SWAP_ELEMS( left1.ptr, left.ptr, elem_size );
+                            swap_cnt = 1;
+                            CV_NEXT_SEQ_ELEM( elem_size, left1 );
+                        }
+                        if( r <= 0 )
+                        {
+                            CV_NEXT_SEQ_ELEM( elem_size, left );
+                        }
+                        else
+                        {
+                            CV_PREV_SEQ_ELEM( elem_size, right );
+                        }
+                        break;
+                    }
+
+                    CV_SWAP_ELEMS( left.ptr, right.ptr, elem_size );
+                    CV_NEXT_SEQ_ELEM( elem_size, left );
+                    r = left.ptr == right.ptr;
+                    CV_PREV_SEQ_ELEM( elem_size, right );
+                    swap_cnt = 1;
+                    if( r )
+                        break;
+                }
+
+                if( swap_cnt == 0 )
+                {
+                    left = left0, right = right0;
+                    goto insert_sort;
+                }
+
+                l = cvGetSeqReaderPos( &left );
+                if( l == 0 )
+                    l = seq->total;
+                l0 = cvGetSeqReaderPos( &left0 );
+                l1 = cvGetSeqReaderPos( &left1 );
+                if( l1 == 0 )
+                    l1 = seq->total;
+
+                n = MIN( l - l1, l1 - l0 );
+                if( n > 0 )
+                {
+                    tmp0 = left0;
+                    tmp1 = left;
+                    cvSetSeqReaderPos( &tmp1, 0-n, 1 );
+                    for( i = 0; i < n; i++ )
+                    {
+                        CV_SWAP_ELEMS( tmp0.ptr, tmp1.ptr, elem_size );
+                        CV_NEXT_SEQ_ELEM( elem_size, tmp0 );
+                        CV_NEXT_SEQ_ELEM( elem_size, tmp1 );
+                    }
+                }
+
+                r = cvGetSeqReaderPos( &right );
+                r0 = cvGetSeqReaderPos( &right0 );
+                r1 = cvGetSeqReaderPos( &right1 );
+                m = MIN( r0 - r1, r1 - r );
+                if( m > 0 )
+                {
+                    tmp0 = left;
+                    tmp1 = right0;
+                    cvSetSeqReaderPos( &tmp1, 1-m, 1 );
+                    for( i = 0; i < m; i++ )
+                    {
+                        CV_SWAP_ELEMS( tmp0.ptr, tmp1.ptr, elem_size );
+                        CV_NEXT_SEQ_ELEM( elem_size, tmp0 );
+                        CV_NEXT_SEQ_ELEM( elem_size, tmp1 );
+                    }
+                }
+
+                n = l - l1;
+                m = r1 - r;
+                if( n > 1 )
+                {
+                    if( m > 1 )
+                    {
+                        if( n > m )
+                        {
+                            sp++;
+                            CV_SAVE_READER_POS( left0, stack[sp].lb );
+                            cvSetSeqReaderPos( &left0, n - 1, 1 );
+                            CV_SAVE_READER_POS( left0, stack[sp].ub );
+                            left = right = right0;
+                            cvSetSeqReaderPos( &left, 1 - m, 1 );
+                        }
+                        else
+                        {
+                            sp++;
+                            CV_SAVE_READER_POS( right0, stack[sp].ub );
+                            cvSetSeqReaderPos( &right0, 1 - m, 1 );
+                            CV_SAVE_READER_POS( right0, stack[sp].lb );
+                            left = right = left0;
+                            cvSetSeqReaderPos( &right, n - 1, 1 );
+                        }
+                    }
+                    else
+                    {
+                        left = right = left0;
+                        cvSetSeqReaderPos( &right, n - 1, 1 );
+                    }
+                }
+                else if( m > 1 )
+                {
+                    left = right = right0;
+                    cvSetSeqReaderPos( &left, 1 - m, 1 );
+                }
+                else
+                    break;
+            }
+        }
+    }
+}
+
+
+CV_IMPL schar*
+cvSeqSearch( CvSeq* seq, const void* _elem, CvCmpFunc cmp_func,
+             int is_sorted, int* _idx, void* userdata )
+{
+    schar* result = 0;
+    const schar* elem = (const schar*)_elem;
+    int idx = -1;
+    int i, j;
+
+    if( _idx )
+        *_idx = idx;
+
+    if( !CV_IS_SEQ(seq) )
+        CV_Error( !seq ? CV_StsNullPtr : CV_StsBadArg, "Bad input sequence" );
+
+    if( !elem )
+        CV_Error( CV_StsNullPtr, "Null element pointer" );
+
+    int elem_size = seq->elem_size;
+    int total = seq->total;
+
+    if( total == 0 )
+        return 0;
+
+    if( !is_sorted )
+    {
+        CvSeqReader reader;
+        cvStartReadSeq( seq, &reader, 0 );
+
+        if( cmp_func )
+        {
+            for( i = 0; i < total; i++ )
+            {
+                if( cmp_func( elem, reader.ptr, userdata ) == 0 )
+                    break;
+                CV_NEXT_SEQ_ELEM( elem_size, reader );
+            }
+        }
+        else if( (elem_size & (sizeof(int)-1)) == 0 )
+        {
+            for( i = 0; i < total; i++ )
+            {
+                for( j = 0; j < elem_size; j += sizeof(int) )
+                {
+                    if( *(const int*)(reader.ptr + j) != *(const int*)(elem + j) )
+                        break;
+                }
+                if( j == elem_size )
+                    break;
+                CV_NEXT_SEQ_ELEM( elem_size, reader );
+            }
+        }
+        else
+        {
+            for( i = 0; i < total; i++ )
+            {
+                for( j = 0; j < elem_size; j++ )
+                {
+                    if( reader.ptr[j] != elem[j] )
+                        break;
+                }
+                if( j == elem_size )
+                    break;
+                CV_NEXT_SEQ_ELEM( elem_size, reader );
+            }
+        }
+
+        idx = i;
+        if( i < total )
+            result = reader.ptr;
+    }
+    else
+    {
+        if( !cmp_func )
+            CV_Error( CV_StsNullPtr, "Null compare function" );
+
+        i = 0, j = total;
+
+        while( j > i )
+        {
+            int k = (i+j)>>1, code;
+            schar* ptr = cvGetSeqElem( seq, k );
+            code = cmp_func( elem, ptr, userdata );
+            if( !code )
+            {
+                result = ptr;
+                idx = k;
+                if( _idx )
+                    *_idx = idx;
+                return result;
+            }
+            if( code < 0 )
+                j = k;
+            else
+                i = k+1;
+        }
+        idx = j;
+    }
+
+    if( _idx )
+        *_idx = idx;
+
+    return result;
+}
+
+
+CV_IMPL void
+cvSeqInvert( CvSeq* seq )
+{
+    CvSeqReader left_reader, right_reader;
+    int elem_size;
+    int i, count;
+
+    cvStartReadSeq( seq, &left_reader, 0 );
+    cvStartReadSeq( seq, &right_reader, 1 );
+    elem_size = seq->elem_size;
+    count = seq->total >> 1;
+
+    for( i = 0; i < count; i++ )
+    {
+        CV_SWAP_ELEMS( left_reader.ptr, right_reader.ptr, elem_size );
+        CV_NEXT_SEQ_ELEM( elem_size, left_reader );
+        CV_PREV_SEQ_ELEM( elem_size, right_reader );
+    }
+}
+
+
+typedef struct CvPTreeNode
+{
+    struct CvPTreeNode* parent;
+    schar* element;
+    int rank;
+}
+CvPTreeNode;
+
+
+// This function splits the input sequence or set into one or more equivalence classes.
+// is_equal(a,b,...) returns non-zero if the two sequence elements
+// belong to the same class.  The function returns sequence of integers -
+// 0-based class indexes for each element.
+//
+// The algorithm is described in "Introduction to Algorithms"
+// by Cormen, Leiserson and Rivest, chapter "Data structures for disjoint sets"
+CV_IMPL  int
+cvSeqPartition( const CvSeq* seq, CvMemStorage* storage, CvSeq** labels,
+                CvCmpFunc is_equal, void* userdata )
+{
+    CvSeq* result = 0;
+    CvMemStorage* temp_storage = 0;
+    int class_idx = 0;
+
+    CvSeqWriter writer;
+    CvSeqReader reader, reader0;
+    CvSeq* nodes;
+    int i, j;
+    int is_set;
+
+    if( !labels )
+        CV_Error( CV_StsNullPtr, "" );
+
+    if( !seq || !is_equal )
+        CV_Error( CV_StsNullPtr, "" );
+
+    if( !storage )
+        storage = seq->storage;
+
+    if( !storage )
+        CV_Error( CV_StsNullPtr, "" );
+
+    is_set = CV_IS_SET(seq);
+
+    temp_storage = cvCreateChildMemStorage( storage );
+
+    nodes = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvPTreeNode), temp_storage );
+
+    cvStartReadSeq( seq, &reader );
+    memset( &writer, 0, sizeof(writer));
+    cvStartAppendToSeq( nodes, &writer );
+
+    // Initial O(N) pass. Make a forest of single-vertex trees.
+    for( i = 0; i < seq->total; i++ )
+    {
+        CvPTreeNode node = { 0, 0, 0 };
+        if( !is_set || CV_IS_SET_ELEM( reader.ptr ))
+            node.element = reader.ptr;
+        CV_WRITE_SEQ_ELEM( node, writer );
+        CV_NEXT_SEQ_ELEM( seq->elem_size, reader );
+    }
+
+    cvEndWriteSeq( &writer );
+
+    // Because in the next loop we will iterate
+    // through all the sequence nodes each time,
+    // we do not need to initialize reader every time:
+    cvStartReadSeq( nodes, &reader );
+    cvStartReadSeq( nodes, &reader0 );
+
+    // The main O(N^2) pass. Merge connected components.
+    for( i = 0; i < nodes->total; i++ )
+    {
+        CvPTreeNode* node = (CvPTreeNode*)(reader0.ptr);
+        CvPTreeNode* root = node;
+        CV_NEXT_SEQ_ELEM( nodes->elem_size, reader0 );
+
+        if( !node->element )
+            continue;
+
+        // find root
+        while( root->parent )
+            root = root->parent;
+
+        for( j = 0; j < nodes->total; j++ )
+        {
+            CvPTreeNode* node2 = (CvPTreeNode*)reader.ptr;
+
+            if( node2->element && node2 != node &&
+                is_equal( node->element, node2->element, userdata ))
+            {
+                CvPTreeNode* root2 = node2;
+
+                // unite both trees
+                while( root2->parent )
+                    root2 = root2->parent;
+
+                if( root2 != root )
+                {
+                    if( root->rank > root2->rank )
+                        root2->parent = root;
+                    else
+                    {
+                        root->parent = root2;
+                        root2->rank += root->rank == root2->rank;
+                        root = root2;
+                    }
+                    assert( root->parent == 0 );
+
+                    // Compress path from node2 to the root:
+                    while( node2->parent )
+                    {
+                        CvPTreeNode* temp = node2;
+                        node2 = node2->parent;
+                        temp->parent = root;
+                    }
+
+                    // Compress path from node to the root:
+                    node2 = node;
+                    while( node2->parent )
+                    {
+                        CvPTreeNode* temp = node2;
+                        node2 = node2->parent;
+                        temp->parent = root;
+                    }
+                }
+            }
+
+            CV_NEXT_SEQ_ELEM( sizeof(*node), reader );
+        }
+    }
+
+    // Final O(N) pass (Enumerate classes)
+    // Reuse reader one more time
+    result = cvCreateSeq( 0, sizeof(CvSeq), sizeof(int), storage );
+    cvStartAppendToSeq( result, &writer );
+
+    for( i = 0; i < nodes->total; i++ )
+    {
+        CvPTreeNode* node = (CvPTreeNode*)reader.ptr;
+        int idx = -1;
+
+        if( node->element )
+        {
+            while( node->parent )
+                node = node->parent;
+            if( node->rank >= 0 )
+                node->rank = ~class_idx++;
+            idx = ~node->rank;
+        }
+
+        CV_NEXT_SEQ_ELEM( sizeof(*node), reader );
+        CV_WRITE_SEQ_ELEM( idx, writer );
+    }
+
+    cvEndWriteSeq( &writer );
+
+    if( labels )
+        *labels = result;
+
+    cvReleaseMemStorage( &temp_storage );
+    return class_idx;
+}
+
+
+/****************************************************************************************\
+*                                      Set implementation                                *
+\****************************************************************************************/
+
+/* Creates empty set: */
+CV_IMPL CvSet*
+cvCreateSet( int set_flags, int header_size, int elem_size, CvMemStorage * storage )
+{
+    if( !storage )
+        CV_Error( CV_StsNullPtr, "" );
+    if( header_size < (int)sizeof( CvSet ) ||
+        elem_size < (int)sizeof(void*)*2 ||
+        (elem_size & (sizeof(void*)-1)) != 0 )
+        CV_Error( CV_StsBadSize, "" );
+
+    CvSet* set = (CvSet*) cvCreateSeq( set_flags, header_size, elem_size, storage );
+    set->flags = (set->flags & ~CV_MAGIC_MASK) | CV_SET_MAGIC_VAL;
+
+    return set;
+}
+
+
+/* Add new element to the set: */
+CV_IMPL int
+cvSetAdd( CvSet* set, CvSetElem* element, CvSetElem** inserted_element )
+{
+    int id = -1;
+    CvSetElem *free_elem;
+
+    if( !set )
+        CV_Error( CV_StsNullPtr, "" );
+
+    if( !(set->free_elems) )
+    {
+        int count = set->total;
+        int elem_size = set->elem_size;
+        schar *ptr;
+        icvGrowSeq( (CvSeq *) set, 0 );
+
+        set->free_elems = (CvSetElem*) (ptr = set->ptr);
+        for( ; ptr + elem_size <= set->block_max; ptr += elem_size, count++ )
+        {
+            ((CvSetElem*)ptr)->flags = count | CV_SET_ELEM_FREE_FLAG;
+            ((CvSetElem*)ptr)->next_free = (CvSetElem*)(ptr + elem_size);
+        }
+        assert( count <= CV_SET_ELEM_IDX_MASK+1 );
+        ((CvSetElem*)(ptr - elem_size))->next_free = 0;
+        set->first->prev->count += count - set->total;
+        set->total = count;
+        set->ptr = set->block_max;
+    }
+
+    free_elem = set->free_elems;
+    set->free_elems = free_elem->next_free;
+
+    id = free_elem->flags & CV_SET_ELEM_IDX_MASK;
+    if( element )
+        memcpy( free_elem, element, set->elem_size );
+
+    free_elem->flags = id;
+    set->active_count++;
+
+    if( inserted_element )
+        *inserted_element = free_elem;
+
+    return id;
+}
+
+
+/* Remove element from a set given element index: */
+CV_IMPL void
+cvSetRemove( CvSet* set, int index )
+{
+    CvSetElem* elem = cvGetSetElem( set, index );
+    if( elem )
+        cvSetRemoveByPtr( set, elem );
+    else if( !set )
+        CV_Error( CV_StsNullPtr, "" );
+}
+
+
+/* Remove all elements from a set: */
+CV_IMPL void
+cvClearSet( CvSet* set )
+{
+    cvClearSeq( (CvSeq*)set );
+    set->free_elems = 0;
+    set->active_count = 0;
+}
+
+
+/****************************************************************************************\
+*                                 Graph  implementation                                  *
+\****************************************************************************************/
+
+/* Create a new graph: */
+CV_IMPL CvGraph *
+cvCreateGraph( int graph_type, int header_size,
+               int vtx_size, int edge_size, CvMemStorage * storage )
+{
+    CvGraph *graph = 0;
+    CvSet *edges = 0;
+    CvSet *vertices = 0;
+
+    if( header_size < (int) sizeof( CvGraph     )
+    ||  edge_size   < (int) sizeof( CvGraphEdge )
+    ||  vtx_size    < (int) sizeof( CvGraphVtx  )
+    ){
+        CV_Error( CV_StsBadSize, "" );
+    }
+
+    vertices = cvCreateSet( graph_type, header_size, vtx_size, storage );
+    edges = cvCreateSet( CV_SEQ_KIND_GENERIC | CV_SEQ_ELTYPE_GRAPH_EDGE,
+                                  sizeof( CvSet ), edge_size, storage );
+
+    graph = (CvGraph*)vertices;
+    graph->edges = edges;
+
+    return graph;
+}
+
+
+/* Remove all vertices and edges from a graph: */
+CV_IMPL void
+cvClearGraph( CvGraph * graph )
+{
+    if( !graph )
+        CV_Error( CV_StsNullPtr, "" );
+
+    cvClearSet( graph->edges );
+    cvClearSet( (CvSet*)graph );
+}
+
+
+/* Add a vertex to a graph: */
+CV_IMPL int
+cvGraphAddVtx( CvGraph* graph, const CvGraphVtx* _vertex, CvGraphVtx** _inserted_vertex )
+{
+    CvGraphVtx *vertex = 0;
+    int index = -1;
+
+    if( !graph )
+        CV_Error( CV_StsNullPtr, "" );
+
+    vertex = (CvGraphVtx*)cvSetNew((CvSet*)graph);
+    if( vertex )
+    {
+        if( _vertex )
+            memcpy( vertex + 1, _vertex + 1, graph->elem_size - sizeof(CvGraphVtx) );
+        vertex->first = 0;
+        index = vertex->flags;
+    }
+
+    if( _inserted_vertex )
+        *_inserted_vertex = vertex;
+
+    return index;
+}
+
+
+/* Remove a vertex from the graph together with its incident edges: */
+CV_IMPL int
+cvGraphRemoveVtxByPtr( CvGraph* graph, CvGraphVtx* vtx )
+{
+    int count = -1;
+
+    if( !graph || !vtx )
+        CV_Error( CV_StsNullPtr, "" );
+
+    if( !CV_IS_SET_ELEM(vtx))
+        CV_Error( CV_StsBadArg, "The vertex does not belong to the graph" );
+
+    count = graph->edges->active_count;
+    for( ;; )
+    {
+        CvGraphEdge *edge = vtx->first;
+        if( !edge )
+            break;
+        cvGraphRemoveEdgeByPtr( graph, edge->vtx[0], edge->vtx[1] );
+    }
+    count -= graph->edges->active_count;
+    cvSetRemoveByPtr( (CvSet*)graph, vtx );
+
+    return count;
+}
+
+
+/* Remove a vertex from the graph together with its incident edges: */
+CV_IMPL int
+cvGraphRemoveVtx( CvGraph* graph, int index )
+{
+    int count = -1;
+    CvGraphVtx *vtx = 0;
+
+    if( !graph )
+        CV_Error( CV_StsNullPtr, "" );
+
+    vtx = cvGetGraphVtx( graph, index );
+    if( !vtx )
+        CV_Error( CV_StsBadArg, "The vertex is not found" );
+
+    count = graph->edges->active_count;
+    for( ;; )
+    {
+        CvGraphEdge *edge = vtx->first;
+        count++;
+
+        if( !edge )
+            break;
+        cvGraphRemoveEdgeByPtr( graph, edge->vtx[0], edge->vtx[1] );
+    }
+    count -= graph->edges->active_count;
+    cvSetRemoveByPtr( (CvSet*)graph, vtx );
+
+    return count;
+}
+
+
+/* Find a graph edge given pointers to the ending vertices: */
+CV_IMPL CvGraphEdge*
+cvFindGraphEdgeByPtr( const CvGraph* graph,
+                      const CvGraphVtx* start_vtx,
+                      const CvGraphVtx* end_vtx )
+{
+    int ofs = 0;
+
+    if( !graph || !start_vtx || !end_vtx )
+        CV_Error( CV_StsNullPtr, "" );
+
+    if( start_vtx == end_vtx )
+        return 0;
+
+    if( !CV_IS_GRAPH_ORIENTED( graph ) &&
+        (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) )
+    {
+        const CvGraphVtx* t;
+        CV_SWAP( start_vtx, end_vtx, t );
+    }
+
+    CvGraphEdge* edge = start_vtx->first;
+    for( ; edge; edge = edge->next[ofs] )
+    {
+        ofs = start_vtx == edge->vtx[1];
+        assert( ofs == 1 || start_vtx == edge->vtx[0] );
+        if( edge->vtx[1] == end_vtx )
+            break;
+    }
+
+    return edge;
+}
+
+
+/* Find an edge in the graph given indices of the ending vertices: */
+CV_IMPL CvGraphEdge *
+cvFindGraphEdge( const CvGraph* graph, int start_idx, int end_idx )
+{
+    CvGraphVtx *start_vtx;
+    CvGraphVtx *end_vtx;
+
+    if( !graph )
+        CV_Error( CV_StsNullPtr, "graph pointer is NULL" );
+
+    start_vtx = cvGetGraphVtx( graph, start_idx );
+    end_vtx = cvGetGraphVtx( graph, end_idx );
+
+    return cvFindGraphEdgeByPtr( graph, start_vtx, end_vtx );
+}
+
+
+/* Given two vertices, return the edge
+ * connecting them, creating it if it
+ * did not already exist:
+ */
+CV_IMPL int
+cvGraphAddEdgeByPtr( CvGraph* graph,
+                     CvGraphVtx* start_vtx, CvGraphVtx* end_vtx,
+                     const CvGraphEdge* _edge,
+                     CvGraphEdge ** _inserted_edge )
+{
+    CvGraphEdge *edge = 0;
+    int result = -1;
+    int delta;
+
+    if( !graph )
+        CV_Error( CV_StsNullPtr, "graph pointer is NULL" );
+
+    if( !CV_IS_GRAPH_ORIENTED( graph ) &&
+        (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) )
+    {
+        CvGraphVtx* t;
+        CV_SWAP( start_vtx, end_vtx, t );
+    }
+
+    edge = cvFindGraphEdgeByPtr( graph, start_vtx, end_vtx );
+    if( edge )
+    {
+        result = 0;
+        if( _inserted_edge )
+            *_inserted_edge = edge;
+        return result;
+    }
+
+    if( start_vtx == end_vtx )
+        CV_Error( start_vtx ? CV_StsBadArg : CV_StsNullPtr,
+        "vertex pointers coinside (or set to NULL)" );
+
+    edge = (CvGraphEdge*)cvSetNew( (CvSet*)(graph->edges) );
+    assert( edge->flags >= 0 );
+
+    edge->vtx[0] = start_vtx;
+    edge->vtx[1] = end_vtx;
+    edge->next[0] = start_vtx->first;
+    edge->next[1] = end_vtx->first;
+    start_vtx->first = end_vtx->first = edge;
+
+    delta = graph->edges->elem_size - sizeof(*edge);
+    if( _edge )
+    {
+        if( delta > 0 )
+            memcpy( edge + 1, _edge + 1, delta );
+        edge->weight = _edge->weight;
+    }
+    else
+    {
+        if( delta > 0 )
+            memset( edge + 1, 0, delta );
+        edge->weight = 1.f;
+    }
+
+    result = 1;
+
+    if( _inserted_edge )
+        *_inserted_edge = edge;
+
+    return result;
+}
+
+/* Given two vertices, return the edge
+ * connecting them, creating it if it
+ * did not already exist:
+ */
+CV_IMPL int
+cvGraphAddEdge( CvGraph* graph,
+                int start_idx, int end_idx,
+                const CvGraphEdge* _edge,
+                CvGraphEdge ** _inserted_edge )
+{
+    CvGraphVtx *start_vtx;
+    CvGraphVtx *end_vtx;
+
+    if( !graph )
+        CV_Error( CV_StsNullPtr, "" );
+
+    start_vtx = cvGetGraphVtx( graph, start_idx );
+    end_vtx = cvGetGraphVtx( graph, end_idx );
+
+    return cvGraphAddEdgeByPtr( graph, start_vtx, end_vtx, _edge, _inserted_edge );
+}
+
+
+/* Remove the graph edge connecting two given vertices: */
+CV_IMPL void
+cvGraphRemoveEdgeByPtr( CvGraph* graph, CvGraphVtx* start_vtx, CvGraphVtx* end_vtx )
+{
+    int ofs, prev_ofs;
+    CvGraphEdge *edge, *next_edge, *prev_edge;
+
+    if( !graph || !start_vtx || !end_vtx )
+        CV_Error( CV_StsNullPtr, "" );
+
+    if( start_vtx == end_vtx )
+        return;
+
+    if( !CV_IS_GRAPH_ORIENTED( graph ) &&
+        (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) )
+    {
+        CvGraphVtx* t;
+        CV_SWAP( start_vtx, end_vtx, t );
+    }
+
+    for( ofs = prev_ofs = 0, prev_edge = 0, edge = start_vtx->first; edge != 0;
+         prev_ofs = ofs, prev_edge = edge, edge = edge->next[ofs] )
+    {
+        ofs = start_vtx == edge->vtx[1];
+        assert( ofs == 1 || start_vtx == edge->vtx[0] );
+        if( edge->vtx[1] == end_vtx )
+            break;
+    }
+
+    if( !edge )
+        return;
+
+    next_edge = edge->next[ofs];
+    if( prev_edge )
+        prev_edge->next[prev_ofs] = next_edge;
+    else
+        start_vtx->first = next_edge;
+
+    for( ofs = prev_ofs = 0, prev_edge = 0, edge = end_vtx->first; edge != 0;
+         prev_ofs = ofs, prev_edge = edge, edge = edge->next[ofs] )
+    {
+        ofs = end_vtx == edge->vtx[1];
+        assert( ofs == 1 || end_vtx == edge->vtx[0] );
+        if( edge->vtx[0] == start_vtx )
+            break;
+    }
+
+    assert( edge != 0 );
+
+    next_edge = edge->next[ofs];
+    if( prev_edge )
+        prev_edge->next[prev_ofs] = next_edge;
+    else
+        end_vtx->first = next_edge;
+
+    cvSetRemoveByPtr( graph->edges, edge );
+}
+
+
+/* Remove the graph edge connecting two given vertices: */
+CV_IMPL void
+cvGraphRemoveEdge( CvGraph* graph, int start_idx, int end_idx )
+{
+    CvGraphVtx *start_vtx;
+    CvGraphVtx *end_vtx;
+
+    if( !graph )
+        CV_Error( CV_StsNullPtr, "" );
+
+    start_vtx = cvGetGraphVtx( graph, start_idx );
+    end_vtx = cvGetGraphVtx( graph, end_idx );
+
+    cvGraphRemoveEdgeByPtr( graph, start_vtx, end_vtx );
+}
+
+
+/* Count number of edges incident to a given vertex: */
+CV_IMPL int
+cvGraphVtxDegreeByPtr( const CvGraph* graph, const CvGraphVtx* vertex )
+{
+    CvGraphEdge *edge;
+    int count;
+
+    if( !graph || !vertex )
+        CV_Error( CV_StsNullPtr, "" );
+
+    for( edge = vertex->first, count = 0; edge; )
+    {
+        count++;
+        edge = CV_NEXT_GRAPH_EDGE( edge, vertex );
+    }
+
+    return count;
+}
+
+
+/* Count number of edges incident to a given vertex: */
+CV_IMPL int
+cvGraphVtxDegree( const CvGraph* graph, int vtx_idx )
+{
+    CvGraphVtx *vertex;
+    CvGraphEdge *edge;
+    int count;
+
+    if( !graph )
+        CV_Error( CV_StsNullPtr, "" );
+
+    vertex = cvGetGraphVtx( graph, vtx_idx );
+    if( !vertex )
+        CV_Error( CV_StsObjectNotFound, "" );
+
+    for( edge = vertex->first, count = 0; edge; )
+    {
+        count++;
+        edge = CV_NEXT_GRAPH_EDGE( edge, vertex );
+    }
+
+    return count;
+}
+
+
+typedef struct CvGraphItem
+{
+    CvGraphVtx* vtx;
+    CvGraphEdge* edge;
+}
+CvGraphItem;
+
+
+static  void
+icvSeqElemsClearFlags( CvSeq* seq, int offset, int clear_mask )
+{
+    CvSeqReader reader;
+    int i, total, elem_size;
+
+    if( !seq )
+        CV_Error( CV_StsNullPtr, "" );
+
+    elem_size = seq->elem_size;
+    total = seq->total;
+
+    if( (unsigned)offset > (unsigned)elem_size )
+        CV_Error( CV_StsBadArg, "" );
+
+    cvStartReadSeq( seq, &reader );
+
+    for( i = 0; i < total; i++ )
+    {
+        int* flag_ptr = (int*)(reader.ptr + offset);
+        *flag_ptr &= ~clear_mask;
+
+        CV_NEXT_SEQ_ELEM( elem_size, reader );
+    }
+}
+
+
+static  schar*
+icvSeqFindNextElem( CvSeq* seq, int offset, int mask,
+                    int value, int* start_index )
+{
+    schar* elem_ptr = 0;
+
+    CvSeqReader reader;
+    int total, elem_size, index;
+
+    if( !seq || !start_index )
+        CV_Error( CV_StsNullPtr, "" );
+
+    elem_size = seq->elem_size;
+    total = seq->total;
+    index = *start_index;
+
+    if( (unsigned)offset > (unsigned)elem_size )
+        CV_Error( CV_StsBadArg, "" );
+
+    if( total == 0 )
+        return 0;
+
+    if( (unsigned)index >= (unsigned)total )
+    {
+        index %= total;
+        index += index < 0 ? total : 0;
+    }
+
+    cvStartReadSeq( seq, &reader );
+
+    if( index != 0 )
+        cvSetSeqReaderPos( &reader, index );
+
+    for( index = 0; index < total; index++ )
+    {
+        int* flag_ptr = (int*)(reader.ptr + offset);
+        if( (*flag_ptr & mask) == value )
+            break;
+
+        CV_NEXT_SEQ_ELEM( elem_size, reader );
+    }
+
+    if( index < total )
+    {
+        elem_ptr = reader.ptr;
+        *start_index = index;
+    }
+
+    return  elem_ptr;
+}
+
+#define CV_FIELD_OFFSET( field, structtype ) ((int)(size_t)&((structtype*)0)->field)
+
+CV_IMPL CvGraphScanner*
+cvCreateGraphScanner( CvGraph* graph, CvGraphVtx* vtx, int mask )
+{
+    if( !graph )
+        CV_Error( CV_StsNullPtr, "Null graph pointer" );
+
+    CV_Assert( graph->storage != 0 );
+
+    CvGraphScanner* scanner = (CvGraphScanner*)cvAlloc( sizeof(*scanner) );
+    memset( scanner, 0, sizeof(*scanner));
+
+    scanner->graph = graph;
+    scanner->mask = mask;
+    scanner->vtx = vtx;
+    scanner->index = vtx == 0 ? 0 : -1;
+
+    CvMemStorage* child_storage = cvCreateChildMemStorage( graph->storage );
+
+    scanner->stack = cvCreateSeq( 0, sizeof(CvSet),
+                       sizeof(CvGraphItem), child_storage );
+
+    icvSeqElemsClearFlags( (CvSeq*)graph,
+                                    CV_FIELD_OFFSET( flags, CvGraphVtx),
+                                    CV_GRAPH_ITEM_VISITED_FLAG|
+                                    CV_GRAPH_SEARCH_TREE_NODE_FLAG );
+
+    icvSeqElemsClearFlags( (CvSeq*)(graph->edges),
+                                    CV_FIELD_OFFSET( flags, CvGraphEdge),
+                                    CV_GRAPH_ITEM_VISITED_FLAG );
+
+    return scanner;
+}
+
+
+CV_IMPL void
+cvReleaseGraphScanner( CvGraphScanner** scanner )
+{
+    if( !scanner )
+        CV_Error( CV_StsNullPtr, "Null double pointer to graph scanner" );
+
+    if( *scanner )
+    {
+        if( (*scanner)->stack )
+            cvReleaseMemStorage( &((*scanner)->stack->storage));
+        cvFree( scanner );
+    }
+}
+
+
+CV_IMPL int
+cvNextGraphItem( CvGraphScanner* scanner )
+{
+    int code = -1;
+    CvGraphVtx* vtx;
+    CvGraphVtx* dst;
+    CvGraphEdge* edge;
+    CvGraphItem item;
+
+    if( !scanner || !(scanner->stack))
+        CV_Error( CV_StsNullPtr, "Null graph scanner" );
+
+    dst = scanner->dst;
+    vtx = scanner->vtx;
+    edge = scanner->edge;
+
+    for(;;)
+    {
+        for(;;)
+        {
+            if( dst && !CV_IS_GRAPH_VERTEX_VISITED(dst) )
+            {
+                scanner->vtx = vtx = dst;
+                edge = vtx->first;
+                dst->flags |= CV_GRAPH_ITEM_VISITED_FLAG;
+
+                if((scanner->mask & CV_GRAPH_VERTEX))
+                {
+                    scanner->vtx = vtx;
+                    scanner->edge = vtx->first;
+                    scanner->dst = 0;
+                    code = CV_GRAPH_VERTEX;
+                    return code;
+                }
+            }
+
+            while( edge )
+            {
+                dst = edge->vtx[vtx == edge->vtx[0]];
+
+                if( !CV_IS_GRAPH_EDGE_VISITED(edge) )
+                {
+                    // Check that the edge is outgoing:
+                    if( !CV_IS_GRAPH_ORIENTED( scanner->graph ) || dst != edge->vtx[0] )
+                    {
+                        edge->flags |= CV_GRAPH_ITEM_VISITED_FLAG;
+
+                        if( !CV_IS_GRAPH_VERTEX_VISITED(dst) )
+                        {
+                            item.vtx = vtx;
+                            item.edge = edge;
+
+                            vtx->flags |= CV_GRAPH_SEARCH_TREE_NODE_FLAG;
+
+                            cvSeqPush( scanner->stack, &item );
+
+                            if( scanner->mask & CV_GRAPH_TREE_EDGE )
+                            {
+                                code = CV_GRAPH_TREE_EDGE;
+                                scanner->vtx = vtx;
+                                scanner->dst = dst;
+                                scanner->edge = edge;
+                                return code;
+                            }
+                            break;
+                        }
+                        else
+                        {
+                            if( scanner->mask & (CV_GRAPH_BACK_EDGE|
+                                                 CV_GRAPH_CROSS_EDGE|
+                                                 CV_GRAPH_FORWARD_EDGE) )
+                            {
+                                code = (dst->flags & CV_GRAPH_SEARCH_TREE_NODE_FLAG) ?
+                                       CV_GRAPH_BACK_EDGE :
+                                       (edge->flags & CV_GRAPH_FORWARD_EDGE_FLAG) ?
+                                       CV_GRAPH_FORWARD_EDGE : CV_GRAPH_CROSS_EDGE;
+                                edge->flags &= ~CV_GRAPH_FORWARD_EDGE_FLAG;
+                                if( scanner->mask & code )
+                                {
+                                    scanner->vtx = vtx;
+                                    scanner->dst = dst;
+                                    scanner->edge = edge;
+                                    return code;
+                                }
+                            }
+                        }
+                    }
+                    else if( (dst->flags & (CV_GRAPH_ITEM_VISITED_FLAG|
+                             CV_GRAPH_SEARCH_TREE_NODE_FLAG)) ==
+                             (CV_GRAPH_ITEM_VISITED_FLAG|
+                             CV_GRAPH_SEARCH_TREE_NODE_FLAG))
+                    {
+                        edge->flags |= CV_GRAPH_FORWARD_EDGE_FLAG;
+                    }
+                }
+
+                edge = CV_NEXT_GRAPH_EDGE( edge, vtx );
+            }
+
+            if( !edge ) /* need to backtrack */
+            {
+                if( scanner->stack->total == 0 )
+                {
+                    if( scanner->index >= 0 )
+                        vtx = 0;
+                    else
+                        scanner->index = 0;
+                    break;
+                }
+                cvSeqPop( scanner->stack, &item );
+                vtx = item.vtx;
+                vtx->flags &= ~CV_GRAPH_SEARCH_TREE_NODE_FLAG;
+                edge = item.edge;
+                dst = 0;
+
+                if( scanner->mask & CV_GRAPH_BACKTRACKING )
+                {
+                    scanner->vtx = vtx;
+                    scanner->edge = edge;
+                    scanner->dst = edge->vtx[vtx == edge->vtx[0]];
+                    code = CV_GRAPH_BACKTRACKING;
+                    return code;
+                }
+            }
+        }
+
+        if( !vtx )
+        {
+            vtx = (CvGraphVtx*)icvSeqFindNextElem( (CvSeq*)(scanner->graph),
+                  CV_FIELD_OFFSET( flags, CvGraphVtx ), CV_GRAPH_ITEM_VISITED_FLAG|INT_MIN,
+                  0, &(scanner->index) );
+
+            if( !vtx )
+            {
+                code = CV_GRAPH_OVER;
+                break;
+            }
+        }
+
+        dst = vtx;
+        if( scanner->mask & CV_GRAPH_NEW_TREE )
+        {
+            scanner->dst = dst;
+            scanner->edge = 0;
+            scanner->vtx = 0;
+            code = CV_GRAPH_NEW_TREE;
+            break;
+        }
+    }
+
+    return code;
+}
+
+
+CV_IMPL CvGraph*
+cvCloneGraph( const CvGraph* graph, CvMemStorage* storage )
+{
+    int* flag_buffer = 0;
+    CvGraphVtx** ptr_buffer = 0;
+    CvGraph* result = 0;
+
+    int i, k;
+    int vtx_size, edge_size;
+    CvSeqReader reader;
+
+    if( !CV_IS_GRAPH(graph))
+        CV_Error( CV_StsBadArg, "Invalid graph pointer" );
+
+    if( !storage )
+        storage = graph->storage;
+
+    if( !storage )
+        CV_Error( CV_StsNullPtr, "NULL storage pointer" );
+
+    vtx_size = graph->elem_size;
+    edge_size = graph->edges->elem_size;
+
+    flag_buffer = (int*)cvAlloc( graph->total*sizeof(flag_buffer[0]));
+    ptr_buffer = (CvGraphVtx**)cvAlloc( graph->total*sizeof(ptr_buffer[0]));
+    result = cvCreateGraph( graph->flags, graph->header_size,
+                                     vtx_size, edge_size, storage );
+    memcpy( result + sizeof(CvGraph), graph + sizeof(CvGraph),
+            graph->header_size - sizeof(CvGraph));
+
+    // Pass 1.  Save flags, copy vertices:
+    cvStartReadSeq( (CvSeq*)graph, &reader );
+    for( i = 0, k = 0; i < graph->total; i++ )
+    {
+        if( CV_IS_SET_ELEM( reader.ptr ))
+        {
+            CvGraphVtx* vtx = (CvGraphVtx*)reader.ptr;
+            CvGraphVtx* dstvtx = 0;
+            cvGraphAddVtx( result, vtx, &dstvtx );
+            flag_buffer[k] = dstvtx->flags = vtx->flags;
+            vtx->flags = k;
+            ptr_buffer[k++] = dstvtx;
+        }
+        CV_NEXT_SEQ_ELEM( vtx_size, reader );
+    }
+
+    // Pass 2.  Copy edges:
+    cvStartReadSeq( (CvSeq*)graph->edges, &reader );
+    for( i = 0; i < graph->edges->total; i++ )
+    {
+        if( CV_IS_SET_ELEM( reader.ptr ))
+        {
+            CvGraphEdge* edge = (CvGraphEdge*)reader.ptr;
+            CvGraphEdge* dstedge = 0;
+            CvGraphVtx* new_org = ptr_buffer[edge->vtx[0]->flags];
+            CvGraphVtx* new_dst = ptr_buffer[edge->vtx[1]->flags];
+            cvGraphAddEdgeByPtr( result, new_org, new_dst, edge, &dstedge );
+            dstedge->flags = edge->flags;
+        }
+        CV_NEXT_SEQ_ELEM( edge_size, reader );
+    }
+
+    // Pass 3.  Restore flags:
+    cvStartReadSeq( (CvSeq*)graph, &reader );
+    for( i = 0, k = 0; i < graph->edges->total; i++ )
+    {
+        if( CV_IS_SET_ELEM( reader.ptr ))
+        {
+            CvGraphVtx* vtx = (CvGraphVtx*)reader.ptr;
+            vtx->flags = flag_buffer[k++];
+        }
+        CV_NEXT_SEQ_ELEM( vtx_size, reader );
+    }
+
+    cvFree( &flag_buffer );
+    cvFree( &ptr_buffer );
+
+    if( cvGetErrStatus() < 0 )
+        result = 0;
+
+    return result;
+}
+
+
+/****************************************************************************************\
+*                                 Working with sequence tree                             *
+\****************************************************************************************/
+
+// Gather pointers to all the sequences, accessible from the <first>, to the single sequence.
+CV_IMPL CvSeq*
+cvTreeToNodeSeq( const void* first, int header_size, CvMemStorage* storage )
+{
+    CvSeq* allseq = 0;
+    CvTreeNodeIterator iterator;
+
+    if( !storage )
+        CV_Error( CV_StsNullPtr, "NULL storage pointer" );
+
+    allseq = cvCreateSeq( 0, header_size, sizeof(first), storage );
+
+    if( first )
+    {
+        cvInitTreeNodeIterator( &iterator, first, INT_MAX );
+
+        for(;;)
+        {
+            void* node = cvNextTreeNode( &iterator );
+            if( !node )
+                break;
+            cvSeqPush( allseq, &node );
+        }
+    }
+
+    
+
+    return allseq;
+}
+
+
+typedef struct CvTreeNode
+{
+    int       flags;         /* micsellaneous flags */
+    int       header_size;   /* size of sequence header */
+    struct    CvTreeNode* h_prev; /* previous sequence */
+    struct    CvTreeNode* h_next; /* next sequence */
+    struct    CvTreeNode* v_prev; /* 2nd previous sequence */
+    struct    CvTreeNode* v_next; /* 2nd next sequence */
+}
+CvTreeNode;
+
+
+
+// Insert contour into tree given certain parent sequence.
+// If parent is equal to frame (the most external contour),
+// then added contour will have null pointer to parent:
+CV_IMPL void
+cvInsertNodeIntoTree( void* _node, void* _parent, void* _frame )
+{
+    CvTreeNode* node = (CvTreeNode*)_node;
+    CvTreeNode* parent = (CvTreeNode*)_parent;
+
+    if( !node || !parent )
+        CV_Error( CV_StsNullPtr, "" );
+
+    node->v_prev = _parent != _frame ? parent : 0;
+    node->h_next = parent->v_next;
+
+    assert( parent->v_next != node );
+
+    if( parent->v_next )
+        parent->v_next->h_prev = node;
+    parent->v_next = node;
+}
+
+
+// Remove contour from tree, together with the contour's children:
+CV_IMPL void
+cvRemoveNodeFromTree( void* _node, void* _frame )
+{
+    CvTreeNode* node = (CvTreeNode*)_node;
+    CvTreeNode* frame = (CvTreeNode*)_frame;
+
+    if( !node )
+        CV_Error( CV_StsNullPtr, "" );
+
+    if( node == frame )
+        CV_Error( CV_StsBadArg, "frame node could not be deleted" );
+
+    if( node->h_next )
+        node->h_next->h_prev = node->h_prev;
+
+    if( node->h_prev )
+        node->h_prev->h_next = node->h_next;
+    else
+    {
+        CvTreeNode* parent = node->v_prev;
+        if( !parent )
+            parent = frame;
+
+        if( parent )
+        {
+            assert( parent->v_next == node );
+            parent->v_next = node->h_next;
+        }
+    }
+}
+
+
+CV_IMPL void
+cvInitTreeNodeIterator( CvTreeNodeIterator* treeIterator,
+                        const void* first, int max_level )
+{
+    if( !treeIterator || !first )
+        CV_Error( CV_StsNullPtr, "" );
+
+    if( max_level < 0 )
+        CV_Error( CV_StsOutOfRange, "" );
+
+    treeIterator->node = (void*)first;
+    treeIterator->level = 0;
+    treeIterator->max_level = max_level;
+}
+
+
+CV_IMPL void*
+cvNextTreeNode( CvTreeNodeIterator* treeIterator )
+{
+    CvTreeNode* prevNode = 0;
+    CvTreeNode* node;
+    int level;
+
+    if( !treeIterator )
+        CV_Error( CV_StsNullPtr, "NULL iterator pointer" );
+
+    prevNode = node = (CvTreeNode*)treeIterator->node;
+    level = treeIterator->level;
+
+    if( node )
+    {
+        if( node->v_next && level+1 < treeIterator->max_level )
+        {
+            node = node->v_next;
+            level++;
+        }
+        else
+        {
+            while( node->h_next == 0 )
+            {
+                node = node->v_prev;
+                if( --level < 0 )
+                {
+                    node = 0;
+                    break;
+                }
+            }
+            node = node && treeIterator->max_level != 0 ? node->h_next : 0;
+        }
+    }
+
+    treeIterator->node = node;
+    treeIterator->level = level;
+    return prevNode;
+}
+
+
+CV_IMPL void*
+cvPrevTreeNode( CvTreeNodeIterator* treeIterator )
+{
+    CvTreeNode* prevNode = 0;
+    CvTreeNode* node;
+    int level;
+
+    if( !treeIterator )
+        CV_Error( CV_StsNullPtr, "" );
+
+    prevNode = node = (CvTreeNode*)treeIterator->node;
+    level = treeIterator->level;
+
+    if( node )
+    {
+        if( !node->h_prev )
+        {
+            node = node->v_prev;
+            if( --level < 0 )
+                node = 0;
+        }
+        else
+        {
+            node = node->h_prev;
+
+            while( node->v_next && level < treeIterator->max_level )
+            {
+                node = node->v_next;
+                level++;
+
+                while( node->h_next )
+                    node = node->h_next;
+            }
+        }
+    }
+
+    treeIterator->node = node;
+    treeIterator->level = level;
+    return prevNode;
+}
+
+
+namespace cv
+{
+
+// This is reimplementation of kd-trees from cvkdtree*.* by Xavier Delacour, cleaned-up and
+// adopted to work with the new OpenCV data structures. It's in cxcore to be shared by
+// both cv (CvFeatureTree) and ml (kNN).
+
+// The algorithm is taken from:
+// J.S. Beis and D.G. Lowe. Shape indexing using approximate nearest-neighbor search 
+// in highdimensional spaces. In Proc. IEEE Conf. Comp. Vision Patt. Recog., 
+// pages 1000--1006, 1997. http://citeseer.ist.psu.edu/beis97shape.html 
+
+const int MAX_TREE_DEPTH = 32;
+
+KDTree::KDTree()
+{
+    maxDepth = -1;
+    normType = NORM_L2;
+}
+
+KDTree::KDTree(InputArray _points, bool _copyData)
+{
+    maxDepth = -1;
+    normType = NORM_L2;
+    build(_points, _copyData);
+}
+
+KDTree::KDTree(InputArray _points, InputArray _labels, bool _copyData)
+{
+    maxDepth = -1;
+    normType = NORM_L2;
+    build(_points, _labels, _copyData);
+}    
+    
+struct SubTree
+{
+    SubTree() : first(0), last(0), nodeIdx(0), depth(0) {}
+    SubTree(int _first, int _last, int _nodeIdx, int _depth)
+        : first(_first), last(_last), nodeIdx(_nodeIdx), depth(_depth) {}
+    int first;
+    int last;
+    int nodeIdx;
+    int depth;
+};
+
+
+static float
+medianPartition( size_t* ofs, int a, int b, const float* vals )
+{
+    int k, a0 = a, b0 = b;
+    int middle = (a + b)/2;
+    while( b > a )
+    {
+        int i0 = a, i1 = (a+b)/2, i2 = b;
+        float v0 = vals[ofs[i0]], v1 = vals[ofs[i1]], v2 = vals[ofs[i2]];
+        int ip = v0 < v1 ? (v1 < v2 ? i1 : v0 < v2 ? i2 : i0) :
+            v0 < v2 ? i0 : (v1 < v2 ? i2 : i1);
+        float pivot = vals[ofs[ip]];
+        std::swap(ofs[ip], ofs[i2]);
+
+        for( i1 = i0, i0--; i1 <= i2; i1++ )
+            if( vals[ofs[i1]] <= pivot )
+            {
+                i0++;
+                std::swap(ofs[i0], ofs[i1]);
+            }
+        if( i0 == middle )
+            break;
+        if( i0 > middle )
+            b = i0 - (b == i0);
+        else
+            a = i0;
+    }
+    
+    float pivot = vals[ofs[middle]];
+    int less = 0, more = 0;
+    for( k = a0; k < middle; k++ )
+    {
+        CV_Assert(vals[ofs[k]] <= pivot);
+        less += vals[ofs[k]] < pivot;
+    }
+    for( k = b0; k > middle; k-- )
+    {
+        CV_Assert(vals[ofs[k]] >= pivot);
+        more += vals[ofs[k]] > pivot;
+    }
+    CV_Assert(std::abs(more - less) <= 1);
+
+    return vals[ofs[middle]];
+}
+
+static void
+computeSums( const Mat& points, const size_t* ofs, int a, int b, double* sums )
+{
+    int i, j, dims = points.cols;
+    const float* data = points.ptr<float>(0);
+    for( j = 0; j < dims; j++ )
+        sums[j*2] = sums[j*2+1] = 0;
+    for( i = a; i <= b; i++ )
+    {
+        const float* row = data + ofs[i];
+        for( j = 0; j < dims; j++ )
+        {
+            double t = row[j], s = sums[j*2] + t, s2 = sums[j*2+1] + t*t;
+            sums[j*2] = s; sums[j*2+1] = s2;
+        }
+    }
+}
+
+    
+void KDTree::build(InputArray _points, bool _copyData)
+{
+    build(_points, noArray(), _copyData);
+}
+
+
+void KDTree::build(InputArray __points, InputArray __labels, bool _copyData)
+{
+    Mat _points = __points.getMat(), _labels = __labels.getMat();
+    CV_Assert(_points.type() == CV_32F && !_points.empty());
+    vector<KDTree::Node>().swap(nodes);
+
+    if( !_copyData )
+        points = _points;
+    else
+    {
+        points.release();
+        points.create(_points.size(), _points.type());
+    }
+    
+    int i, j, n = _points.rows, dims = _points.cols, top = 0;
+    const float* data = _points.ptr<float>(0);
+    float* dstdata = points.ptr<float>(0);
+    size_t step = _points.step1();
+    size_t dstep = points.step1();
+    int ptpos = 0;
+    labels.resize(n);
+    const int* _labels_data = 0;
+    
+    if( !_labels.empty() )
+    {
+        int nlabels = _labels.checkVector(1, CV_32S, true);
+        CV_Assert(nlabels == n);
+        _labels_data = (const int*)_labels.data;
+    }
+
+    Mat sumstack(MAX_TREE_DEPTH*2, dims*2, CV_64F);
+    SubTree stack[MAX_TREE_DEPTH*2];
+    
+    vector<size_t> _ptofs(n);
+    size_t* ptofs = &_ptofs[0];
+
+    for( i = 0; i < n; i++ )
+        ptofs[i] = i*step;
+
+    nodes.push_back(Node());
+    computeSums(points, ptofs, 0, n-1, sumstack.ptr<double>(top));
+    stack[top++] = SubTree(0, n-1, 0, 0);
+    int _maxDepth = 0;
+    
+    while( --top >= 0 )
+    {
+        int first = stack[top].first, last = stack[top].last;
+        int depth = stack[top].depth, nidx = stack[top].nodeIdx;
+        int count = last - first + 1, dim = -1;
+        const double* sums = sumstack.ptr<double>(top);
+        double invCount = 1./count, maxVar = -1.;
+
+        if( count == 1 )
+        {
+            int idx0 = (int)(ptofs[first]/step);
+            int idx = _copyData ? ptpos++ : idx0;
+            nodes[nidx].idx = ~idx;
+            if( _copyData )
+            {
+                const float* src = data + ptofs[first];
+                float* dst = dstdata + idx*dstep;
+                for( j = 0; j < dims; j++ )
+                    dst[j] = src[j];
+            }
+            labels[idx] = _labels_data ? _labels_data[idx0] : idx0; 
+            _maxDepth = std::max(_maxDepth, depth);
+            continue;
+        }
+
+        // find the dimensionality with the biggest variance
+        for( j = 0; j < dims; j++ )
+        {
+            double m = sums[j*2]*invCount;
+            double varj = sums[j*2+1]*invCount - m*m;
+            if( maxVar < varj )
+            {
+                maxVar = varj;
+                dim = j;
+            }
+        }
+
+        int left = (int)nodes.size(), right = left + 1;
+        nodes.push_back(Node());
+        nodes.push_back(Node());
+        nodes[nidx].idx = dim;
+        nodes[nidx].left = left;
+        nodes[nidx].right = right;
+        nodes[nidx].boundary = medianPartition(ptofs, first, last, data + dim);
+
+        int middle = (first + last)/2;
+        double *lsums = (double*)sums, *rsums = lsums + dims*2;
+        computeSums(points, ptofs, middle+1, last, rsums);
+        for( j = 0; j < dims*2; j++ )
+            lsums[j] = sums[j] - rsums[j];
+        stack[top++] = SubTree(first, middle, left, depth+1);
+        stack[top++] = SubTree(middle+1, last, right, depth+1);
+    }
+    maxDepth = _maxDepth;
+}
+
+
+struct PQueueElem
+{
+    PQueueElem() : dist(0), idx(0) {}
+    PQueueElem(float _dist, int _idx) : dist(_dist), idx(_idx) {}
+    float dist;
+    int idx;
+};
+
+
+int KDTree::findNearest(InputArray _vec, int K, int emax,
+                        OutputArray _neighborsIdx, OutputArray _neighbors,
+                        OutputArray _dist, OutputArray _labels) const
+    
+{
+    Mat vecmat = _vec.getMat();
+    CV_Assert( vecmat.isContinuous() && vecmat.type() == CV_32F && vecmat.total() == (size_t)points.cols );
+    const float* vec = vecmat.ptr<float>();
+    K = std::min(K, points.rows);
+    int dims = points.cols;
+
+    CV_Assert(K > 0 && (normType == NORM_L2 || normType == NORM_L1));
+
+    AutoBuffer<uchar> _buf((K+1)*(sizeof(float) + sizeof(int)));
+    int* idx = (int*)(uchar*)_buf;
+    float* dist = (float*)(idx + K + 1);
+    int i, j, ncount = 0, e = 0;
+
+    int qsize = 0, maxqsize = 1 << 10;
+    AutoBuffer<uchar> _pqueue(maxqsize*sizeof(PQueueElem));
+    PQueueElem* pqueue = (PQueueElem*)(uchar*)_pqueue;
+    emax = std::max(emax, 1);
+
+    for( e = 0; e < emax; )
+    {
+        float d, alt_d = 0.f;
+        int nidx;
+        
+        if( e == 0 )
+            nidx = 0;
+        else
+        {
+            // take the next node from the priority queue
+            if( qsize == 0 )
+                break;
+            nidx = pqueue[0].idx;
+            alt_d = pqueue[0].dist;
+            if( --qsize > 0 )
+            {
+                std::swap(pqueue[0], pqueue[qsize]);
+                d = pqueue[0].dist;
+                for( i = 0;;)
+                {
+                    int left = i*2 + 1, right = i*2 + 2;
+                    if( left >= qsize )
+                        break;
+                    if( right < qsize && pqueue[right].dist < pqueue[left].dist )
+                        left = right;
+                    if( pqueue[left].dist >= d )
+                        break;
+                    std::swap(pqueue[i], pqueue[left]);
+                    i = left;
+                }
+            }
+            
+            if( ncount == K && alt_d > dist[ncount-1] )
+                continue;
+        }
+
+        for(;;)
+        {
+            if( nidx < 0 )
+                break;
+            const Node& n = nodes[nidx];
+            
+            if( n.idx < 0 )
+            {
+                i = ~n.idx;
+                const float* row = points.ptr<float>(i);
+                if( normType == NORM_L2 )
+                    for( j = 0, d = 0.f; j < dims; j++ )
+                    {
+                        float t = vec[j] - row[j];
+                        d += t*t;
+                    }
+                else
+                    for( j = 0, d = 0.f; j < dims; j++ )
+                        d += std::abs(vec[j] - row[j]);
+                
+                dist[ncount] = d;
+                idx[ncount] = i;
+                for( i = ncount-1; i >= 0; i-- )
+                {
+                    if( dist[i] <= d )
+                        break;
+                    std::swap(dist[i], dist[i+1]);
+                    std::swap(idx[i], idx[i+1]);
+                }
+                ncount += ncount < K;
+                e++;
+                break; 
+            }
+            
+            int alt;
+            if( vec[n.idx] <= n.boundary )
+            {
+                nidx = n.left;
+                alt = n.right;
+            }
+            else
+            {
+                nidx = n.right;
+                alt = n.left;
+            }
+            
+            d = vec[n.idx] - n.boundary;
+            if( normType == NORM_L2 )
+                d = d*d + alt_d;
+            else
+                d = std::abs(d) + alt_d;
+            // subtree prunning
+            if( ncount == K && d > dist[ncount-1] )
+                continue;
+            // add alternative subtree to the priority queue
+            pqueue[qsize] = PQueueElem(d, alt);
+            for( i = qsize; i > 0; )
+            {
+                int parent = (i-1)/2;
+                if( parent < 0 || pqueue[parent].dist <= d )
+                    break;
+                std::swap(pqueue[i], pqueue[parent]);
+                i = parent;
+            }
+            qsize += qsize+1 < maxqsize;
+        }
+    }
+
+    K = std::min(K, ncount);
+    if( _neighborsIdx.needed() )
+    {
+        _neighborsIdx.create(K, 1, CV_32S, -1, true);
+        Mat nidx = _neighborsIdx.getMat();
+        Mat(nidx.size(), CV_32S, &idx[0]).copyTo(nidx);
+    }
+    if( _dist.needed() )
+        sqrt(Mat(K, 1, CV_32F, dist), _dist);
+
+    if( _neighbors.needed() || _labels.needed() )
+        getPoints(Mat(K, 1, CV_32S, idx), _neighbors, _labels);
+    return K;
+}
+
+
+void KDTree::findOrthoRange(InputArray _lowerBound,
+                            InputArray _upperBound,
+                            OutputArray _neighborsIdx,
+                            OutputArray _neighbors,
+                            OutputArray _labels ) const
+{
+    int dims = points.cols;
+    Mat lowerBound = _lowerBound.getMat(), upperBound = _upperBound.getMat();
+    CV_Assert( lowerBound.size == upperBound.size &&
+               lowerBound.isContinuous() &&
+               upperBound.isContinuous() &&
+               lowerBound.type() == upperBound.type() &&
+               lowerBound.type() == CV_32F &&
+               lowerBound.total() == (size_t)dims );
+    const float* L = lowerBound.ptr<float>();
+    const float* R = upperBound.ptr<float>();
+    
+    vector<int> idx;
+    AutoBuffer<int> _stack(MAX_TREE_DEPTH*2 + 1);
+    int* stack = _stack;
+    int top = 0;
+    
+    stack[top++] = 0;
+
+    while( --top >= 0 )
+    {
+        int nidx = stack[top];
+        if( nidx < 0 )
+            break;
+        const Node& n = nodes[nidx];
+        if( n.idx < 0 )
+        {
+            int j, i = ~n.idx;
+            const float* row = points.ptr<float>(i);
+            for( j = 0; j < dims; j++ )
+                if( row[j] < L[j] || row[j] >= R[j] )
+                    break;
+            if( j == dims )
+                idx.push_back(i);
+            continue;
+        }
+        if( L[n.idx] <= n.boundary )
+            stack[top++] = n.left;
+        if( R[n.idx] > n.boundary )
+            stack[top++] = n.right;
+    }
+
+    if( _neighborsIdx.needed() )
+    {
+        _neighborsIdx.create((int)idx.size(), 1, CV_32S, -1, true);
+        Mat nidx = _neighborsIdx.getMat();
+        Mat(nidx.size(), CV_32S, &idx[0]).copyTo(nidx);
+    }
+    getPoints( idx, _neighbors, _labels );
+}
+
+    
+void KDTree::getPoints(InputArray _idx, OutputArray _pts, OutputArray _labels) const
+{
+    Mat idxmat = _idx.getMat(), pts, labelsmat;
+    CV_Assert( idxmat.isContinuous() && idxmat.type() == CV_32S &&
+               (idxmat.cols == 1 || idxmat.rows == 1) );
+    const int* idx = idxmat.ptr<int>();
+    int* dstlabels = 0;
+    
+    int dims = points.cols;
+    int i, nidx = (int)idxmat.total();
+    if( nidx == 0 )
+    {
+        _pts.release();
+        _labels.release();
+        return;
+    }
+    
+    if( _pts.needed() )
+    {
+        _pts.create( nidx, dims, points.type());
+        pts = _pts.getMat();
+    }
+    
+    if(_labels.needed())
+    {
+        _labels.create(nidx, 1, CV_32S, -1, true);
+        labelsmat = _labels.getMat();
+        CV_Assert( labelsmat.isContinuous() );
+        dstlabels = labelsmat.ptr<int>();
+    }
+    const int* srclabels = !labels.empty() ? &labels[0] : 0;
+    
+    for( i = 0; i < nidx; i++ )
+    {
+        int k = idx[i];
+        CV_Assert( (unsigned)k < (unsigned)points.rows );
+        const float* src = points.ptr<float>(k);
+        if( pts.data )
+            std::copy(src, src + dims, pts.ptr<float>(i));
+        if( dstlabels )
+            dstlabels[i] = srclabels ? srclabels[k] : k;
+    }
+}
+
+
+const float* KDTree::getPoint(int ptidx, int* label) const
+{
+    CV_Assert( (unsigned)ptidx < (unsigned)points.rows);
+    if(label)
+        *label = labels[ptidx];
+    return points.ptr<float>(ptidx);
+}
+
+
+int KDTree::dims() const
+{
+    return !points.empty() ? points.cols : 0;
+}
+    
+////////////////////////////////////////////////////////////////////////////////
+    
+schar*  seqPush( CvSeq* seq, const void* element )
+{
+    return cvSeqPush(seq, element);
+}
+
+schar*  seqPushFront( CvSeq* seq, const void* element )
+{
+    return cvSeqPushFront(seq, element);
+}
+
+void  seqPop( CvSeq* seq, void* element )
+{
+    cvSeqPop(seq, element);
+}
+
+void  seqPopFront( CvSeq* seq, void* element )
+{
+    cvSeqPopFront(seq, element);
+}
+
+void  seqRemove( CvSeq* seq, int index )
+{
+    cvSeqRemove(seq, index);
+}
+
+void  clearSeq( CvSeq* seq )
+{
+    cvClearSeq(seq);
+}
+
+schar*  getSeqElem( const CvSeq* seq, int index )
+{
+    return cvGetSeqElem(seq, index);
+}
+
+void  seqRemoveSlice( CvSeq* seq, CvSlice slice )
+{
+    return cvSeqRemoveSlice(seq, slice);
+}
+
+void  seqInsertSlice( CvSeq* seq, int before_index, const CvArr* from_arr )
+{
+    cvSeqInsertSlice(seq, before_index, from_arr);
+}
+
+}
+
+/* End of file. */