datastructs.cpp

00001 /*M///////////////////////////////////////////////////////////////////////////////////////
00002 //
00003 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
00004 //
00005 //  By downloading, copying, installing or using the software you agree to this license.
00006 //  If you do not agree to this license, do not download, install,
00007 //  copy or use the software.
00008 //
00009 //
00010 //                        Intel License Agreement
00011 //                For Open Source Computer Vision Library
00012 //
00013 // Copyright (C) 2000, Intel Corporation, all rights reserved.
00014 // Third party copyrights are property of their respective owners.
00015 //
00016 // Redistribution and use in source and binary forms, with or without modification,
00017 // are permitted provided that the following conditions are met:
00018 //
00019 //   * Redistribution's of source code must retain the above copyright notice,
00020 //     this list of conditions and the following disclaimer.
00021 //
00022 //   * Redistribution's in binary form must reproduce the above copyright notice,
00023 //     this list of conditions and the following disclaimer in the documentation
00024 //     and/or other materials provided with the distribution.
00025 //
00026 //   * The name of Intel Corporation may not be used to endorse or promote products
00027 //     derived from this software without specific prior written permission.
00028 //
00029 // This software is provided by the copyright holders and contributors "as is" and
00030 // any express or implied warranties, including, but not limited to, the implied
00031 // warranties of merchantability and fitness for a particular purpose are disclaimed.
00032 // In no event shall the Intel Corporation or contributors be liable for any direct,
00033 // indirect, incidental, special, exemplary, or consequential damages
00034 // (including, but not limited to, procurement of substitute goods or services;
00035 // loss of use, data, or profits; or business interruption) however caused
00036 // and on any theory of liability, whether in contract, strict liability,
00037 // or tort (including negligence or otherwise) arising in any way out of
00038 // the use of this software, even if advised of the possibility of such damage.
00039 //
00040 //M*/
00041 #include "precomp.hpp"
00042 
00043 /* default alignment for dynamic data strucutures, resided in storages. */
00044 #define  CV_STRUCT_ALIGN    ((int)sizeof(double))
00045 
00046 /* default storage block size */
00047 #define  CV_STORAGE_BLOCK_SIZE   ((1<<16) - 128)
00048 
00049 #define ICV_FREE_PTR(storage)  \
00050     ((schar*)(storage)->top + (storage)->block_size - (storage)->free_space)
00051 
00052 #define ICV_ALIGNED_SEQ_BLOCK_SIZE  \
00053     (int)cvAlign(sizeof(CvSeqBlock), CV_STRUCT_ALIGN)
00054 
00055 CV_INLINE int
00056 cvAlignLeft( int size, int align )
00057 {
00058     return size & -align;
00059 }
00060 
00061 #define CV_GET_LAST_ELEM( seq, block ) \
00062     ((block)->data + ((block)->count - 1)*((seq)->elem_size))
00063 
00064 #define CV_SWAP_ELEMS(a,b,elem_size)  \
00065 {                                     \
00066     int k;                            \
00067     for( k = 0; k < elem_size; k++ )  \
00068     {                                 \
00069         char t0 = (a)[k];             \
00070         char t1 = (b)[k];             \
00071         (a)[k] = t1;                  \
00072         (b)[k] = t0;                  \
00073     }                                 \
00074 }
00075 
00076 #define ICV_SHIFT_TAB_MAX 32
00077 static const schar icvPower2ShiftTab[] =
00078 {
00079     0, 1, -1, 2, -1, -1, -1, 3, -1, -1, -1, -1, -1, -1, -1, 4,
00080     -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 5
00081 };
00082 
00083 /****************************************************************************************\
00084 *            Functions for manipulating memory storage - list of memory blocks           *
00085 \****************************************************************************************/
00086 
00087 /* Initialize allocated storage: */
00088 static void
00089 icvInitMemStorage( CvMemStorage* storage, int block_size )
00090 {
00091     if( !storage )
00092         CV_Error( CV_StsNullPtr, "" );
00093 
00094     if( block_size <= 0 )
00095         block_size = CV_STORAGE_BLOCK_SIZE;
00096 
00097     block_size = cvAlign( block_size, CV_STRUCT_ALIGN );
00098     assert( sizeof(CvMemBlock) % CV_STRUCT_ALIGN == 0 );
00099 
00100     memset( storage, 0, sizeof( *storage ));
00101     storage->signature = CV_STORAGE_MAGIC_VAL;
00102     storage->block_size = block_size;
00103 }
00104 
00105 
00106 /* Create root memory storage: */
00107 CV_IMPL CvMemStorage*
00108 cvCreateMemStorage( int block_size )
00109 {
00110     CvMemStorage* storage = (CvMemStorage *)cvAlloc( sizeof( CvMemStorage ));
00111     icvInitMemStorage( storage, block_size );
00112     return storage;
00113 }
00114 
00115 
00116 /* Create child memory storage: */
00117 CV_IMPL CvMemStorage *
00118 cvCreateChildMemStorage( CvMemStorage * parent )
00119 {
00120     if( !parent )
00121         CV_Error( CV_StsNullPtr, "" );
00122 
00123     CvMemStorage* storage = cvCreateMemStorage(parent->block_size);
00124     storage->parent = parent;
00125 
00126     return storage;
00127 }
00128 
00129 
00130 /* Release all blocks of the storage (or return them to parent, if any): */
00131 static void
00132 icvDestroyMemStorage( CvMemStorage* storage )
00133 {
00134     int k = 0;
00135 
00136     CvMemBlock *block;
00137     CvMemBlock *dst_top = 0;
00138 
00139     if( !storage )
00140         CV_Error( CV_StsNullPtr, "" );
00141 
00142     if( storage->parent )
00143         dst_top = storage->parent->top;
00144 
00145     for( block = storage->bottom; block != 0; k++ )
00146     {
00147         CvMemBlock *temp = block;
00148 
00149         block = block->next;
00150         if( storage->parent )
00151         {
00152             if( dst_top )
00153             {
00154                 temp->prev = dst_top;
00155                 temp->next = dst_top->next;
00156                 if( temp->next )
00157                     temp->next->prev = temp;
00158                 dst_top = dst_top->next = temp;
00159             }
00160             else
00161             {
00162                 dst_top = storage->parent->bottom = storage->parent->top = temp;
00163                 temp->prev = temp->next = 0;
00164                 storage->free_space = storage->block_size - sizeof( *temp );
00165             }
00166         }
00167         else
00168         {
00169             cvFree( &temp );
00170         }
00171     }
00172 
00173     storage->top = storage->bottom = 0;
00174     storage->free_space = 0;
00175 }
00176 
00177 
00178 /* Release memory storage: */
00179 CV_IMPL void
00180 cvReleaseMemStorage( CvMemStorage** storage )
00181 {
00182     if( !storage )
00183         CV_Error( CV_StsNullPtr, "" );
00184 
00185     CvMemStorage* st = *storage;
00186     *storage = 0;
00187     if( st )
00188     {
00189         icvDestroyMemStorage( st );
00190         cvFree( &st );
00191     }
00192 }
00193 
00194 
00195 /* Clears memory storage (return blocks to the parent, if any): */
00196 CV_IMPL void
00197 cvClearMemStorage( CvMemStorage * storage )
00198 {
00199     if( !storage )
00200         CV_Error( CV_StsNullPtr, "" );
00201 
00202     if( storage->parent )
00203         icvDestroyMemStorage( storage );
00204     else
00205     {
00206         storage->top = storage->bottom;
00207         storage->free_space = storage->bottom ? storage->block_size - sizeof(CvMemBlock) : 0;
00208     }
00209 }
00210 
00211 
00212 /* Moves stack pointer to next block.
00213    If no blocks, allocate new one and link it to the storage: */
00214 static void
00215 icvGoNextMemBlock( CvMemStorage * storage )
00216 {
00217     if( !storage )
00218         CV_Error( CV_StsNullPtr, "" );
00219 
00220     if( !storage->top || !storage->top->next )
00221     {
00222         CvMemBlock *block;
00223 
00224         if( !(storage->parent) )
00225         {
00226             block = (CvMemBlock *)cvAlloc( storage->block_size );
00227         }
00228         else
00229         {
00230             CvMemStorage *parent = storage->parent;
00231             CvMemStoragePos parent_pos;
00232 
00233             cvSaveMemStoragePos( parent, &parent_pos );
00234             icvGoNextMemBlock( parent );
00235 
00236             block = parent->top;
00237             cvRestoreMemStoragePos( parent, &parent_pos );
00238 
00239             if( block == parent->top )  /* the single allocated block */
00240             {
00241                 assert( parent->bottom == block );
00242                 parent->top = parent->bottom = 0;
00243                 parent->free_space = 0;
00244             }
00245             else
00246             {
00247                 /* cut the block from the parent's list of blocks */
00248                 parent->top->next = block->next;
00249                 if( block->next )
00250                     block->next->prev = parent->top;
00251             }
00252         }
00253 
00254         /* link block */
00255         block->next = 0;
00256         block->prev = storage->top;
00257 
00258         if( storage->top )
00259             storage->top->next = block;
00260         else
00261             storage->top = storage->bottom = block;
00262     }
00263 
00264     if( storage->top->next )
00265         storage->top = storage->top->next;
00266     storage->free_space = storage->block_size - sizeof(CvMemBlock);
00267     assert( storage->free_space % CV_STRUCT_ALIGN == 0 );
00268 }
00269 
00270 
00271 /* Remember memory storage position: */
00272 CV_IMPL void
00273 cvSaveMemStoragePos( const CvMemStorage * storage, CvMemStoragePos * pos )
00274 {
00275     if( !storage || !pos )
00276         CV_Error( CV_StsNullPtr, "" );
00277 
00278     pos->top = storage->top;
00279     pos->free_space = storage->free_space;
00280 }
00281 
00282 
00283 /* Restore memory storage position: */
00284 CV_IMPL void
00285 cvRestoreMemStoragePos( CvMemStorage * storage, CvMemStoragePos * pos )
00286 {
00287     if( !storage || !pos )
00288         CV_Error( CV_StsNullPtr, "" );
00289     if( pos->free_space > storage->block_size )
00290         CV_Error( CV_StsBadSize, "" );
00291 
00292     /*
00293     // this breaks icvGoNextMemBlock, so comment it off for now
00294     if( storage->parent && (!pos->top || pos->top->next) )
00295     {
00296         CvMemBlock* save_bottom;
00297         if( !pos->top )
00298             save_bottom = 0;
00299         else
00300         {
00301             save_bottom = storage->bottom;
00302             storage->bottom = pos->top->next;
00303             pos->top->next = 0;
00304             storage->bottom->prev = 0;
00305         }
00306         icvDestroyMemStorage( storage );
00307         storage->bottom = save_bottom;
00308     }*/
00309 
00310     storage->top = pos->top;
00311     storage->free_space = pos->free_space;
00312 
00313     if( !storage->top )
00314     {
00315         storage->top = storage->bottom;
00316         storage->free_space = storage->top ? storage->block_size - sizeof(CvMemBlock) : 0;
00317     }
00318 }
00319 
00320 
00321 /* Allocate continuous buffer of the specified size in the storage: */
00322 CV_IMPL void*
00323 cvMemStorageAlloc( CvMemStorage* storage, size_t size )
00324 {
00325     schar *ptr = 0;
00326     if( !storage )
00327         CV_Error( CV_StsNullPtr, "NULL storage pointer" );
00328 
00329     if( size > INT_MAX )
00330         CV_Error( CV_StsOutOfRange, "Too large memory block is requested" );
00331 
00332     assert( storage->free_space % CV_STRUCT_ALIGN == 0 );
00333 
00334     if( (size_t)storage->free_space < size )
00335     {
00336         size_t max_free_space = cvAlignLeft(storage->block_size - sizeof(CvMemBlock), CV_STRUCT_ALIGN);
00337         if( max_free_space < size )
00338             CV_Error( CV_StsOutOfRange, "requested size is negative or too big" );
00339 
00340         icvGoNextMemBlock( storage );
00341     }
00342 
00343     ptr = ICV_FREE_PTR(storage);
00344     assert( (size_t)ptr % CV_STRUCT_ALIGN == 0 );
00345     storage->free_space = cvAlignLeft(storage->free_space - (int)size, CV_STRUCT_ALIGN );
00346 
00347     return ptr;
00348 }
00349 
00350 
00351 CV_IMPL CvString
00352 cvMemStorageAllocString( CvMemStorage* storage, const char* ptr, int len )
00353 {
00354     CvString str;
00355     memset(&str, 0, sizeof(CvString));
00356 
00357     str.len = len >= 0 ? len : (int)strlen(ptr);
00358     str.ptr = (char*)cvMemStorageAlloc( storage, str.len + 1 );
00359     memcpy( str.ptr, ptr, str.len );
00360     str.ptr[str.len] = '\0';
00361 
00362     return str;
00363 }
00364 
00365 
00366 /****************************************************************************************\
00367 *                               Sequence implementation                                  *
00368 \****************************************************************************************/
00369 
00370 /* Create empty sequence: */
00371 CV_IMPL CvSeq *
00372 cvCreateSeq( int seq_flags, size_t header_size, size_t elem_size, CvMemStorage* storage )
00373 {
00374     CvSeq *seq = 0;
00375 
00376     if( !storage )
00377         CV_Error( CV_StsNullPtr, "" );
00378     if( header_size < sizeof( CvSeq ) || elem_size <= 0 )
00379         CV_Error( CV_StsBadSize, "" );
00380 
00381     /* allocate sequence header */
00382     seq = (CvSeq*)cvMemStorageAlloc( storage, header_size );
00383     memset( seq, 0, header_size );
00384 
00385     seq->header_size = (int)header_size;
00386     seq->flags = (seq_flags & ~CV_MAGIC_MASK) | CV_SEQ_MAGIC_VAL;
00387     {
00388         int elemtype = CV_MAT_TYPE(seq_flags);
00389         int typesize = CV_ELEM_SIZE(elemtype);
00390 
00391         if( elemtype != CV_SEQ_ELTYPE_GENERIC && elemtype != CV_USRTYPE1 &&
00392             typesize != 0 && typesize != (int)elem_size )
00393             CV_Error( CV_StsBadSize,
00394             "Specified element size doesn't match to the size of the specified element type "
00395             "(try to use 0 for element type)" );
00396     }
00397     seq->elem_size = (int)elem_size;
00398     seq->storage = storage;
00399 
00400     cvSetSeqBlockSize( seq, (int)((1 << 10)/elem_size) );
00401 
00402     return seq;
00403 }
00404 
00405 
00406 /* adjusts <delta_elems> field of sequence. It determines how much the sequence
00407    grows if there are no free space inside the sequence buffers */
00408 CV_IMPL void
00409 cvSetSeqBlockSize( CvSeq *seq, int delta_elements )
00410 {
00411     int elem_size;
00412     int useful_block_size;
00413 
00414     if( !seq || !seq->storage )
00415         CV_Error( CV_StsNullPtr, "" );
00416     if( delta_elements < 0 )
00417         CV_Error( CV_StsOutOfRange, "" );
00418 
00419     useful_block_size = cvAlignLeft(seq->storage->block_size - sizeof(CvMemBlock) -
00420                                     sizeof(CvSeqBlock), CV_STRUCT_ALIGN);
00421     elem_size = seq->elem_size;
00422 
00423     if( delta_elements == 0 )
00424     {
00425         delta_elements = (1 << 10) / elem_size;
00426         delta_elements = MAX( delta_elements, 1 );
00427     }
00428     if( delta_elements * elem_size > useful_block_size )
00429     {
00430         delta_elements = useful_block_size / elem_size;
00431         if( delta_elements == 0 )
00432             CV_Error( CV_StsOutOfRange, "Storage block size is too small "
00433                                         "to fit the sequence elements" );
00434     }
00435 
00436     seq->delta_elems = delta_elements;
00437 }
00438 
00439 
00440 /* Find a sequence element by its index: */
00441 CV_IMPL schar*
00442 cvGetSeqElem( const CvSeq *seq, int index )
00443 {
00444     CvSeqBlock *block;
00445     int count, total = seq->total;
00446 
00447     if( (unsigned)index >= (unsigned)total )
00448     {
00449         index += index < 0 ? total : 0;
00450         index -= index >= total ? total : 0;
00451         if( (unsigned)index >= (unsigned)total )
00452             return 0;
00453     }
00454 
00455     block = seq->first;
00456     if( index + index <= total )
00457     {
00458         while( index >= (count = block->count) )
00459         {
00460             block = block->next;
00461             index -= count;
00462         }
00463     }
00464     else
00465     {
00466         do
00467         {
00468             block = block->prev;
00469             total -= block->count;
00470         }
00471         while( index < total );
00472         index -= total;
00473     }
00474 
00475     return block->data + index * seq->elem_size;
00476 }
00477 
00478 
00479 /* Calculate index of a sequence element: */
00480 CV_IMPL int
00481 cvSeqElemIdx( const CvSeq* seq, const void* _element, CvSeqBlock** _block )
00482 {
00483     const schar *element = (const schar *)_element;
00484     int elem_size;
00485     int id = -1;
00486     CvSeqBlock *first_block;
00487     CvSeqBlock *block;
00488 
00489     if( !seq || !element )
00490         CV_Error( CV_StsNullPtr, "" );
00491 
00492     block = first_block = seq->first;
00493     elem_size = seq->elem_size;
00494 
00495     for( ;; )
00496     {
00497         if( (unsigned)(element - block->data) < (unsigned) (block->count * elem_size) )
00498         {
00499             if( _block )
00500                 *_block = block;
00501             if( elem_size <= ICV_SHIFT_TAB_MAX && (id = icvPower2ShiftTab[elem_size - 1]) >= 0 )
00502                 id = (int)((size_t)(element - block->data) >> id);
00503             else
00504                 id = (int)((size_t)(element - block->data) / elem_size);
00505             id += block->start_index - seq->first->start_index;
00506             break;
00507         }
00508         block = block->next;
00509         if( block == first_block )
00510             break;
00511     }
00512 
00513     return id;
00514 }
00515 
00516 
00517 CV_IMPL int
00518 cvSliceLength( CvSlice slice, const CvSeq* seq )
00519 {
00520     int total = seq->total;
00521     int length = slice.end_index - slice.start_index;
00522 
00523     if( length != 0 )
00524     {
00525         if( slice.start_index < 0 )
00526             slice.start_index += total;
00527         if( slice.end_index <= 0 )
00528             slice.end_index += total;
00529 
00530         length = slice.end_index - slice.start_index;
00531     }
00532 
00533     while( length < 0 )
00534         length += total;
00535     if( length > total )
00536         length = total;
00537 
00538     return length;
00539 }
00540 
00541 
00542 /* Copy all sequence elements into single continuous array: */
00543 CV_IMPL void*
00544 cvCvtSeqToArray( const CvSeq *seq, void *array, CvSlice slice )
00545 {
00546     int elem_size, total;
00547     CvSeqReader reader;
00548     char *dst = (char*)array;
00549 
00550     if( !seq || !array )
00551         CV_Error( CV_StsNullPtr, "" );
00552 
00553     elem_size = seq->elem_size;
00554     total = cvSliceLength( slice, seq )*elem_size;
00555 
00556     if( total == 0 )
00557         return 0;
00558 
00559     cvStartReadSeq( seq, &reader, 0 );
00560     cvSetSeqReaderPos( &reader, slice.start_index, 0 );
00561 
00562     do
00563     {
00564         int count = (int)(reader.block_max - reader.ptr);
00565         if( count > total )
00566             count = total;
00567 
00568         memcpy( dst, reader.ptr, count );
00569         dst += count;
00570         reader.block = reader.block->next;
00571         reader.ptr = reader.block->data;
00572         reader.block_max = reader.ptr + reader.block->count*elem_size;
00573         total -= count;
00574     }
00575     while( total > 0 );
00576 
00577     return array;
00578 }
00579 
00580 
00581 /* Construct a sequence from an array without copying any data.
00582    NB: The resultant sequence cannot grow beyond its initial size: */
00583 CV_IMPL CvSeq*
00584 cvMakeSeqHeaderForArray( int seq_flags, int header_size, int elem_size,
00585                          void *array, int total, CvSeq *seq, CvSeqBlock * block )
00586 {
00587     CvSeq* result = 0;
00588 
00589     if( elem_size <= 0 || header_size < (int)sizeof( CvSeq ) || total < 0 )
00590         CV_Error( CV_StsBadSize, "" );
00591 
00592     if( !seq || ((!array || !block) && total > 0) )
00593         CV_Error( CV_StsNullPtr, "" );
00594 
00595     memset( seq, 0, header_size );
00596 
00597     seq->header_size = header_size;
00598     seq->flags = (seq_flags & ~CV_MAGIC_MASK) | CV_SEQ_MAGIC_VAL;
00599     {
00600         int elemtype = CV_MAT_TYPE(seq_flags);
00601         int typesize = CV_ELEM_SIZE(elemtype);
00602 
00603         if( elemtype != CV_SEQ_ELTYPE_GENERIC &&
00604             typesize != 0 && typesize != elem_size )
00605             CV_Error( CV_StsBadSize,
00606             "Element size doesn't match to the size of predefined element type "
00607             "(try to use 0 for sequence element type)" );
00608     }
00609     seq->elem_size = elem_size;
00610     seq->total = total;
00611     seq->block_max = seq->ptr = (schar *) array + total * elem_size;
00612 
00613     if( total > 0 )
00614     {
00615         seq->first = block;
00616         block->prev = block->next = block;
00617         block->start_index = 0;
00618         block->count = total;
00619         block->data = (schar *) array;
00620     }
00621 
00622     result = seq;
00623 
00624     return result;
00625 }
00626 
00627 
00628 /* The function allocates space for at least one more sequence element.
00629    If there are free sequence blocks (seq->free_blocks != 0)
00630    they are reused, otherwise the space is allocated in the storage: */
00631 static void
00632 icvGrowSeq( CvSeq *seq, int in_front_of )
00633 {
00634     CvSeqBlock *block;
00635 
00636     if( !seq )
00637         CV_Error( CV_StsNullPtr, "" );
00638     block = seq->free_blocks;
00639 
00640     if( !block )
00641     {
00642         int elem_size = seq->elem_size;
00643         int delta_elems = seq->delta_elems;
00644         CvMemStorage *storage = seq->storage;
00645 
00646         if( seq->total >= delta_elems*4 )
00647             cvSetSeqBlockSize( seq, delta_elems*2 );
00648 
00649         if( !storage )
00650             CV_Error( CV_StsNullPtr, "The sequence has NULL storage pointer" );
00651 
00652         /* If there is a free space just after last allocated block
00653            and it is big enough then enlarge the last block.
00654            This can happen only if the new block is added to the end of sequence: */
00655         if( (size_t)(ICV_FREE_PTR(storage) - seq->block_max) < CV_STRUCT_ALIGN &&
00656             storage->free_space >= seq->elem_size && !in_front_of )
00657         {
00658             int delta = storage->free_space / elem_size;
00659 
00660             delta = MIN( delta, delta_elems ) * elem_size;
00661             seq->block_max += delta;
00662             storage->free_space = cvAlignLeft((int)(((schar*)storage->top + storage->block_size) -
00663                                               seq->block_max), CV_STRUCT_ALIGN );
00664             return;
00665         }
00666         else
00667         {
00668             int delta = elem_size * delta_elems + ICV_ALIGNED_SEQ_BLOCK_SIZE;
00669 
00670             /* Try to allocate <delta_elements> elements: */
00671             if( storage->free_space < delta )
00672             {
00673                 int small_block_size = MAX(1, delta_elems/3)*elem_size +
00674                                        ICV_ALIGNED_SEQ_BLOCK_SIZE;
00675                 /* try to allocate smaller part */
00676                 if( storage->free_space >= small_block_size + CV_STRUCT_ALIGN )
00677                 {
00678                     delta = (storage->free_space - ICV_ALIGNED_SEQ_BLOCK_SIZE)/seq->elem_size;
00679                     delta = delta*seq->elem_size + ICV_ALIGNED_SEQ_BLOCK_SIZE;
00680                 }
00681                 else
00682                 {
00683                     icvGoNextMemBlock( storage );
00684                     assert( storage->free_space >= delta );
00685                 }
00686             }
00687 
00688             block = (CvSeqBlock*)cvMemStorageAlloc( storage, delta );
00689             block->data = (schar*)cvAlignPtr( block + 1, CV_STRUCT_ALIGN );
00690             block->count = delta - ICV_ALIGNED_SEQ_BLOCK_SIZE;
00691             block->prev = block->next = 0;
00692         }
00693     }
00694     else
00695     {
00696         seq->free_blocks = block->next;
00697     }
00698 
00699     if( !(seq->first) )
00700     {
00701         seq->first = block;
00702         block->prev = block->next = block;
00703     }
00704     else
00705     {
00706         block->prev = seq->first->prev;
00707         block->next = seq->first;
00708         block->prev->next = block->next->prev = block;
00709     }
00710 
00711     /* For free blocks the <count> field means
00712      * total number of bytes in the block.
00713      *
00714      * For used blocks it means current number
00715      * of sequence elements in the block:
00716      */
00717     assert( block->count % seq->elem_size == 0 && block->count > 0 );
00718 
00719     if( !in_front_of )
00720     {
00721         seq->ptr = block->data;
00722         seq->block_max = block->data + block->count;
00723         block->start_index = block == block->prev ? 0 :
00724             block->prev->start_index + block->prev->count;
00725     }
00726     else
00727     {
00728         int delta = block->count / seq->elem_size;
00729         block->data += block->count;
00730 
00731         if( block != block->prev )
00732         {
00733             assert( seq->first->start_index == 0 );
00734             seq->first = block;
00735         }
00736         else
00737         {
00738             seq->block_max = seq->ptr = block->data;
00739         }
00740 
00741         block->start_index = 0;
00742 
00743         for( ;; )
00744         {
00745             block->start_index += delta;
00746             block = block->next;
00747             if( block == seq->first )
00748                 break;
00749         }
00750     }
00751 
00752     block->count = 0;
00753 }
00754 
00755 /* Recycle a sequence block: */
00756 static void
00757 icvFreeSeqBlock( CvSeq *seq, int in_front_of )
00758 {
00759     CvSeqBlock *block = seq->first;
00760 
00761     assert( (in_front_of ? block : block->prev)->count == 0 );
00762 
00763     if( block == block->prev )  /* single block case */
00764     {
00765         block->count = (int)(seq->block_max - block->data) + block->start_index * seq->elem_size;
00766         block->data = seq->block_max - block->count;
00767         seq->first = 0;
00768         seq->ptr = seq->block_max = 0;
00769         seq->total = 0;
00770     }
00771     else
00772     {
00773         if( !in_front_of )
00774         {
00775             block = block->prev;
00776             assert( seq->ptr == block->data );
00777 
00778             block->count = (int)(seq->block_max - seq->ptr);
00779             seq->block_max = seq->ptr = block->prev->data +
00780                 block->prev->count * seq->elem_size;
00781         }
00782         else
00783         {
00784             int delta = block->start_index;
00785 
00786             block->count = delta * seq->elem_size;
00787             block->data -= block->count;
00788 
00789             /* Update start indices of sequence blocks: */
00790             for( ;; )
00791             {
00792                 block->start_index -= delta;
00793                 block = block->next;
00794                 if( block == seq->first )
00795                     break;
00796             }
00797 
00798             seq->first = block->next;
00799         }
00800 
00801         block->prev->next = block->next;
00802         block->next->prev = block->prev;
00803     }
00804 
00805     assert( block->count > 0 && block->count % seq->elem_size == 0 );
00806     block->next = seq->free_blocks;
00807     seq->free_blocks = block;
00808 }
00809 
00810 
00811 /****************************************************************************************\
00812 *                             Sequence Writer implementation                             *
00813 \****************************************************************************************/
00814 
00815 /* Initialize sequence writer: */
00816 CV_IMPL void
00817 cvStartAppendToSeq( CvSeq *seq, CvSeqWriter * writer )
00818 {
00819     if( !seq || !writer )
00820         CV_Error( CV_StsNullPtr, "" );
00821 
00822     memset( writer, 0, sizeof( *writer ));
00823     writer->header_size = sizeof( CvSeqWriter );
00824 
00825     writer->seq = seq;
00826     writer->block = seq->first ? seq->first->prev : 0;
00827     writer->ptr = seq->ptr;
00828     writer->block_max = seq->block_max;
00829 }
00830 
00831 
00832 /* Initialize sequence writer: */
00833 CV_IMPL void
00834 cvStartWriteSeq( int seq_flags, int header_size,
00835                  int elem_size, CvMemStorage * storage, CvSeqWriter * writer )
00836 {
00837     if( !storage || !writer )
00838         CV_Error( CV_StsNullPtr, "" );
00839 
00840     CvSeq* seq = cvCreateSeq( seq_flags, header_size, elem_size, storage );
00841     cvStartAppendToSeq( seq, writer );
00842 }
00843 
00844 
00845 /* Update sequence header: */
00846 CV_IMPL void
00847 cvFlushSeqWriter( CvSeqWriter * writer )
00848 {
00849     if( !writer )
00850         CV_Error( CV_StsNullPtr, "" );
00851 
00852     CvSeq* seq = writer->seq;
00853     seq->ptr = writer->ptr;
00854 
00855     if( writer->block )
00856     {
00857         int total = 0;
00858         CvSeqBlock *first_block = writer->seq->first;
00859         CvSeqBlock *block = first_block;
00860 
00861         writer->block->count = (int)((writer->ptr - writer->block->data) / seq->elem_size);
00862         assert( writer->block->count > 0 );
00863 
00864         do
00865         {
00866             total += block->count;
00867             block = block->next;
00868         }
00869         while( block != first_block );
00870 
00871         writer->seq->total = total;
00872     }
00873 }
00874 
00875 
00876 /* Calls icvFlushSeqWriter and finishes writing process: */
00877 CV_IMPL CvSeq *
00878 cvEndWriteSeq( CvSeqWriter * writer )
00879 {
00880     if( !writer )
00881         CV_Error( CV_StsNullPtr, "" );
00882 
00883     cvFlushSeqWriter( writer );
00884     CvSeq* seq = writer->seq;
00885 
00886     /* Truncate the last block: */
00887     if( writer->block && writer->seq->storage )
00888     {
00889         CvMemStorage *storage = seq->storage;
00890         schar *storage_block_max = (schar *) storage->top + storage->block_size;
00891 
00892         assert( writer->block->count > 0 );
00893 
00894         if( (unsigned)((storage_block_max - storage->free_space)
00895             - seq->block_max) < CV_STRUCT_ALIGN )
00896         {
00897             storage->free_space = cvAlignLeft((int)(storage_block_max - seq->ptr), CV_STRUCT_ALIGN);
00898             seq->block_max = seq->ptr;
00899         }
00900     }
00901 
00902     writer->ptr = 0;
00903     return seq;
00904 }
00905 
00906 
00907 /* Create new sequence block: */
00908 CV_IMPL void
00909 cvCreateSeqBlock( CvSeqWriter * writer )
00910 {
00911     if( !writer || !writer->seq )
00912         CV_Error( CV_StsNullPtr, "" );
00913 
00914     CvSeq* seq = writer->seq;
00915 
00916     cvFlushSeqWriter( writer );
00917 
00918     icvGrowSeq( seq, 0 );
00919 
00920     writer->block = seq->first->prev;
00921     writer->ptr = seq->ptr;
00922     writer->block_max = seq->block_max;
00923 }
00924 
00925 
00926 /****************************************************************************************\
00927 *                               Sequence Reader implementation                           *
00928 \****************************************************************************************/
00929 
00930 /* Initialize sequence reader: */
00931 CV_IMPL void
00932 cvStartReadSeq( const CvSeq *seq, CvSeqReader * reader, int reverse )
00933 {
00934     CvSeqBlock *first_block;
00935     CvSeqBlock *last_block;
00936 
00937     if( reader )
00938     {
00939         reader->seq = 0;
00940         reader->block = 0;
00941         reader->ptr = reader->block_max = reader->block_min = 0;
00942     }
00943 
00944     if( !seq || !reader )
00945         CV_Error( CV_StsNullPtr, "" );
00946 
00947     reader->header_size = sizeof( CvSeqReader );
00948     reader->seq = (CvSeq*)seq;
00949 
00950     first_block = seq->first;
00951 
00952     if( first_block )
00953     {
00954         last_block = first_block->prev;
00955         reader->ptr = first_block->data;
00956         reader->prev_elem = CV_GET_LAST_ELEM( seq, last_block );
00957         reader->delta_index = seq->first->start_index;
00958 
00959         if( reverse )
00960         {
00961             schar *temp = reader->ptr;
00962 
00963             reader->ptr = reader->prev_elem;
00964             reader->prev_elem = temp;
00965 
00966             reader->block = last_block;
00967         }
00968         else
00969         {
00970             reader->block = first_block;
00971         }
00972 
00973         reader->block_min = reader->block->data;
00974         reader->block_max = reader->block_min + reader->block->count * seq->elem_size;
00975     }
00976     else
00977     {
00978         reader->delta_index = 0;
00979         reader->block = 0;
00980 
00981         reader->ptr = reader->prev_elem = reader->block_min = reader->block_max = 0;
00982     }
00983 }
00984 
00985 
00986 /* Change the current reading block
00987  * to the previous or to the next:
00988  */
00989 CV_IMPL void
00990 cvChangeSeqBlock( void* _reader, int direction )
00991 {
00992     CvSeqReader* reader = (CvSeqReader*)_reader;
00993 
00994     if( !reader )
00995         CV_Error( CV_StsNullPtr, "" );
00996 
00997     if( direction > 0 )
00998     {
00999         reader->block = reader->block->next;
01000         reader->ptr = reader->block->data;
01001     }
01002     else
01003     {
01004         reader->block = reader->block->prev;
01005         reader->ptr = CV_GET_LAST_ELEM( reader->seq, reader->block );
01006     }
01007     reader->block_min = reader->block->data;
01008     reader->block_max = reader->block_min + reader->block->count * reader->seq->elem_size;
01009 }
01010 
01011 
01012 /* Return the current reader position: */
01013 CV_IMPL int
01014 cvGetSeqReaderPos( CvSeqReader* reader )
01015 {
01016     int elem_size;
01017     int index = -1;
01018 
01019     if( !reader || !reader->ptr )
01020         CV_Error( CV_StsNullPtr, "" );
01021 
01022     elem_size = reader->seq->elem_size;
01023     if( elem_size <= ICV_SHIFT_TAB_MAX && (index = icvPower2ShiftTab[elem_size - 1]) >= 0 )
01024         index = (int)((reader->ptr - reader->block_min) >> index);
01025     else
01026         index = (int)((reader->ptr - reader->block_min) / elem_size);
01027 
01028     index += reader->block->start_index - reader->delta_index;
01029 
01030     return index;
01031 }
01032 
01033 
01034 /* Set reader position to given position,
01035  * either absolute or relative to the
01036  *  current one:
01037  */
01038 CV_IMPL void
01039 cvSetSeqReaderPos( CvSeqReader* reader, int index, int is_relative )
01040 {
01041     CvSeqBlock *block;
01042     int elem_size, count, total;
01043 
01044     if( !reader || !reader->seq )
01045         CV_Error( CV_StsNullPtr, "" );
01046 
01047     total = reader->seq->total;
01048     elem_size = reader->seq->elem_size;
01049 
01050     if( !is_relative )
01051     {
01052         if( index < 0 )
01053         {
01054             if( index < -total )
01055                 CV_Error( CV_StsOutOfRange, "" );
01056             index += total;
01057         }
01058         else if( index >= total )
01059         {
01060             index -= total;
01061             if( index >= total )
01062                 CV_Error( CV_StsOutOfRange, "" );
01063         }
01064 
01065         block = reader->seq->first;
01066         if( index >= (count = block->count) )
01067         {
01068             if( index + index <= total )
01069             {
01070                 do
01071                 {
01072                     block = block->next;
01073                     index -= count;
01074                 }
01075                 while( index >= (count = block->count) );
01076             }
01077             else
01078             {
01079                 do
01080                 {
01081                     block = block->prev;
01082                     total -= block->count;
01083                 }
01084                 while( index < total );
01085                 index -= total;
01086             }
01087         }
01088         reader->ptr = block->data + index * elem_size;
01089         if( reader->block != block )
01090         {
01091             reader->block = block;
01092             reader->block_min = block->data;
01093             reader->block_max = block->data + block->count * elem_size;
01094         }
01095     }
01096     else
01097     {
01098         schar* ptr = reader->ptr;
01099         index *= elem_size;
01100         block = reader->block;
01101 
01102         if( index > 0 )
01103         {
01104             while( ptr + index >= reader->block_max )
01105             {
01106                 int delta = (int)(reader->block_max - ptr);
01107                 index -= delta;
01108                 reader->block = block = block->next;
01109                 reader->block_min = ptr = block->data;
01110                 reader->block_max = block->data + block->count*elem_size;
01111             }
01112             reader->ptr = ptr + index;
01113         }
01114         else
01115         {
01116             while( ptr + index < reader->block_min )
01117             {
01118                 int delta = (int)(ptr - reader->block_min);
01119                 index += delta;
01120                 reader->block = block = block->prev;
01121                 reader->block_min = block->data;
01122                 reader->block_max = ptr = block->data + block->count*elem_size;
01123             }
01124             reader->ptr = ptr + index;
01125         }
01126     }
01127 }
01128 
01129 
01130 /* Push element onto the sequence: */
01131 CV_IMPL schar*
01132 cvSeqPush( CvSeq *seq, const void *element )
01133 {
01134     schar *ptr = 0;
01135     size_t elem_size;
01136 
01137     if( !seq )
01138         CV_Error( CV_StsNullPtr, "" );
01139 
01140     elem_size = seq->elem_size;
01141     ptr = seq->ptr;
01142 
01143     if( ptr >= seq->block_max )
01144     {
01145         icvGrowSeq( seq, 0 );
01146 
01147         ptr = seq->ptr;
01148         assert( ptr + elem_size <= seq->block_max /*&& ptr == seq->block_min */  );
01149     }
01150 
01151     if( element )
01152         memcpy( ptr, element, elem_size );
01153     seq->first->prev->count++;
01154     seq->total++;
01155     seq->ptr = ptr + elem_size;
01156 
01157     return ptr;
01158 }
01159 
01160 
01161 /* Pop last element off of the sequence: */
01162 CV_IMPL void
01163 cvSeqPop( CvSeq *seq, void *element )
01164 {
01165     schar *ptr;
01166     int elem_size;
01167 
01168     if( !seq )
01169         CV_Error( CV_StsNullPtr, "" );
01170     if( seq->total <= 0 )
01171         CV_Error( CV_StsBadSize, "" );
01172 
01173     elem_size = seq->elem_size;
01174     seq->ptr = ptr = seq->ptr - elem_size;
01175 
01176     if( element )
01177         memcpy( element, ptr, elem_size );
01178     seq->ptr = ptr;
01179     seq->total--;
01180 
01181     if( --(seq->first->prev->count) == 0 )
01182     {
01183         icvFreeSeqBlock( seq, 0 );
01184         assert( seq->ptr == seq->block_max );
01185     }
01186 }
01187 
01188 
01189 /* Push element onto the front of the sequence: */
01190 CV_IMPL schar*
01191 cvSeqPushFront( CvSeq *seq, const void *element )
01192 {
01193     schar* ptr = 0;
01194     int elem_size;
01195     CvSeqBlock *block;
01196 
01197     if( !seq )
01198         CV_Error( CV_StsNullPtr, "" );
01199 
01200     elem_size = seq->elem_size;
01201     block = seq->first;
01202 
01203     if( !block || block->start_index == 0 )
01204     {
01205         icvGrowSeq( seq, 1 );
01206 
01207         block = seq->first;
01208         assert( block->start_index > 0 );
01209     }
01210 
01211     ptr = block->data -= elem_size;
01212 
01213     if( element )
01214         memcpy( ptr, element, elem_size );
01215     block->count++;
01216     block->start_index--;
01217     seq->total++;
01218 
01219     return ptr;
01220 }
01221 
01222 
01223 /* Shift out first element of the sequence: */
01224 CV_IMPL void
01225 cvSeqPopFront( CvSeq *seq, void *element )
01226 {
01227     int elem_size;
01228     CvSeqBlock *block;
01229 
01230     if( !seq )
01231         CV_Error( CV_StsNullPtr, "" );
01232     if( seq->total <= 0 )
01233         CV_Error( CV_StsBadSize, "" );
01234 
01235     elem_size = seq->elem_size;
01236     block = seq->first;
01237 
01238     if( element )
01239         memcpy( element, block->data, elem_size );
01240     block->data += elem_size;
01241     block->start_index++;
01242     seq->total--;
01243 
01244     if( --(block->count) == 0 )
01245         icvFreeSeqBlock( seq, 1 );
01246 }
01247 
01248 /* Insert new element in middle of sequence: */
01249 CV_IMPL schar*
01250 cvSeqInsert( CvSeq *seq, int before_index, const void *element )
01251 {
01252     int elem_size;
01253     int block_size;
01254     CvSeqBlock *block;
01255     int delta_index;
01256     int total;
01257     schar* ret_ptr = 0;
01258 
01259     if( !seq )
01260         CV_Error( CV_StsNullPtr, "" );
01261 
01262     total = seq->total;
01263     before_index += before_index < 0 ? total : 0;
01264     before_index -= before_index > total ? total : 0;
01265 
01266     if( (unsigned)before_index > (unsigned)total )
01267         CV_Error( CV_StsOutOfRange, "" );
01268 
01269     if( before_index == total )
01270     {
01271         ret_ptr = cvSeqPush( seq, element );
01272     }
01273     else if( before_index == 0 )
01274     {
01275         ret_ptr = cvSeqPushFront( seq, element );
01276     }
01277     else
01278     {
01279         elem_size = seq->elem_size;
01280 
01281         if( before_index >= total >> 1 )
01282         {
01283             schar *ptr = seq->ptr + elem_size;
01284 
01285             if( ptr > seq->block_max )
01286             {
01287                 icvGrowSeq( seq, 0 );
01288 
01289                 ptr = seq->ptr + elem_size;
01290                 assert( ptr <= seq->block_max );
01291             }
01292 
01293             delta_index = seq->first->start_index;
01294             block = seq->first->prev;
01295             block->count++;
01296             block_size = (int)(ptr - block->data);
01297 
01298             while( before_index < block->start_index - delta_index )
01299             {
01300                 CvSeqBlock *prev_block = block->prev;
01301 
01302                 memmove( block->data + elem_size, block->data, block_size - elem_size );
01303                 block_size = prev_block->count * elem_size;
01304                 memcpy( block->data, prev_block->data + block_size - elem_size, elem_size );
01305                 block = prev_block;
01306 
01307                 /* Check that we don't fall into an infinite loop: */
01308                 assert( block != seq->first->prev );
01309             }
01310 
01311             before_index = (before_index - block->start_index + delta_index) * elem_size;
01312             memmove( block->data + before_index + elem_size, block->data + before_index,
01313                      block_size - before_index - elem_size );
01314 
01315             ret_ptr = block->data + before_index;
01316 
01317             if( element )
01318                 memcpy( ret_ptr, element, elem_size );
01319             seq->ptr = ptr;
01320         }
01321         else
01322         {
01323             block = seq->first;
01324 
01325             if( block->start_index == 0 )
01326             {
01327                 icvGrowSeq( seq, 1 );
01328 
01329                 block = seq->first;
01330             }
01331 
01332             delta_index = block->start_index;
01333             block->count++;
01334             block->start_index--;
01335             block->data -= elem_size;
01336 
01337             while( before_index > block->start_index - delta_index + block->count )
01338             {
01339                 CvSeqBlock *next_block = block->next;
01340 
01341                 block_size = block->count * elem_size;
01342                 memmove( block->data, block->data + elem_size, block_size - elem_size );
01343                 memcpy( block->data + block_size - elem_size, next_block->data, elem_size );
01344                 block = next_block;
01345 
01346                 /* Check that we don't fall into an infinite loop: */
01347                 assert( block != seq->first );
01348             }
01349 
01350             before_index = (before_index - block->start_index + delta_index) * elem_size;
01351             memmove( block->data, block->data + elem_size, before_index - elem_size );
01352 
01353             ret_ptr = block->data + before_index - elem_size;
01354 
01355             if( element )
01356                 memcpy( ret_ptr, element, elem_size );
01357         }
01358 
01359         seq->total = total + 1;
01360     }
01361 
01362     return ret_ptr;
01363 }
01364 
01365 
01366 /* Removes element from sequence: */
01367 CV_IMPL void
01368 cvSeqRemove( CvSeq *seq, int index )
01369 {
01370     schar *ptr;
01371     int elem_size;
01372     int block_size;
01373     CvSeqBlock *block;
01374     int delta_index;
01375     int total, front = 0;
01376 
01377     if( !seq )
01378         CV_Error( CV_StsNullPtr, "" );
01379 
01380     total = seq->total;
01381 
01382     index += index < 0 ? total : 0;
01383     index -= index >= total ? total : 0;
01384 
01385     if( (unsigned) index >= (unsigned) total )
01386         CV_Error( CV_StsOutOfRange, "Invalid index" );
01387 
01388     if( index == total - 1 )
01389     {
01390         cvSeqPop( seq, 0 );
01391     }
01392     else if( index == 0 )
01393     {
01394         cvSeqPopFront( seq, 0 );
01395     }
01396     else
01397     {
01398         block = seq->first;
01399         elem_size = seq->elem_size;
01400         delta_index = block->start_index;
01401         while( block->start_index - delta_index + block->count <= index )
01402             block = block->next;
01403 
01404         ptr = block->data + (index - block->start_index + delta_index) * elem_size;
01405 
01406         front = index < total >> 1;
01407         if( !front )
01408         {
01409             block_size = block->count * elem_size - (int)(ptr - block->data);
01410 
01411             while( block != seq->first->prev )  /* while not the last block */
01412             {
01413                 CvSeqBlock *next_block = block->next;
01414 
01415                 memmove( ptr, ptr + elem_size, block_size - elem_size );
01416                 memcpy( ptr + block_size - elem_size, next_block->data, elem_size );
01417                 block = next_block;
01418                 ptr = block->data;
01419                 block_size = block->count * elem_size;
01420             }
01421 
01422             memmove( ptr, ptr + elem_size, block_size - elem_size );
01423             seq->ptr -= elem_size;
01424         }
01425         else
01426         {
01427             ptr += elem_size;
01428             block_size = (int)(ptr - block->data);
01429 
01430             while( block != seq->first )
01431             {
01432                 CvSeqBlock *prev_block = block->prev;
01433 
01434                 memmove( block->data + elem_size, block->data, block_size - elem_size );
01435                 block_size = prev_block->count * elem_size;
01436                 memcpy( block->data, prev_block->data + block_size - elem_size, elem_size );
01437                 block = prev_block;
01438             }
01439 
01440             memmove( block->data + elem_size, block->data, block_size - elem_size );
01441             block->data += elem_size;
01442             block->start_index++;
01443         }
01444 
01445         seq->total = total - 1;
01446         if( --block->count == 0 )
01447             icvFreeSeqBlock( seq, front );
01448     }
01449 }
01450 
01451 
01452 /* Add several elements to the beginning or end of a sequence: */
01453 CV_IMPL void
01454 cvSeqPushMulti( CvSeq *seq, const void *_elements, int count, int front )
01455 {
01456     char *elements = (char *) _elements;
01457 
01458     if( !seq )
01459         CV_Error( CV_StsNullPtr, "NULL sequence pointer" );
01460     if( count < 0 )
01461         CV_Error( CV_StsBadSize, "number of removed elements is negative" );
01462 
01463     int elem_size = seq->elem_size;
01464 
01465     if( !front )
01466     {
01467         while( count > 0 )
01468         {
01469             int delta = (int)((seq->block_max - seq->ptr) / elem_size);
01470 
01471             delta = MIN( delta, count );
01472             if( delta > 0 )
01473             {
01474                 seq->first->prev->count += delta;
01475                 seq->total += delta;
01476                 count -= delta;
01477                 delta *= elem_size;
01478                 if( elements )
01479                 {
01480                     memcpy( seq->ptr, elements, delta );
01481                     elements += delta;
01482                 }
01483                 seq->ptr += delta;
01484             }
01485 
01486             if( count > 0 )
01487                 icvGrowSeq( seq, 0 );
01488         }
01489     }
01490     else
01491     {
01492         CvSeqBlock* block = seq->first;
01493 
01494         while( count > 0 )
01495         {
01496             int delta;
01497 
01498             if( !block || block->start_index == 0 )
01499             {
01500                 icvGrowSeq( seq, 1 );
01501 
01502                 block = seq->first;
01503                 assert( block->start_index > 0 );
01504             }
01505 
01506             delta = MIN( block->start_index, count );
01507             count -= delta;
01508             block->start_index -= delta;
01509             block->count += delta;
01510             seq->total += delta;
01511             delta *= elem_size;
01512             block->data -= delta;
01513 
01514             if( elements )
01515                 memcpy( block->data, elements + count*elem_size, delta );
01516         }
01517     }
01518 }
01519 
01520 
01521 /* Remove several elements from the end of sequence: */
01522 CV_IMPL void
01523 cvSeqPopMulti( CvSeq *seq, void *_elements, int count, int front )
01524 {
01525     char *elements = (char *) _elements;
01526 
01527     if( !seq )
01528         CV_Error( CV_StsNullPtr, "NULL sequence pointer" );
01529     if( count < 0 )
01530         CV_Error( CV_StsBadSize, "number of removed elements is negative" );
01531 
01532     count = MIN( count, seq->total );
01533 
01534     if( !front )
01535     {
01536         if( elements )
01537             elements += count * seq->elem_size;
01538 
01539         while( count > 0 )
01540         {
01541             int delta = seq->first->prev->count;
01542 
01543             delta = MIN( delta, count );
01544             assert( delta > 0 );
01545 
01546             seq->first->prev->count -= delta;
01547             seq->total -= delta;
01548             count -= delta;
01549             delta *= seq->elem_size;
01550             seq->ptr -= delta;
01551 
01552             if( elements )
01553             {
01554                 elements -= delta;
01555                 memcpy( elements, seq->ptr, delta );
01556             }
01557 
01558             if( seq->first->prev->count == 0 )
01559                 icvFreeSeqBlock( seq, 0 );
01560         }
01561     }
01562     else
01563     {
01564         while( count > 0 )
01565         {
01566             int delta = seq->first->count;
01567 
01568             delta = MIN( delta, count );
01569             assert( delta > 0 );
01570 
01571             seq->first->count -= delta;
01572             seq->total -= delta;
01573             count -= delta;
01574             seq->first->start_index += delta;
01575             delta *= seq->elem_size;
01576 
01577             if( elements )
01578             {
01579                 memcpy( elements, seq->first->data, delta );
01580                 elements += delta;
01581             }
01582 
01583             seq->first->data += delta;
01584             if( seq->first->count == 0 )
01585                 icvFreeSeqBlock( seq, 1 );
01586         }
01587     }
01588 }
01589 
01590 
01591 /* Remove all elements from a sequence: */
01592 CV_IMPL void
01593 cvClearSeq( CvSeq *seq )
01594 {
01595     if( !seq )
01596         CV_Error( CV_StsNullPtr, "" );
01597     cvSeqPopMulti( seq, 0, seq->total );
01598 }
01599 
01600 
01601 CV_IMPL CvSeq*
01602 cvSeqSlice( const CvSeq* seq, CvSlice slice, CvMemStorage* storage, int copy_data )
01603 {
01604     CvSeq* subseq = 0;
01605     int elem_size, count, length;
01606     CvSeqReader reader;
01607     CvSeqBlock *block, *first_block = 0, *last_block = 0;
01608 
01609     if( !CV_IS_SEQ(seq) )
01610         CV_Error( CV_StsBadArg, "Invalid sequence header" );
01611 
01612     if( !storage )
01613     {
01614         storage = seq->storage;
01615         if( !storage )
01616             CV_Error( CV_StsNullPtr, "NULL storage pointer" );
01617     }
01618 
01619     elem_size = seq->elem_size;
01620     length = cvSliceLength( slice, seq );
01621     if( slice.start_index < 0 )
01622         slice.start_index += seq->total;
01623     else if( slice.start_index >= seq->total )
01624         slice.start_index -= seq->total;
01625     if( (unsigned)length > (unsigned)seq->total ||
01626         ((unsigned)slice.start_index >= (unsigned)seq->total && length != 0) )
01627         CV_Error( CV_StsOutOfRange, "Bad sequence slice" );
01628 
01629     subseq = cvCreateSeq( seq->flags, seq->header_size, elem_size, storage );
01630 
01631     if( length > 0 )
01632     {
01633         cvStartReadSeq( seq, &reader, 0 );
01634         cvSetSeqReaderPos( &reader, slice.start_index, 0 );
01635         count = (int)((reader.block_max - reader.ptr)/elem_size);
01636 
01637         do
01638         {
01639             int bl = MIN( count, length );
01640 
01641             if( !copy_data )
01642             {
01643                 block = (CvSeqBlock*)cvMemStorageAlloc( storage, sizeof(*block) );
01644                 if( !first_block )
01645                 {
01646                     first_block = subseq->first = block->prev = block->next = block;
01647                     block->start_index = 0;
01648                 }
01649                 else
01650                 {
01651                     block->prev = last_block;
01652                     block->next = first_block;
01653                     last_block->next = first_block->prev = block;
01654                     block->start_index = last_block->start_index + last_block->count;
01655                 }
01656                 last_block = block;
01657                 block->data = reader.ptr;
01658                 block->count = bl;
01659                 subseq->total += bl;
01660             }
01661             else
01662                 cvSeqPushMulti( subseq, reader.ptr, bl, 0 );
01663             length -= bl;
01664             reader.block = reader.block->next;
01665             reader.ptr = reader.block->data;
01666             count = reader.block->count;
01667         }
01668         while( length > 0 );
01669     }
01670 
01671     return subseq;
01672 }
01673 
01674 
01675 // Remove slice from the middle of the sequence.
01676 // !!! TODO !!! Implement more efficient algorithm
01677 CV_IMPL void
01678 cvSeqRemoveSlice( CvSeq* seq, CvSlice slice )
01679 {
01680     int total, length;
01681 
01682     if( !CV_IS_SEQ(seq) )
01683         CV_Error( CV_StsBadArg, "Invalid sequence header" );
01684 
01685     length = cvSliceLength( slice, seq );
01686     total = seq->total;
01687 
01688     if( slice.start_index < 0 )
01689         slice.start_index += total;
01690     else if( slice.start_index >= total )
01691         slice.start_index -= total;
01692 
01693     if( (unsigned)slice.start_index >= (unsigned)total )
01694         CV_Error( CV_StsOutOfRange, "start slice index is out of range" );
01695 
01696     slice.end_index = slice.start_index + length;
01697 
01698     if ( slice.start_index == slice.end_index )
01699         return;
01700 
01701     if( slice.end_index < total )
01702     {
01703         CvSeqReader reader_to, reader_from;
01704         int elem_size = seq->elem_size;
01705 
01706         cvStartReadSeq( seq, &reader_to );
01707         cvStartReadSeq( seq, &reader_from );
01708 
01709         if( slice.start_index > total - slice.end_index )
01710         {
01711             int i, count = seq->total - slice.end_index;
01712             cvSetSeqReaderPos( &reader_to, slice.start_index );
01713             cvSetSeqReaderPos( &reader_from, slice.end_index );
01714 
01715             for( i = 0; i < count; i++ )
01716             {
01717                 memcpy( reader_to.ptr, reader_from.ptr, elem_size );
01718                 CV_NEXT_SEQ_ELEM( elem_size, reader_to );
01719                 CV_NEXT_SEQ_ELEM( elem_size, reader_from );
01720             }
01721 
01722             cvSeqPopMulti( seq, 0, slice.end_index - slice.start_index );
01723         }
01724         else
01725         {
01726             int i, count = slice.start_index;
01727             cvSetSeqReaderPos( &reader_to, slice.end_index );
01728             cvSetSeqReaderPos( &reader_from, slice.start_index );
01729 
01730             for( i = 0; i < count; i++ )
01731             {
01732                 CV_PREV_SEQ_ELEM( elem_size, reader_to );
01733                 CV_PREV_SEQ_ELEM( elem_size, reader_from );
01734 
01735                 memcpy( reader_to.ptr, reader_from.ptr, elem_size );
01736             }
01737 
01738             cvSeqPopMulti( seq, 0, slice.end_index - slice.start_index, 1 );
01739         }
01740     }
01741     else
01742     {
01743         cvSeqPopMulti( seq, 0, total - slice.start_index );
01744         cvSeqPopMulti( seq, 0, slice.end_index - total, 1 );
01745     }
01746 }
01747 
01748 
01749 // Insert a sequence into the middle of another sequence:
01750 // !!! TODO !!! Implement more efficient algorithm
01751 CV_IMPL void
01752 cvSeqInsertSlice( CvSeq* seq, int index, const CvArr* from_arr )
01753 {
01754     CvSeqReader reader_to, reader_from;
01755     int i, elem_size, total, from_total;
01756     CvSeq from_header, *from = (CvSeq*)from_arr;
01757     CvSeqBlock block;
01758 
01759     if( !CV_IS_SEQ(seq) )
01760         CV_Error( CV_StsBadArg, "Invalid destination sequence header" );
01761 
01762     if( !CV_IS_SEQ(from))
01763     {
01764         CvMat* mat = (CvMat*)from;
01765         if( !CV_IS_MAT(mat))
01766             CV_Error( CV_StsBadArg, "Source is not a sequence nor matrix" );
01767 
01768         if( !CV_IS_MAT_CONT(mat->type) || (mat->rows != 1 && mat->cols != 1) )
01769             CV_Error( CV_StsBadArg, "The source array must be 1d coninuous vector" );
01770 
01771         from = cvMakeSeqHeaderForArray( CV_SEQ_KIND_GENERIC, sizeof(from_header),
01772                                                  CV_ELEM_SIZE(mat->type),
01773                                                  mat->data.ptr, mat->cols + mat->rows - 1,
01774                                                  &from_header, &block );
01775     }
01776 
01777     if( seq->elem_size != from->elem_size )
01778         CV_Error( CV_StsUnmatchedSizes,
01779         "Source and destination sequence element sizes are different." );
01780 
01781     from_total = from->total;
01782 
01783     if( from_total == 0 )
01784         return;
01785 
01786     total = seq->total;
01787     index += index < 0 ? total : 0;
01788     index -= index > total ? total : 0;
01789 
01790     if( (unsigned)index > (unsigned)total )
01791         CV_Error( CV_StsOutOfRange, "" );
01792 
01793     elem_size = seq->elem_size;
01794 
01795     if( index < (total >> 1) )
01796     {
01797         cvSeqPushMulti( seq, 0, from_total, 1 );
01798 
01799         cvStartReadSeq( seq, &reader_to );
01800         cvStartReadSeq( seq, &reader_from );
01801         cvSetSeqReaderPos( &reader_from, from_total );
01802 
01803         for( i = 0; i < index; i++ )
01804         {
01805             memcpy( reader_to.ptr, reader_from.ptr, elem_size );
01806             CV_NEXT_SEQ_ELEM( elem_size, reader_to );
01807             CV_NEXT_SEQ_ELEM( elem_size, reader_from );
01808         }
01809     }
01810     else
01811     {
01812         cvSeqPushMulti( seq, 0, from_total );
01813 
01814         cvStartReadSeq( seq, &reader_to );
01815         cvStartReadSeq( seq, &reader_from );
01816         cvSetSeqReaderPos( &reader_from, total );
01817         cvSetSeqReaderPos( &reader_to, seq->total );
01818 
01819         for( i = 0; i < total - index; i++ )
01820         {
01821             CV_PREV_SEQ_ELEM( elem_size, reader_to );
01822             CV_PREV_SEQ_ELEM( elem_size, reader_from );
01823             memcpy( reader_to.ptr, reader_from.ptr, elem_size );
01824         }
01825     }
01826 
01827     cvStartReadSeq( from, &reader_from );
01828     cvSetSeqReaderPos( &reader_to, index );
01829 
01830     for( i = 0; i < from_total; i++ )
01831     {
01832         memcpy( reader_to.ptr, reader_from.ptr, elem_size );
01833         CV_NEXT_SEQ_ELEM( elem_size, reader_to );
01834         CV_NEXT_SEQ_ELEM( elem_size, reader_from );
01835     }
01836 }
01837 
01838 // Sort the sequence using user-specified comparison function.
01839 // The semantics is similar to qsort() function.
01840 // The code is based on BSD system qsort():
01841 //    * Copyright (c) 1992, 1993
01842 //    *  The Regents of the University of California.  All rights reserved.
01843 //    *
01844 //    * Redistribution and use in source and binary forms, with or without
01845 //    * modification, are permitted provided that the following conditions
01846 //    * are met:
01847 //    * 1. Redistributions of source code must retain the above copyright
01848 //    *    notice, this list of conditions and the following disclaimer.
01849 //    * 2. Redistributions in binary form must reproduce the above copyright
01850 //    *    notice, this list of conditions and the following disclaimer in the
01851 //    *    documentation and/or other materials provided with the distribution.
01852 //    * 3. All advertising materials mentioning features or use of this software
01853 //    *    must display the following acknowledgement:
01854 //    *  This product includes software developed by the University of
01855 //    *  California, Berkeley and its contributors.
01856 //    * 4. Neither the name of the University nor the names of its contributors
01857 //    *    may be used to endorse or promote products derived from this software
01858 //    *    without specific prior written permission.
01859 //    *
01860 //    * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
01861 //    * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
01862 //    * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
01863 //    * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
01864 //    * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
01865 //    * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
01866 //    * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
01867 //    * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
01868 //    * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
01869 //    * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
01870 //    * SUCH DAMAGE.
01871 
01872 typedef struct CvSeqReaderPos
01873 {
01874     CvSeqBlock* block;
01875     schar* ptr;
01876     schar* block_min;
01877     schar* block_max;
01878 }
01879 CvSeqReaderPos;
01880 
01881 #define CV_SAVE_READER_POS( reader, pos )   \
01882 {                                           \
01883     (pos).block = (reader).block;           \
01884     (pos).ptr = (reader).ptr;               \
01885     (pos).block_min = (reader).block_min;   \
01886     (pos).block_max = (reader).block_max;   \
01887 }
01888 
01889 #define CV_RESTORE_READER_POS( reader, pos )\
01890 {                                           \
01891     (reader).block = (pos).block;           \
01892     (reader).ptr = (pos).ptr;               \
01893     (reader).block_min = (pos).block_min;   \
01894     (reader).block_max = (pos).block_max;   \
01895 }
01896 
01897 inline schar*
01898 icvMed3( schar* a, schar* b, schar* c, CvCmpFunc cmp_func, void* aux )
01899 {
01900     return cmp_func(a, b, aux) < 0 ?
01901       (cmp_func(b, c, aux) < 0 ? b : cmp_func(a, c, aux) < 0 ? c : a)
01902      :(cmp_func(b, c, aux) > 0 ? b : cmp_func(a, c, aux) < 0 ? a : c);
01903 }
01904 
01905 CV_IMPL void
01906 cvSeqSort( CvSeq* seq, CvCmpFunc cmp_func, void* aux )
01907 {
01908     int elem_size;
01909     int isort_thresh = 7;
01910     CvSeqReader left, right;
01911     int sp = 0;
01912 
01913     struct
01914     {
01915         CvSeqReaderPos lb;
01916         CvSeqReaderPos ub;
01917     }
01918     stack[48];
01919 
01920     if( !CV_IS_SEQ(seq) )
01921         CV_Error( !seq ? CV_StsNullPtr : CV_StsBadArg, "Bad input sequence" );
01922 
01923     if( !cmp_func )
01924         CV_Error( CV_StsNullPtr, "Null compare function" );
01925 
01926     if( seq->total <= 1 )
01927         return;
01928 
01929     elem_size = seq->elem_size;
01930     isort_thresh *= elem_size;
01931 
01932     cvStartReadSeq( seq, &left, 0 );
01933     right = left;
01934     CV_SAVE_READER_POS( left, stack[0].lb );
01935     CV_PREV_SEQ_ELEM( elem_size, right );
01936     CV_SAVE_READER_POS( right, stack[0].ub );
01937 
01938     while( sp >= 0 )
01939     {
01940         CV_RESTORE_READER_POS( left, stack[sp].lb );
01941         CV_RESTORE_READER_POS( right, stack[sp].ub );
01942         sp--;
01943 
01944         for(;;)
01945         {
01946             int i, n, m;
01947             CvSeqReader ptr, ptr2;
01948 
01949             if( left.block == right.block )
01950                 n = (int)(right.ptr - left.ptr) + elem_size;
01951             else
01952             {
01953                 n = cvGetSeqReaderPos( &right );
01954                 n = (n - cvGetSeqReaderPos( &left ) + 1)*elem_size;
01955             }
01956 
01957             if( n <= isort_thresh )
01958             {
01959             insert_sort:
01960                 ptr = ptr2 = left;
01961                 CV_NEXT_SEQ_ELEM( elem_size, ptr );
01962                 CV_NEXT_SEQ_ELEM( elem_size, right );
01963                 while( ptr.ptr != right.ptr )
01964                 {
01965                     ptr2.ptr = ptr.ptr;
01966                     if( ptr2.block != ptr.block )
01967                     {
01968                         ptr2.block = ptr.block;
01969                         ptr2.block_min = ptr.block_min;
01970                         ptr2.block_max = ptr.block_max;
01971                     }
01972                     while( ptr2.ptr != left.ptr )
01973                     {
01974                         schar* cur = ptr2.ptr;
01975                         CV_PREV_SEQ_ELEM( elem_size, ptr2 );
01976                         if( cmp_func( ptr2.ptr, cur, aux ) <= 0 )
01977                             break;
01978                         CV_SWAP_ELEMS( ptr2.ptr, cur, elem_size );
01979                     }
01980                     CV_NEXT_SEQ_ELEM( elem_size, ptr );
01981                 }
01982                 break;
01983             }
01984             else
01985             {
01986                 CvSeqReader left0, left1, right0, right1;
01987                 CvSeqReader tmp0, tmp1;
01988                 schar *m1, *m2, *m3, *pivot;
01989                 int swap_cnt = 0;
01990                 int l, l0, l1, r, r0, r1;
01991 
01992                 left0 = tmp0 = left;
01993                 right0 = right1 = right;
01994                 n /= elem_size;
01995 
01996                 if( n > 40 )
01997                 {
01998                     int d = n / 8;
01999                     schar *p1, *p2, *p3;
02000                     p1 = tmp0.ptr;
02001                     cvSetSeqReaderPos( &tmp0, d, 1 );
02002                     p2 = tmp0.ptr;
02003                     cvSetSeqReaderPos( &tmp0, d, 1 );
02004                     p3 = tmp0.ptr;
02005                     m1 = icvMed3( p1, p2, p3, cmp_func, aux );
02006                     cvSetSeqReaderPos( &tmp0, (n/2) - d*3, 1 );
02007                     p1 = tmp0.ptr;
02008                     cvSetSeqReaderPos( &tmp0, d, 1 );
02009                     p2 = tmp0.ptr;
02010                     cvSetSeqReaderPos( &tmp0, d, 1 );
02011                     p3 = tmp0.ptr;
02012                     m2 = icvMed3( p1, p2, p3, cmp_func, aux );
02013                     cvSetSeqReaderPos( &tmp0, n - 1 - d*3 - n/2, 1 );
02014                     p1 = tmp0.ptr;
02015                     cvSetSeqReaderPos( &tmp0, d, 1 );
02016                     p2 = tmp0.ptr;
02017                     cvSetSeqReaderPos( &tmp0, d, 1 );
02018                     p3 = tmp0.ptr;
02019                     m3 = icvMed3( p1, p2, p3, cmp_func, aux );
02020                 }
02021                 else
02022                 {
02023                     m1 = tmp0.ptr;
02024                     cvSetSeqReaderPos( &tmp0, n/2, 1 );
02025                     m2 = tmp0.ptr;
02026                     cvSetSeqReaderPos( &tmp0, n - 1 - n/2, 1 );
02027                     m3 = tmp0.ptr;
02028                 }
02029 
02030                 pivot = icvMed3( m1, m2, m3, cmp_func, aux );
02031                 left = left0;
02032                 if( pivot != left.ptr )
02033                 {
02034                     CV_SWAP_ELEMS( pivot, left.ptr, elem_size );
02035                     pivot = left.ptr;
02036                 }
02037                 CV_NEXT_SEQ_ELEM( elem_size, left );
02038                 left1 = left;
02039 
02040                 for(;;)
02041                 {
02042                     while( left.ptr != right.ptr && (r = cmp_func(left.ptr, pivot, aux)) <= 0 )
02043                     {
02044                         if( r == 0 )
02045                         {
02046                             if( left1.ptr != left.ptr )
02047                                 CV_SWAP_ELEMS( left1.ptr, left.ptr, elem_size );
02048                             swap_cnt = 1;
02049                             CV_NEXT_SEQ_ELEM( elem_size, left1 );
02050                         }
02051                         CV_NEXT_SEQ_ELEM( elem_size, left );
02052                     }
02053 
02054                     while( left.ptr != right.ptr && (r = cmp_func(right.ptr,pivot, aux)) >= 0 )
02055                     {
02056                         if( r == 0 )
02057                         {
02058                             if( right1.ptr != right.ptr )
02059                                 CV_SWAP_ELEMS( right1.ptr, right.ptr, elem_size );
02060                             swap_cnt = 1;
02061                             CV_PREV_SEQ_ELEM( elem_size, right1 );
02062                         }
02063                         CV_PREV_SEQ_ELEM( elem_size, right );
02064                     }
02065 
02066                     if( left.ptr == right.ptr )
02067                     {
02068                         r = cmp_func(left.ptr, pivot, aux);
02069                         if( r == 0 )
02070                         {
02071                             if( left1.ptr != left.ptr )
02072                                 CV_SWAP_ELEMS( left1.ptr, left.ptr, elem_size );
02073                             swap_cnt = 1;
02074                             CV_NEXT_SEQ_ELEM( elem_size, left1 );
02075                         }
02076                         if( r <= 0 )
02077                         {
02078                             CV_NEXT_SEQ_ELEM( elem_size, left );
02079                         }
02080                         else
02081                         {
02082                             CV_PREV_SEQ_ELEM( elem_size, right );
02083                         }
02084                         break;
02085                     }
02086 
02087                     CV_SWAP_ELEMS( left.ptr, right.ptr, elem_size );
02088                     CV_NEXT_SEQ_ELEM( elem_size, left );
02089                     r = left.ptr == right.ptr;
02090                     CV_PREV_SEQ_ELEM( elem_size, right );
02091                     swap_cnt = 1;
02092                     if( r )
02093                         break;
02094                 }
02095 
02096                 if( swap_cnt == 0 )
02097                 {
02098                     left = left0, right = right0;
02099                     goto insert_sort;
02100                 }
02101 
02102                 l = cvGetSeqReaderPos( &left );
02103                 if( l == 0 )
02104                     l = seq->total;
02105                 l0 = cvGetSeqReaderPos( &left0 );
02106                 l1 = cvGetSeqReaderPos( &left1 );
02107                 if( l1 == 0 )
02108                     l1 = seq->total;
02109 
02110                 n = MIN( l - l1, l1 - l0 );
02111                 if( n > 0 )
02112                 {
02113                     tmp0 = left0;
02114                     tmp1 = left;
02115                     cvSetSeqReaderPos( &tmp1, 0-n, 1 );
02116                     for( i = 0; i < n; i++ )
02117                     {
02118                         CV_SWAP_ELEMS( tmp0.ptr, tmp1.ptr, elem_size );
02119                         CV_NEXT_SEQ_ELEM( elem_size, tmp0 );
02120                         CV_NEXT_SEQ_ELEM( elem_size, tmp1 );
02121                     }
02122                 }
02123 
02124                 r = cvGetSeqReaderPos( &right );
02125                 r0 = cvGetSeqReaderPos( &right0 );
02126                 r1 = cvGetSeqReaderPos( &right1 );
02127                 m = MIN( r0 - r1, r1 - r );
02128                 if( m > 0 )
02129                 {
02130                     tmp0 = left;
02131                     tmp1 = right0;
02132                     cvSetSeqReaderPos( &tmp1, 1-m, 1 );
02133                     for( i = 0; i < m; i++ )
02134                     {
02135                         CV_SWAP_ELEMS( tmp0.ptr, tmp1.ptr, elem_size );
02136                         CV_NEXT_SEQ_ELEM( elem_size, tmp0 );
02137                         CV_NEXT_SEQ_ELEM( elem_size, tmp1 );
02138                     }
02139                 }
02140 
02141                 n = l - l1;
02142                 m = r1 - r;
02143                 if( n > 1 )
02144                 {
02145                     if( m > 1 )
02146                     {
02147                         if( n > m )
02148                         {
02149                             sp++;
02150                             CV_SAVE_READER_POS( left0, stack[sp].lb );
02151                             cvSetSeqReaderPos( &left0, n - 1, 1 );
02152                             CV_SAVE_READER_POS( left0, stack[sp].ub );
02153                             left = right = right0;
02154                             cvSetSeqReaderPos( &left, 1 - m, 1 );
02155                         }
02156                         else
02157                         {
02158                             sp++;
02159                             CV_SAVE_READER_POS( right0, stack[sp].ub );
02160                             cvSetSeqReaderPos( &right0, 1 - m, 1 );
02161                             CV_SAVE_READER_POS( right0, stack[sp].lb );
02162                             left = right = left0;
02163                             cvSetSeqReaderPos( &right, n - 1, 1 );
02164                         }
02165                     }
02166                     else
02167                     {
02168                         left = right = left0;
02169                         cvSetSeqReaderPos( &right, n - 1, 1 );
02170                     }
02171                 }
02172                 else if( m > 1 )
02173                 {
02174                     left = right = right0;
02175                     cvSetSeqReaderPos( &left, 1 - m, 1 );
02176                 }
02177                 else
02178                     break;
02179             }
02180         }
02181     }
02182 }
02183 
02184 
02185 CV_IMPL schar*
02186 cvSeqSearch( CvSeq* seq, const void* _elem, CvCmpFunc cmp_func,
02187              int is_sorted, int* _idx, void* userdata )
02188 {
02189     schar* result = 0;
02190     const schar* elem = (const schar*)_elem;
02191     int idx = -1;
02192     int i, j;
02193 
02194     if( _idx )
02195         *_idx = idx;
02196 
02197     if( !CV_IS_SEQ(seq) )
02198         CV_Error( !seq ? CV_StsNullPtr : CV_StsBadArg, "Bad input sequence" );
02199 
02200     if( !elem )
02201         CV_Error( CV_StsNullPtr, "Null element pointer" );
02202 
02203     int elem_size = seq->elem_size;
02204     int total = seq->total;
02205 
02206     if( total == 0 )
02207         return 0;
02208 
02209     if( !is_sorted )
02210     {
02211         CvSeqReader reader;
02212         cvStartReadSeq( seq, &reader, 0 );
02213 
02214         if( cmp_func )
02215         {
02216             for( i = 0; i < total; i++ )
02217             {
02218                 if( cmp_func( elem, reader.ptr, userdata ) == 0 )
02219                     break;
02220                 CV_NEXT_SEQ_ELEM( elem_size, reader );
02221             }
02222         }
02223         else if( (elem_size & (sizeof(int)-1)) == 0 )
02224         {
02225             for( i = 0; i < total; i++ )
02226             {
02227                 for( j = 0; j < elem_size; j += sizeof(int) )
02228                 {
02229                     if( *(const int*)(reader.ptr + j) != *(const int*)(elem + j) )
02230                         break;
02231                 }
02232                 if( j == elem_size )
02233                     break;
02234                 CV_NEXT_SEQ_ELEM( elem_size, reader );
02235             }
02236         }
02237         else
02238         {
02239             for( i = 0; i < total; i++ )
02240             {
02241                 for( j = 0; j < elem_size; j++ )
02242                 {
02243                     if( reader.ptr[j] != elem[j] )
02244                         break;
02245                 }
02246                 if( j == elem_size )
02247                     break;
02248                 CV_NEXT_SEQ_ELEM( elem_size, reader );
02249             }
02250         }
02251 
02252         idx = i;
02253         if( i < total )
02254             result = reader.ptr;
02255     }
02256     else
02257     {
02258         if( !cmp_func )
02259             CV_Error( CV_StsNullPtr, "Null compare function" );
02260 
02261         i = 0, j = total;
02262 
02263         while( j > i )
02264         {
02265             int k = (i+j)>>1, code;
02266             schar* ptr = cvGetSeqElem( seq, k );
02267             code = cmp_func( elem, ptr, userdata );
02268             if( !code )
02269             {
02270                 result = ptr;
02271                 idx = k;
02272                 if( _idx )
02273                     *_idx = idx;
02274                 return result;
02275             }
02276             if( code < 0 )
02277                 j = k;
02278             else
02279                 i = k+1;
02280         }
02281         idx = j;
02282     }
02283 
02284     if( _idx )
02285         *_idx = idx;
02286 
02287     return result;
02288 }
02289 
02290 
02291 CV_IMPL void
02292 cvSeqInvert( CvSeq* seq )
02293 {
02294     CvSeqReader left_reader, right_reader;
02295     int elem_size;
02296     int i, count;
02297 
02298     cvStartReadSeq( seq, &left_reader, 0 );
02299     cvStartReadSeq( seq, &right_reader, 1 );
02300     elem_size = seq->elem_size;
02301     count = seq->total >> 1;
02302 
02303     for( i = 0; i < count; i++ )
02304     {
02305         CV_SWAP_ELEMS( left_reader.ptr, right_reader.ptr, elem_size );
02306         CV_NEXT_SEQ_ELEM( elem_size, left_reader );
02307         CV_PREV_SEQ_ELEM( elem_size, right_reader );
02308     }
02309 }
02310 
02311 
02312 typedef struct CvPTreeNode
02313 {
02314     struct CvPTreeNode* parent;
02315     schar* element;
02316     int rank;
02317 }
02318 CvPTreeNode;
02319 
02320 
02321 // This function splits the input sequence or set into one or more equivalence classes.
02322 // is_equal(a,b,...) returns non-zero if the two sequence elements
02323 // belong to the same class.  The function returns sequence of integers -
02324 // 0-based class indexes for each element.
02325 //
02326 // The algorithm is described in "Introduction to Algorithms"
02327 // by Cormen, Leiserson and Rivest, chapter "Data structures for disjoint sets"
02328 CV_IMPL  int
02329 cvSeqPartition( const CvSeq* seq, CvMemStorage* storage, CvSeq** labels,
02330                 CvCmpFunc is_equal, void* userdata )
02331 {
02332     CvSeq* result = 0;
02333     CvMemStorage* temp_storage = 0;
02334     int class_idx = 0;
02335 
02336     CvSeqWriter writer;
02337     CvSeqReader reader, reader0;
02338     CvSeq* nodes;
02339     int i, j;
02340     int is_set;
02341 
02342     if( !labels )
02343         CV_Error( CV_StsNullPtr, "" );
02344 
02345     if( !seq || !is_equal )
02346         CV_Error( CV_StsNullPtr, "" );
02347 
02348     if( !storage )
02349         storage = seq->storage;
02350 
02351     if( !storage )
02352         CV_Error( CV_StsNullPtr, "" );
02353 
02354     is_set = CV_IS_SET(seq);
02355 
02356     temp_storage = cvCreateChildMemStorage( storage );
02357 
02358     nodes = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvPTreeNode), temp_storage );
02359 
02360     cvStartReadSeq( seq, &reader );
02361     memset( &writer, 0, sizeof(writer));
02362     cvStartAppendToSeq( nodes, &writer );
02363 
02364     // Initial O(N) pass. Make a forest of single-vertex trees.
02365     for( i = 0; i < seq->total; i++ )
02366     {
02367         CvPTreeNode node = { 0, 0, 0 };
02368         if( !is_set || CV_IS_SET_ELEM( reader.ptr ))
02369             node.element = reader.ptr;
02370         CV_WRITE_SEQ_ELEM( node, writer );
02371         CV_NEXT_SEQ_ELEM( seq->elem_size, reader );
02372     }
02373 
02374     cvEndWriteSeq( &writer );
02375 
02376     // Because in the next loop we will iterate
02377     // through all the sequence nodes each time,
02378     // we do not need to initialize reader every time:
02379     cvStartReadSeq( nodes, &reader );
02380     cvStartReadSeq( nodes, &reader0 );
02381 
02382     // The main O(N^2) pass. Merge connected components.
02383     for( i = 0; i < nodes->total; i++ )
02384     {
02385         CvPTreeNode* node = (CvPTreeNode*)(reader0.ptr);
02386         CvPTreeNode* root = node;
02387         CV_NEXT_SEQ_ELEM( nodes->elem_size, reader0 );
02388 
02389         if( !node->element )
02390             continue;
02391 
02392         // find root
02393         while( root->parent )
02394             root = root->parent;
02395 
02396         for( j = 0; j < nodes->total; j++ )
02397         {
02398             CvPTreeNode* node2 = (CvPTreeNode*)reader.ptr;
02399 
02400             if( node2->element && node2 != node &&
02401                 is_equal( node->element, node2->element, userdata ))
02402             {
02403                 CvPTreeNode* root2 = node2;
02404 
02405                 // unite both trees
02406                 while( root2->parent )
02407                     root2 = root2->parent;
02408 
02409                 if( root2 != root )
02410                 {
02411                     if( root->rank > root2->rank )
02412                         root2->parent = root;
02413                     else
02414                     {
02415                         root->parent = root2;
02416                         root2->rank += root->rank == root2->rank;
02417                         root = root2;
02418                     }
02419                     assert( root->parent == 0 );
02420 
02421                     // Compress path from node2 to the root:
02422                     while( node2->parent )
02423                     {
02424                         CvPTreeNode* temp = node2;
02425                         node2 = node2->parent;
02426                         temp->parent = root;
02427                     }
02428 
02429                     // Compress path from node to the root:
02430                     node2 = node;
02431                     while( node2->parent )
02432                     {
02433                         CvPTreeNode* temp = node2;
02434                         node2 = node2->parent;
02435                         temp->parent = root;
02436                     }
02437                 }
02438             }
02439 
02440             CV_NEXT_SEQ_ELEM( sizeof(*node), reader );
02441         }
02442     }
02443 
02444     // Final O(N) pass (Enumerate classes)
02445     // Reuse reader one more time
02446     result = cvCreateSeq( 0, sizeof(CvSeq), sizeof(int), storage );
02447     cvStartAppendToSeq( result, &writer );
02448 
02449     for( i = 0; i < nodes->total; i++ )
02450     {
02451         CvPTreeNode* node = (CvPTreeNode*)reader.ptr;
02452         int idx = -1;
02453 
02454         if( node->element )
02455         {
02456             while( node->parent )
02457                 node = node->parent;
02458             if( node->rank >= 0 )
02459                 node->rank = ~class_idx++;
02460             idx = ~node->rank;
02461         }
02462 
02463         CV_NEXT_SEQ_ELEM( sizeof(*node), reader );
02464         CV_WRITE_SEQ_ELEM( idx, writer );
02465     }
02466 
02467     cvEndWriteSeq( &writer );
02468 
02469     if( labels )
02470         *labels = result;
02471 
02472     cvReleaseMemStorage( &temp_storage );
02473     return class_idx;
02474 }
02475 
02476 
02477 /****************************************************************************************\
02478 *                                      Set implementation                                *
02479 \****************************************************************************************/
02480 
02481 /* Creates empty set: */
02482 CV_IMPL CvSet*
02483 cvCreateSet( int set_flags, int header_size, int elem_size, CvMemStorage * storage )
02484 {
02485     if( !storage )
02486         CV_Error( CV_StsNullPtr, "" );
02487     if( header_size < (int)sizeof( CvSet ) ||
02488         elem_size < (int)sizeof(void*)*2 ||
02489         (elem_size & (sizeof(void*)-1)) != 0 )
02490         CV_Error( CV_StsBadSize, "" );
02491 
02492     CvSet* set = (CvSet*) cvCreateSeq( set_flags, header_size, elem_size, storage );
02493     set->flags = (set->flags & ~CV_MAGIC_MASK) | CV_SET_MAGIC_VAL;
02494 
02495     return set;
02496 }
02497 
02498 
02499 /* Add new element to the set: */
02500 CV_IMPL int
02501 cvSetAdd( CvSet* set, CvSetElem* element, CvSetElem** inserted_element )
02502 {
02503     int id = -1;
02504     CvSetElem *free_elem;
02505 
02506     if( !set )
02507         CV_Error( CV_StsNullPtr, "" );
02508 
02509     if( !(set->free_elems) )
02510     {
02511         int count = set->total;
02512         int elem_size = set->elem_size;
02513         schar *ptr;
02514         icvGrowSeq( (CvSeq *) set, 0 );
02515 
02516         set->free_elems = (CvSetElem*) (ptr = set->ptr);
02517         for( ; ptr + elem_size <= set->block_max; ptr += elem_size, count++ )
02518         {
02519             ((CvSetElem*)ptr)->flags = count | CV_SET_ELEM_FREE_FLAG;
02520             ((CvSetElem*)ptr)->next_free = (CvSetElem*)(ptr + elem_size);
02521         }
02522         assert( count <= CV_SET_ELEM_IDX_MASK+1 );
02523         ((CvSetElem*)(ptr - elem_size))->next_free = 0;
02524         set->first->prev->count += count - set->total;
02525         set->total = count;
02526         set->ptr = set->block_max;
02527     }
02528 
02529     free_elem = set->free_elems;
02530     set->free_elems = free_elem->next_free;
02531 
02532     id = free_elem->flags & CV_SET_ELEM_IDX_MASK;
02533     if( element )
02534         memcpy( free_elem, element, set->elem_size );
02535 
02536     free_elem->flags = id;
02537     set->active_count++;
02538 
02539     if( inserted_element )
02540         *inserted_element = free_elem;
02541 
02542     return id;
02543 }
02544 
02545 
02546 /* Remove element from a set given element index: */
02547 CV_IMPL void
02548 cvSetRemove( CvSet* set, int index )
02549 {
02550     CvSetElem* elem = cvGetSetElem( set, index );
02551     if( elem )
02552         cvSetRemoveByPtr( set, elem );
02553     else if( !set )
02554         CV_Error( CV_StsNullPtr, "" );
02555 }
02556 
02557 
02558 /* Remove all elements from a set: */
02559 CV_IMPL void
02560 cvClearSet( CvSet* set )
02561 {
02562     cvClearSeq( (CvSeq*)set );
02563     set->free_elems = 0;
02564     set->active_count = 0;
02565 }
02566 
02567 
02568 /****************************************************************************************\
02569 *                                 Graph  implementation                                  *
02570 \****************************************************************************************/
02571 
02572 /* Create a new graph: */
02573 CV_IMPL CvGraph *
02574 cvCreateGraph( int graph_type, int header_size,
02575                int vtx_size, int edge_size, CvMemStorage * storage )
02576 {
02577     CvGraph *graph = 0;
02578     CvSet *edges = 0;
02579     CvSet *vertices = 0;
02580 
02581     if( header_size < (int) sizeof( CvGraph     )
02582     ||  edge_size   < (int) sizeof( CvGraphEdge )
02583     ||  vtx_size    < (int) sizeof( CvGraphVtx  )
02584     ){
02585         CV_Error( CV_StsBadSize, "" );
02586     }
02587 
02588     vertices = cvCreateSet( graph_type, header_size, vtx_size, storage );
02589     edges = cvCreateSet( CV_SEQ_KIND_GENERIC | CV_SEQ_ELTYPE_GRAPH_EDGE,
02590                                   sizeof( CvSet ), edge_size, storage );
02591 
02592     graph = (CvGraph*)vertices;
02593     graph->edges = edges;
02594 
02595     return graph;
02596 }
02597 
02598 
02599 /* Remove all vertices and edges from a graph: */
02600 CV_IMPL void
02601 cvClearGraph( CvGraph * graph )
02602 {
02603     if( !graph )
02604         CV_Error( CV_StsNullPtr, "" );
02605 
02606     cvClearSet( graph->edges );
02607     cvClearSet( (CvSet*)graph );
02608 }
02609 
02610 
02611 /* Add a vertex to a graph: */
02612 CV_IMPL int
02613 cvGraphAddVtx( CvGraph* graph, const CvGraphVtx* _vertex, CvGraphVtx** _inserted_vertex )
02614 {
02615     CvGraphVtx *vertex = 0;
02616     int index = -1;
02617 
02618     if( !graph )
02619         CV_Error( CV_StsNullPtr, "" );
02620 
02621     vertex = (CvGraphVtx*)cvSetNew((CvSet*)graph);
02622     if( vertex )
02623     {
02624         if( _vertex )
02625             memcpy( vertex + 1, _vertex + 1, graph->elem_size - sizeof(CvGraphVtx) );
02626         vertex->first = 0;
02627         index = vertex->flags;
02628     }
02629 
02630     if( _inserted_vertex )
02631         *_inserted_vertex = vertex;
02632 
02633     return index;
02634 }
02635 
02636 
02637 /* Remove a vertex from the graph together with its incident edges: */
02638 CV_IMPL int
02639 cvGraphRemoveVtxByPtr( CvGraph* graph, CvGraphVtx* vtx )
02640 {
02641     int count = -1;
02642 
02643     if( !graph || !vtx )
02644         CV_Error( CV_StsNullPtr, "" );
02645 
02646     if( !CV_IS_SET_ELEM(vtx))
02647         CV_Error( CV_StsBadArg, "The vertex does not belong to the graph" );
02648 
02649     count = graph->edges->active_count;
02650     for( ;; )
02651     {
02652         CvGraphEdge *edge = vtx->first;
02653         if( !edge )
02654             break;
02655         cvGraphRemoveEdgeByPtr( graph, edge->vtx[0], edge->vtx[1] );
02656     }
02657     count -= graph->edges->active_count;
02658     cvSetRemoveByPtr( (CvSet*)graph, vtx );
02659 
02660     return count;
02661 }
02662 
02663 
02664 /* Remove a vertex from the graph together with its incident edges: */
02665 CV_IMPL int
02666 cvGraphRemoveVtx( CvGraph* graph, int index )
02667 {
02668     int count = -1;
02669     CvGraphVtx *vtx = 0;
02670 
02671     if( !graph )
02672         CV_Error( CV_StsNullPtr, "" );
02673 
02674     vtx = cvGetGraphVtx( graph, index );
02675     if( !vtx )
02676         CV_Error( CV_StsBadArg, "The vertex is not found" );
02677 
02678     count = graph->edges->active_count;
02679     for( ;; )
02680     {
02681         CvGraphEdge *edge = vtx->first;
02682         count++;
02683 
02684         if( !edge )
02685             break;
02686         cvGraphRemoveEdgeByPtr( graph, edge->vtx[0], edge->vtx[1] );
02687     }
02688     count -= graph->edges->active_count;
02689     cvSetRemoveByPtr( (CvSet*)graph, vtx );
02690 
02691     return count;
02692 }
02693 
02694 
02695 /* Find a graph edge given pointers to the ending vertices: */
02696 CV_IMPL CvGraphEdge*
02697 cvFindGraphEdgeByPtr( const CvGraph* graph,
02698                       const CvGraphVtx* start_vtx,
02699                       const CvGraphVtx* end_vtx )
02700 {
02701     int ofs = 0;
02702 
02703     if( !graph || !start_vtx || !end_vtx )
02704         CV_Error( CV_StsNullPtr, "" );
02705 
02706     if( start_vtx == end_vtx )
02707         return 0;
02708 
02709     if( !CV_IS_GRAPH_ORIENTED( graph ) &&
02710         (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) )
02711     {
02712         const CvGraphVtx* t;
02713         CV_SWAP( start_vtx, end_vtx, t );
02714     }
02715 
02716     CvGraphEdge* edge = start_vtx->first;
02717     for( ; edge; edge = edge->next[ofs] )
02718     {
02719         ofs = start_vtx == edge->vtx[1];
02720         assert( ofs == 1 || start_vtx == edge->vtx[0] );
02721         if( edge->vtx[1] == end_vtx )
02722             break;
02723     }
02724 
02725     return edge;
02726 }
02727 
02728 
02729 /* Find an edge in the graph given indices of the ending vertices: */
02730 CV_IMPL CvGraphEdge *
02731 cvFindGraphEdge( const CvGraph* graph, int start_idx, int end_idx )
02732 {
02733     CvGraphVtx *start_vtx;
02734     CvGraphVtx *end_vtx;
02735 
02736     if( !graph )
02737         CV_Error( CV_StsNullPtr, "graph pointer is NULL" );
02738 
02739     start_vtx = cvGetGraphVtx( graph, start_idx );
02740     end_vtx = cvGetGraphVtx( graph, end_idx );
02741 
02742     return cvFindGraphEdgeByPtr( graph, start_vtx, end_vtx );
02743 }
02744 
02745 
02746 /* Given two vertices, return the edge
02747  * connecting them, creating it if it
02748  * did not already exist:
02749  */
02750 CV_IMPL int
02751 cvGraphAddEdgeByPtr( CvGraph* graph,
02752                      CvGraphVtx* start_vtx, CvGraphVtx* end_vtx,
02753                      const CvGraphEdge* _edge,
02754                      CvGraphEdge ** _inserted_edge )
02755 {
02756     CvGraphEdge *edge = 0;
02757     int result = -1;
02758     int delta;
02759 
02760     if( !graph )
02761         CV_Error( CV_StsNullPtr, "graph pointer is NULL" );
02762 
02763     if( !CV_IS_GRAPH_ORIENTED( graph ) &&
02764         (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) )
02765     {
02766         CvGraphVtx* t;
02767         CV_SWAP( start_vtx, end_vtx, t );
02768     }
02769 
02770     edge = cvFindGraphEdgeByPtr( graph, start_vtx, end_vtx );
02771     if( edge )
02772     {
02773         result = 0;
02774         if( _inserted_edge )
02775             *_inserted_edge = edge;
02776         return result;
02777     }
02778 
02779     if( start_vtx == end_vtx )
02780         CV_Error( start_vtx ? CV_StsBadArg : CV_StsNullPtr,
02781         "vertex pointers coinside (or set to NULL)" );
02782 
02783     edge = (CvGraphEdge*)cvSetNew( (CvSet*)(graph->edges) );
02784     assert( edge->flags >= 0 );
02785 
02786     edge->vtx[0] = start_vtx;
02787     edge->vtx[1] = end_vtx;
02788     edge->next[0] = start_vtx->first;
02789     edge->next[1] = end_vtx->first;
02790     start_vtx->first = end_vtx->first = edge;
02791 
02792     delta = graph->edges->elem_size - sizeof(*edge);
02793     if( _edge )
02794     {
02795         if( delta > 0 )
02796             memcpy( edge + 1, _edge + 1, delta );
02797         edge->weight = _edge->weight;
02798     }
02799     else
02800     {
02801         if( delta > 0 )
02802             memset( edge + 1, 0, delta );
02803         edge->weight = 1.f;
02804     }
02805 
02806     result = 1;
02807 
02808     if( _inserted_edge )
02809         *_inserted_edge = edge;
02810 
02811     return result;
02812 }
02813 
02814 /* Given two vertices, return the edge
02815  * connecting them, creating it if it
02816  * did not already exist:
02817  */
02818 CV_IMPL int
02819 cvGraphAddEdge( CvGraph* graph,
02820                 int start_idx, int end_idx,
02821                 const CvGraphEdge* _edge,
02822                 CvGraphEdge ** _inserted_edge )
02823 {
02824     CvGraphVtx *start_vtx;
02825     CvGraphVtx *end_vtx;
02826 
02827     if( !graph )
02828         CV_Error( CV_StsNullPtr, "" );
02829 
02830     start_vtx = cvGetGraphVtx( graph, start_idx );
02831     end_vtx = cvGetGraphVtx( graph, end_idx );
02832 
02833     return cvGraphAddEdgeByPtr( graph, start_vtx, end_vtx, _edge, _inserted_edge );
02834 }
02835 
02836 
02837 /* Remove the graph edge connecting two given vertices: */
02838 CV_IMPL void
02839 cvGraphRemoveEdgeByPtr( CvGraph* graph, CvGraphVtx* start_vtx, CvGraphVtx* end_vtx )
02840 {
02841     int ofs, prev_ofs;
02842     CvGraphEdge *edge, *next_edge, *prev_edge;
02843 
02844     if( !graph || !start_vtx || !end_vtx )
02845         CV_Error( CV_StsNullPtr, "" );
02846 
02847     if( start_vtx == end_vtx )
02848         return;
02849 
02850     if( !CV_IS_GRAPH_ORIENTED( graph ) &&
02851         (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) )
02852     {
02853         CvGraphVtx* t;
02854         CV_SWAP( start_vtx, end_vtx, t );
02855     }
02856 
02857     for( ofs = prev_ofs = 0, prev_edge = 0, edge = start_vtx->first; edge != 0;
02858          prev_ofs = ofs, prev_edge = edge, edge = edge->next[ofs] )
02859     {
02860         ofs = start_vtx == edge->vtx[1];
02861         assert( ofs == 1 || start_vtx == edge->vtx[0] );
02862         if( edge->vtx[1] == end_vtx )
02863             break;
02864     }
02865 
02866     if( !edge )
02867         return;
02868 
02869     next_edge = edge->next[ofs];
02870     if( prev_edge )
02871         prev_edge->next[prev_ofs] = next_edge;
02872     else
02873         start_vtx->first = next_edge;
02874 
02875     for( ofs = prev_ofs = 0, prev_edge = 0, edge = end_vtx->first; edge != 0;
02876          prev_ofs = ofs, prev_edge = edge, edge = edge->next[ofs] )
02877     {
02878         ofs = end_vtx == edge->vtx[1];
02879         assert( ofs == 1 || end_vtx == edge->vtx[0] );
02880         if( edge->vtx[0] == start_vtx )
02881             break;
02882     }
02883 
02884     assert( edge != 0 );
02885 
02886     next_edge = edge->next[ofs];
02887     if( prev_edge )
02888         prev_edge->next[prev_ofs] = next_edge;
02889     else
02890         end_vtx->first = next_edge;
02891 
02892     cvSetRemoveByPtr( graph->edges, edge );
02893 }
02894 
02895 
02896 /* Remove the graph edge connecting two given vertices: */
02897 CV_IMPL void
02898 cvGraphRemoveEdge( CvGraph* graph, int start_idx, int end_idx )
02899 {
02900     CvGraphVtx *start_vtx;
02901     CvGraphVtx *end_vtx;
02902 
02903     if( !graph )
02904         CV_Error( CV_StsNullPtr, "" );
02905 
02906     start_vtx = cvGetGraphVtx( graph, start_idx );
02907     end_vtx = cvGetGraphVtx( graph, end_idx );
02908 
02909     cvGraphRemoveEdgeByPtr( graph, start_vtx, end_vtx );
02910 }
02911 
02912 
02913 /* Count number of edges incident to a given vertex: */
02914 CV_IMPL int
02915 cvGraphVtxDegreeByPtr( const CvGraph* graph, const CvGraphVtx* vertex )
02916 {
02917     CvGraphEdge *edge;
02918     int count;
02919 
02920     if( !graph || !vertex )
02921         CV_Error( CV_StsNullPtr, "" );
02922 
02923     for( edge = vertex->first, count = 0; edge; )
02924     {
02925         count++;
02926         edge = CV_NEXT_GRAPH_EDGE( edge, vertex );
02927     }
02928 
02929     return count;
02930 }
02931 
02932 
02933 /* Count number of edges incident to a given vertex: */
02934 CV_IMPL int
02935 cvGraphVtxDegree( const CvGraph* graph, int vtx_idx )
02936 {
02937     CvGraphVtx *vertex;
02938     CvGraphEdge *edge;
02939     int count;
02940 
02941     if( !graph )
02942         CV_Error( CV_StsNullPtr, "" );
02943 
02944     vertex = cvGetGraphVtx( graph, vtx_idx );
02945     if( !vertex )
02946         CV_Error( CV_StsObjectNotFound, "" );
02947 
02948     for( edge = vertex->first, count = 0; edge; )
02949     {
02950         count++;
02951         edge = CV_NEXT_GRAPH_EDGE( edge, vertex );
02952     }
02953 
02954     return count;
02955 }
02956 
02957 
02958 typedef struct CvGraphItem
02959 {
02960     CvGraphVtx* vtx;
02961     CvGraphEdge* edge;
02962 }
02963 CvGraphItem;
02964 
02965 
02966 static  void
02967 icvSeqElemsClearFlags( CvSeq* seq, int offset, int clear_mask )
02968 {
02969     CvSeqReader reader;
02970     int i, total, elem_size;
02971 
02972     if( !seq )
02973         CV_Error( CV_StsNullPtr, "" );
02974 
02975     elem_size = seq->elem_size;
02976     total = seq->total;
02977 
02978     if( (unsigned)offset > (unsigned)elem_size )
02979         CV_Error( CV_StsBadArg, "" );
02980 
02981     cvStartReadSeq( seq, &reader );
02982 
02983     for( i = 0; i < total; i++ )
02984     {
02985         int* flag_ptr = (int*)(reader.ptr + offset);
02986         *flag_ptr &= ~clear_mask;
02987 
02988         CV_NEXT_SEQ_ELEM( elem_size, reader );
02989     }
02990 }
02991 
02992 
02993 static  schar*
02994 icvSeqFindNextElem( CvSeq* seq, int offset, int mask,
02995                     int value, int* start_index )
02996 {
02997     schar* elem_ptr = 0;
02998 
02999     CvSeqReader reader;
03000     int total, elem_size, index;
03001 
03002     if( !seq || !start_index )
03003         CV_Error( CV_StsNullPtr, "" );
03004 
03005     elem_size = seq->elem_size;
03006     total = seq->total;
03007     index = *start_index;
03008 
03009     if( (unsigned)offset > (unsigned)elem_size )
03010         CV_Error( CV_StsBadArg, "" );
03011 
03012     if( total == 0 )
03013         return 0;
03014 
03015     if( (unsigned)index >= (unsigned)total )
03016     {
03017         index %= total;
03018         index += index < 0 ? total : 0;
03019     }
03020 
03021     cvStartReadSeq( seq, &reader );
03022 
03023     if( index != 0 )
03024         cvSetSeqReaderPos( &reader, index );
03025 
03026     for( index = 0; index < total; index++ )
03027     {
03028         int* flag_ptr = (int*)(reader.ptr + offset);
03029         if( (*flag_ptr & mask) == value )
03030             break;
03031 
03032         CV_NEXT_SEQ_ELEM( elem_size, reader );
03033     }
03034 
03035     if( index < total )
03036     {
03037         elem_ptr = reader.ptr;
03038         *start_index = index;
03039     }
03040 
03041     return  elem_ptr;
03042 }
03043 
03044 #define CV_FIELD_OFFSET( field, structtype ) ((int)(size_t)&((structtype*)0)->field)
03045 
03046 CV_IMPL CvGraphScanner*
03047 cvCreateGraphScanner( CvGraph* graph, CvGraphVtx* vtx, int mask )
03048 {
03049     if( !graph )
03050         CV_Error( CV_StsNullPtr, "Null graph pointer" );
03051 
03052     CV_Assert( graph->storage != 0 );
03053 
03054     CvGraphScanner* scanner = (CvGraphScanner*)cvAlloc( sizeof(*scanner) );
03055     memset( scanner, 0, sizeof(*scanner));
03056 
03057     scanner->graph = graph;
03058     scanner->mask = mask;
03059     scanner->vtx = vtx;
03060     scanner->index = vtx == 0 ? 0 : -1;
03061 
03062     CvMemStorage* child_storage = cvCreateChildMemStorage( graph->storage );
03063 
03064     scanner->stack = cvCreateSeq( 0, sizeof(CvSet),
03065                        sizeof(CvGraphItem), child_storage );
03066 
03067     icvSeqElemsClearFlags( (CvSeq*)graph,
03068                                     CV_FIELD_OFFSET( flags, CvGraphVtx),
03069                                     CV_GRAPH_ITEM_VISITED_FLAG|
03070                                     CV_GRAPH_SEARCH_TREE_NODE_FLAG );
03071 
03072     icvSeqElemsClearFlags( (CvSeq*)(graph->edges),
03073                                     CV_FIELD_OFFSET( flags, CvGraphEdge),
03074                                     CV_GRAPH_ITEM_VISITED_FLAG );
03075 
03076     return scanner;
03077 }
03078 
03079 
03080 CV_IMPL void
03081 cvReleaseGraphScanner( CvGraphScanner** scanner )
03082 {
03083     if( !scanner )
03084         CV_Error( CV_StsNullPtr, "Null double pointer to graph scanner" );
03085 
03086     if( *scanner )
03087     {
03088         if( (*scanner)->stack )
03089             cvReleaseMemStorage( &((*scanner)->stack->storage));
03090         cvFree( scanner );
03091     }
03092 }
03093 
03094 
03095 CV_IMPL int
03096 cvNextGraphItem( CvGraphScanner* scanner )
03097 {
03098     int code = -1;
03099     CvGraphVtx* vtx;
03100     CvGraphVtx* dst;
03101     CvGraphEdge* edge;
03102     CvGraphItem item;
03103 
03104     if( !scanner || !(scanner->stack))
03105         CV_Error( CV_StsNullPtr, "Null graph scanner" );
03106 
03107     dst = scanner->dst;
03108     vtx = scanner->vtx;
03109     edge = scanner->edge;
03110 
03111     for(;;)
03112     {
03113         for(;;)
03114         {
03115             if( dst && !CV_IS_GRAPH_VERTEX_VISITED(dst) )
03116             {
03117                 scanner->vtx = vtx = dst;
03118                 edge = vtx->first;
03119                 dst->flags |= CV_GRAPH_ITEM_VISITED_FLAG;
03120 
03121                 if((scanner->mask & CV_GRAPH_VERTEX))
03122                 {
03123                     scanner->vtx = vtx;
03124                     scanner->edge = vtx->first;
03125                     scanner->dst = 0;
03126                     code = CV_GRAPH_VERTEX;
03127                     return code;
03128                 }
03129             }
03130 
03131             while( edge )
03132             {
03133                 dst = edge->vtx[vtx == edge->vtx[0]];
03134 
03135                 if( !CV_IS_GRAPH_EDGE_VISITED(edge) )
03136                 {
03137                     // Check that the edge is outgoing:
03138                     if( !CV_IS_GRAPH_ORIENTED( scanner->graph ) || dst != edge->vtx[0] )
03139                     {
03140                         edge->flags |= CV_GRAPH_ITEM_VISITED_FLAG;
03141 
03142                         if( !CV_IS_GRAPH_VERTEX_VISITED(dst) )
03143                         {
03144                             item.vtx = vtx;
03145                             item.edge = edge;
03146 
03147                             vtx->flags |= CV_GRAPH_SEARCH_TREE_NODE_FLAG;
03148 
03149                             cvSeqPush( scanner->stack, &item );
03150 
03151                             if( scanner->mask & CV_GRAPH_TREE_EDGE )
03152                             {
03153                                 code = CV_GRAPH_TREE_EDGE;
03154                                 scanner->vtx = vtx;
03155                                 scanner->dst = dst;
03156                                 scanner->edge = edge;
03157                                 return code;
03158                             }
03159                             break;
03160                         }
03161                         else
03162                         {
03163                             if( scanner->mask & (CV_GRAPH_BACK_EDGE|
03164                                                  CV_GRAPH_CROSS_EDGE|
03165                                                  CV_GRAPH_FORWARD_EDGE) )
03166                             {
03167                                 code = (dst->flags & CV_GRAPH_SEARCH_TREE_NODE_FLAG) ?
03168                                        CV_GRAPH_BACK_EDGE :
03169                                        (edge->flags & CV_GRAPH_FORWARD_EDGE_FLAG) ?
03170                                        CV_GRAPH_FORWARD_EDGE : CV_GRAPH_CROSS_EDGE;
03171                                 edge->flags &= ~CV_GRAPH_FORWARD_EDGE_FLAG;
03172                                 if( scanner->mask & code )
03173                                 {
03174                                     scanner->vtx = vtx;
03175                                     scanner->dst = dst;
03176                                     scanner->edge = edge;
03177                                     return code;
03178                                 }
03179                             }
03180                         }
03181                     }
03182                     else if( (dst->flags & (CV_GRAPH_ITEM_VISITED_FLAG|
03183                              CV_GRAPH_SEARCH_TREE_NODE_FLAG)) ==
03184                              (CV_GRAPH_ITEM_VISITED_FLAG|
03185                              CV_GRAPH_SEARCH_TREE_NODE_FLAG))
03186                     {
03187                         edge->flags |= CV_GRAPH_FORWARD_EDGE_FLAG;
03188                     }
03189                 }
03190 
03191                 edge = CV_NEXT_GRAPH_EDGE( edge, vtx );
03192             }
03193 
03194             if( !edge ) /* need to backtrack */
03195             {
03196                 if( scanner->stack->total == 0 )
03197                 {
03198                     if( scanner->index >= 0 )
03199                         vtx = 0;
03200                     else
03201                         scanner->index = 0;
03202                     break;
03203                 }
03204                 cvSeqPop( scanner->stack, &item );
03205                 vtx = item.vtx;
03206                 vtx->flags &= ~CV_GRAPH_SEARCH_TREE_NODE_FLAG;
03207                 edge = item.edge;
03208                 dst = 0;
03209 
03210                 if( scanner->mask & CV_GRAPH_BACKTRACKING )
03211                 {
03212                     scanner->vtx = vtx;
03213                     scanner->edge = edge;
03214                     scanner->dst = edge->vtx[vtx == edge->vtx[0]];
03215                     code = CV_GRAPH_BACKTRACKING;
03216                     return code;
03217                 }
03218             }
03219         }
03220 
03221         if( !vtx )
03222         {
03223             vtx = (CvGraphVtx*)icvSeqFindNextElem( (CvSeq*)(scanner->graph),
03224                   CV_FIELD_OFFSET( flags, CvGraphVtx ), CV_GRAPH_ITEM_VISITED_FLAG|INT_MIN,
03225                   0, &(scanner->index) );
03226 
03227             if( !vtx )
03228             {
03229                 code = CV_GRAPH_OVER;
03230                 break;
03231             }
03232         }
03233 
03234         dst = vtx;
03235         if( scanner->mask & CV_GRAPH_NEW_TREE )
03236         {
03237             scanner->dst = dst;
03238             scanner->edge = 0;
03239             scanner->vtx = 0;
03240             code = CV_GRAPH_NEW_TREE;
03241             break;
03242         }
03243     }
03244 
03245     return code;
03246 }
03247 
03248 
03249 CV_IMPL CvGraph*
03250 cvCloneGraph( const CvGraph* graph, CvMemStorage* storage )
03251 {
03252     int* flag_buffer = 0;
03253     CvGraphVtx** ptr_buffer = 0;
03254     CvGraph* result = 0;
03255 
03256     int i, k;
03257     int vtx_size, edge_size;
03258     CvSeqReader reader;
03259 
03260     if( !CV_IS_GRAPH(graph))
03261         CV_Error( CV_StsBadArg, "Invalid graph pointer" );
03262 
03263     if( !storage )
03264         storage = graph->storage;
03265 
03266     if( !storage )
03267         CV_Error( CV_StsNullPtr, "NULL storage pointer" );
03268 
03269     vtx_size = graph->elem_size;
03270     edge_size = graph->edges->elem_size;
03271 
03272     flag_buffer = (int*)cvAlloc( graph->total*sizeof(flag_buffer[0]));
03273     ptr_buffer = (CvGraphVtx**)cvAlloc( graph->total*sizeof(ptr_buffer[0]));
03274     result = cvCreateGraph( graph->flags, graph->header_size,
03275                                      vtx_size, edge_size, storage );
03276     memcpy( result + sizeof(CvGraph), graph + sizeof(CvGraph),
03277             graph->header_size - sizeof(CvGraph));
03278 
03279     // Pass 1.  Save flags, copy vertices:
03280     cvStartReadSeq( (CvSeq*)graph, &reader );
03281     for( i = 0, k = 0; i < graph->total; i++ )
03282     {
03283         if( CV_IS_SET_ELEM( reader.ptr ))
03284         {
03285             CvGraphVtx* vtx = (CvGraphVtx*)reader.ptr;
03286             CvGraphVtx* dstvtx = 0;
03287             cvGraphAddVtx( result, vtx, &dstvtx );
03288             flag_buffer[k] = dstvtx->flags = vtx->flags;
03289             vtx->flags = k;
03290             ptr_buffer[k++] = dstvtx;
03291         }
03292         CV_NEXT_SEQ_ELEM( vtx_size, reader );
03293     }
03294 
03295     // Pass 2.  Copy edges:
03296     cvStartReadSeq( (CvSeq*)graph->edges, &reader );
03297     for( i = 0; i < graph->edges->total; i++ )
03298     {
03299         if( CV_IS_SET_ELEM( reader.ptr ))
03300         {
03301             CvGraphEdge* edge = (CvGraphEdge*)reader.ptr;
03302             CvGraphEdge* dstedge = 0;
03303             CvGraphVtx* new_org = ptr_buffer[edge->vtx[0]->flags];
03304             CvGraphVtx* new_dst = ptr_buffer[edge->vtx[1]->flags];
03305             cvGraphAddEdgeByPtr( result, new_org, new_dst, edge, &dstedge );
03306             dstedge->flags = edge->flags;
03307         }
03308         CV_NEXT_SEQ_ELEM( edge_size, reader );
03309     }
03310 
03311     // Pass 3.  Restore flags:
03312     cvStartReadSeq( (CvSeq*)graph, &reader );
03313     for( i = 0, k = 0; i < graph->edges->total; i++ )
03314     {
03315         if( CV_IS_SET_ELEM( reader.ptr ))
03316         {
03317             CvGraphVtx* vtx = (CvGraphVtx*)reader.ptr;
03318             vtx->flags = flag_buffer[k++];
03319         }
03320         CV_NEXT_SEQ_ELEM( vtx_size, reader );
03321     }
03322 
03323     cvFree( &flag_buffer );
03324     cvFree( &ptr_buffer );
03325 
03326     if( cvGetErrStatus() < 0 )
03327         result = 0;
03328 
03329     return result;
03330 }
03331 
03332 
03333 /****************************************************************************************\
03334 *                                 Working with sequence tree                             *
03335 \****************************************************************************************/
03336 
03337 // Gather pointers to all the sequences, accessible from the <first>, to the single sequence.
03338 CV_IMPL CvSeq*
03339 cvTreeToNodeSeq( const void* first, int header_size, CvMemStorage* storage )
03340 {
03341     CvSeq* allseq = 0;
03342     CvTreeNodeIterator iterator;
03343 
03344     if( !storage )
03345         CV_Error( CV_StsNullPtr, "NULL storage pointer" );
03346 
03347     allseq = cvCreateSeq( 0, header_size, sizeof(first), storage );
03348 
03349     if( first )
03350     {
03351         cvInitTreeNodeIterator( &iterator, first, INT_MAX );
03352 
03353         for(;;)
03354         {
03355             void* node = cvNextTreeNode( &iterator );
03356             if( !node )
03357                 break;
03358             cvSeqPush( allseq, &node );
03359         }
03360     }
03361 
03362 
03363 
03364     return allseq;
03365 }
03366 
03367 
03368 typedef struct CvTreeNode
03369 {
03370     int       flags;         /* micsellaneous flags */
03371     int       header_size;   /* size of sequence header */
03372     struct    CvTreeNode* h_prev; /* previous sequence */
03373     struct    CvTreeNode* h_next; /* next sequence */
03374     struct    CvTreeNode* v_prev; /* 2nd previous sequence */
03375     struct    CvTreeNode* v_next; /* 2nd next sequence */
03376 }
03377 CvTreeNode;
03378 
03379 
03380 
03381 // Insert contour into tree given certain parent sequence.
03382 // If parent is equal to frame (the most external contour),
03383 // then added contour will have null pointer to parent:
03384 CV_IMPL void
03385 cvInsertNodeIntoTree( void* _node, void* _parent, void* _frame )
03386 {
03387     CvTreeNode* node = (CvTreeNode*)_node;
03388     CvTreeNode* parent = (CvTreeNode*)_parent;
03389 
03390     if( !node || !parent )
03391         CV_Error( CV_StsNullPtr, "" );
03392 
03393     node->v_prev = _parent != _frame ? parent : 0;
03394     node->h_next = parent->v_next;
03395 
03396     assert( parent->v_next != node );
03397 
03398     if( parent->v_next )
03399         parent->v_next->h_prev = node;
03400     parent->v_next = node;
03401 }
03402 
03403 
03404 // Remove contour from tree, together with the contour's children:
03405 CV_IMPL void
03406 cvRemoveNodeFromTree( void* _node, void* _frame )
03407 {
03408     CvTreeNode* node = (CvTreeNode*)_node;
03409     CvTreeNode* frame = (CvTreeNode*)_frame;
03410 
03411     if( !node )
03412         CV_Error( CV_StsNullPtr, "" );
03413 
03414     if( node == frame )
03415         CV_Error( CV_StsBadArg, "frame node could not be deleted" );
03416 
03417     if( node->h_next )
03418         node->h_next->h_prev = node->h_prev;
03419 
03420     if( node->h_prev )
03421         node->h_prev->h_next = node->h_next;
03422     else
03423     {
03424         CvTreeNode* parent = node->v_prev;
03425         if( !parent )
03426             parent = frame;
03427 
03428         if( parent )
03429         {
03430             assert( parent->v_next == node );
03431             parent->v_next = node->h_next;
03432         }
03433     }
03434 }
03435 
03436 
03437 CV_IMPL void
03438 cvInitTreeNodeIterator( CvTreeNodeIterator* treeIterator,
03439                         const void* first, int max_level )
03440 {
03441     if( !treeIterator || !first )
03442         CV_Error( CV_StsNullPtr, "" );
03443 
03444     if( max_level < 0 )
03445         CV_Error( CV_StsOutOfRange, "" );
03446 
03447     treeIterator->node = (void*)first;
03448     treeIterator->level = 0;
03449     treeIterator->max_level = max_level;
03450 }
03451 
03452 
03453 CV_IMPL void*
03454 cvNextTreeNode( CvTreeNodeIterator* treeIterator )
03455 {
03456     CvTreeNode* prevNode = 0;
03457     CvTreeNode* node;
03458     int level;
03459 
03460     if( !treeIterator )
03461         CV_Error( CV_StsNullPtr, "NULL iterator pointer" );
03462 
03463     prevNode = node = (CvTreeNode*)treeIterator->node;
03464     level = treeIterator->level;
03465 
03466     if( node )
03467     {
03468         if( node->v_next && level+1 < treeIterator->max_level )
03469         {
03470             node = node->v_next;
03471             level++;
03472         }
03473         else
03474         {
03475             while( node->h_next == 0 )
03476             {
03477                 node = node->v_prev;
03478                 if( --level < 0 )
03479                 {
03480                     node = 0;
03481                     break;
03482                 }
03483             }
03484             node = node && treeIterator->max_level != 0 ? node->h_next : 0;
03485         }
03486     }
03487 
03488     treeIterator->node = node;
03489     treeIterator->level = level;
03490     return prevNode;
03491 }
03492 
03493 
03494 CV_IMPL void*
03495 cvPrevTreeNode( CvTreeNodeIterator* treeIterator )
03496 {
03497     CvTreeNode* prevNode = 0;
03498     CvTreeNode* node;
03499     int level;
03500 
03501     if( !treeIterator )
03502         CV_Error( CV_StsNullPtr, "" );
03503 
03504     prevNode = node = (CvTreeNode*)treeIterator->node;
03505     level = treeIterator->level;
03506 
03507     if( node )
03508     {
03509         if( !node->h_prev )
03510         {
03511             node = node->v_prev;
03512             if( --level < 0 )
03513                 node = 0;
03514         }
03515         else
03516         {
03517             node = node->h_prev;
03518 
03519             while( node->v_next && level < treeIterator->max_level )
03520             {
03521                 node = node->v_next;
03522                 level++;
03523 
03524                 while( node->h_next )
03525                     node = node->h_next;
03526             }
03527         }
03528     }
03529 
03530     treeIterator->node = node;
03531     treeIterator->level = level;
03532     return prevNode;
03533 }
03534 
03535 namespace cv
03536 {
03537 
03538 ////////////////////////////////////////////////////////////////////////////////
03539 
03540 schar*  seqPush( CvSeq* seq, const void* element )
03541 {
03542     return cvSeqPush(seq, element);
03543 }
03544 
03545 schar*  seqPushFront( CvSeq* seq, const void* element )
03546 {
03547     return cvSeqPushFront(seq, element);
03548 }
03549 
03550 void  seqPop( CvSeq* seq, void* element )
03551 {
03552     cvSeqPop(seq, element);
03553 }
03554 
03555 void  seqPopFront( CvSeq* seq, void* element )
03556 {
03557     cvSeqPopFront(seq, element);
03558 }
03559 
03560 void  seqRemove( CvSeq* seq, int index )
03561 {
03562     cvSeqRemove(seq, index);
03563 }
03564 
03565 void  clearSeq( CvSeq* seq )
03566 {
03567     cvClearSeq(seq);
03568 }
03569 
03570 schar*  getSeqElem( const CvSeq* seq, int index )
03571 {
03572     return cvGetSeqElem(seq, index);
03573 }
03574 
03575 void  seqRemoveSlice( CvSeq* seq, CvSlice slice )
03576 {
03577     return cvSeqRemoveSlice(seq, slice);
03578 }
03579 
03580 void  seqInsertSlice( CvSeq* seq, int before_index, const CvArr* from_arr )
03581 {
03582     cvSeqInsertSlice(seq, before_index, from_arr);
03583 }
03584 
03585 }
03586 
03587 /* End of file. */
03588