1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
43 #define ICV_FREE_PTR(storage) \
44 ((schar*)(storage)->top + (storage)->block_size - (storage)->free_space)
46 #define ICV_ALIGNED_SEQ_BLOCK_SIZE \
47 (int)cvAlign(sizeof(CvSeqBlock), CV_STRUCT_ALIGN)
50 cvAlignLeft( int size, int align )
55 #define CV_GET_LAST_ELEM( seq, block ) \
56 ((block)->data + ((block)->count - 1)*((seq)->elem_size))
58 #define CV_SWAP_ELEMS(a,b,elem_size) \
61 for( k = 0; k < elem_size; k++ ) \
70 #define ICV_SHIFT_TAB_MAX 32
71 static const schar icvPower2ShiftTab[] =
73 0, 1, -1, 2, -1, -1, -1, 3, -1, -1, -1, -1, -1, -1, -1, 4,
74 -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 5
77 /****************************************************************************************\
78 * Functions for manipulating memory storage - list of memory blocks *
79 \****************************************************************************************/
81 /* Initialize allocated storage: */
83 icvInitMemStorage( CvMemStorage* storage, int block_size )
85 CV_FUNCNAME( "icvInitMemStorage " );
90 CV_ERROR( CV_StsNullPtr, "" );
93 block_size = CV_STORAGE_BLOCK_SIZE;
95 block_size = cvAlign( block_size, CV_STRUCT_ALIGN );
96 assert( sizeof(CvMemBlock) % CV_STRUCT_ALIGN == 0 );
98 memset( storage, 0, sizeof( *storage ));
99 storage->signature = CV_STORAGE_MAGIC_VAL;
100 storage->block_size = block_size;
106 /* Create root memory storage: */
107 CV_IMPL CvMemStorage*
108 cvCreateMemStorage( int block_size )
110 CvMemStorage *storage = 0;
112 CV_FUNCNAME( "cvCreateMemStorage" );
116 CV_CALL( storage = (CvMemStorage *)cvAlloc( sizeof( CvMemStorage )));
117 CV_CALL( icvInitMemStorage( storage, block_size ));
121 if( cvGetErrStatus() < 0 )
128 /* Create child memory storage: */
129 CV_IMPL CvMemStorage *
130 cvCreateChildMemStorage( CvMemStorage * parent )
132 CvMemStorage *storage = 0;
133 CV_FUNCNAME( "cvCreateChildMemStorage" );
138 CV_ERROR( CV_StsNullPtr, "" );
140 CV_CALL( storage = cvCreateMemStorage(parent->block_size));
141 storage->parent = parent;
145 if( cvGetErrStatus() < 0 )
152 /* Release all blocks of the storage (or return them to parent, if any): */
154 icvDestroyMemStorage( CvMemStorage* storage )
156 CV_FUNCNAME( "icvDestroyMemStorage" );
163 CvMemBlock *dst_top = 0;
166 CV_ERROR( CV_StsNullPtr, "" );
168 if( storage->parent )
169 dst_top = storage->parent->top;
171 for( block = storage->bottom; block != 0; k++ )
173 CvMemBlock *temp = block;
176 if( storage->parent )
180 temp->prev = dst_top;
181 temp->next = dst_top->next;
183 temp->next->prev = temp;
184 dst_top = dst_top->next = temp;
188 dst_top = storage->parent->bottom = storage->parent->top = temp;
189 temp->prev = temp->next = 0;
190 storage->free_space = storage->block_size - sizeof( *temp );
199 storage->top = storage->bottom = 0;
200 storage->free_space = 0;
206 /* Release memory storage: */
208 cvReleaseMemStorage( CvMemStorage** storage )
211 CV_FUNCNAME( "cvReleaseMemStorage" );
216 CV_ERROR( CV_StsNullPtr, "" );
223 CV_CALL( icvDestroyMemStorage( st ));
231 /* Clears memory storage (return blocks to the parent, if any): */
233 cvClearMemStorage( CvMemStorage * storage )
235 CV_FUNCNAME( "cvClearMemStorage" );
240 CV_ERROR( CV_StsNullPtr, "" );
242 if( storage->parent )
244 icvDestroyMemStorage( storage );
248 storage->top = storage->bottom;
249 storage->free_space = storage->bottom ? storage->block_size - sizeof(CvMemBlock) : 0;
256 /* Moves stack pointer to next block.
257 If no blocks, allocate new one and link it to the storage: */
259 icvGoNextMemBlock( CvMemStorage * storage )
261 CV_FUNCNAME( "icvGoNextMemBlock" );
266 CV_ERROR( CV_StsNullPtr, "" );
268 if( !storage->top || !storage->top->next )
272 if( !(storage->parent) )
274 CV_CALL( block = (CvMemBlock *)cvAlloc( storage->block_size ));
278 CvMemStorage *parent = storage->parent;
279 CvMemStoragePos parent_pos;
281 cvSaveMemStoragePos( parent, &parent_pos );
282 CV_CALL( icvGoNextMemBlock( parent ));
285 cvRestoreMemStoragePos( parent, &parent_pos );
287 if( block == parent->top ) /* the single allocated block */
289 assert( parent->bottom == block );
290 parent->top = parent->bottom = 0;
291 parent->free_space = 0;
295 /* cut the block from the parent's list of blocks */
296 parent->top->next = block->next;
298 block->next->prev = parent->top;
304 block->prev = storage->top;
307 storage->top->next = block;
309 storage->top = storage->bottom = block;
312 if( storage->top->next )
313 storage->top = storage->top->next;
314 storage->free_space = storage->block_size - sizeof(CvMemBlock);
315 assert( storage->free_space % CV_STRUCT_ALIGN == 0 );
321 /* Remember memory storage position: */
323 cvSaveMemStoragePos( const CvMemStorage * storage, CvMemStoragePos * pos )
325 CV_FUNCNAME( "cvSaveMemStoragePos" );
329 if( !storage || !pos )
330 CV_ERROR( CV_StsNullPtr, "" );
332 pos->top = storage->top;
333 pos->free_space = storage->free_space;
339 /* Restore memory storage position: */
341 cvRestoreMemStoragePos( CvMemStorage * storage, CvMemStoragePos * pos )
343 CV_FUNCNAME( "cvRestoreMemStoragePos" );
347 if( !storage || !pos )
348 CV_ERROR( CV_StsNullPtr, "" );
349 if( pos->free_space > storage->block_size )
350 CV_ERROR( CV_StsBadSize, "" );
353 // this breaks icvGoNextMemBlock, so comment it off for now
354 if( storage->parent && (!pos->top || pos->top->next) )
356 CvMemBlock* save_bottom;
361 save_bottom = storage->bottom;
362 storage->bottom = pos->top->next;
364 storage->bottom->prev = 0;
366 icvDestroyMemStorage( storage );
367 storage->bottom = save_bottom;
370 storage->top = pos->top;
371 storage->free_space = pos->free_space;
375 storage->top = storage->bottom;
376 storage->free_space = storage->top ? storage->block_size - sizeof(CvMemBlock) : 0;
383 /* Allocate continuous buffer of the specified size in the storage: */
385 cvMemStorageAlloc( CvMemStorage* storage, size_t size )
389 CV_FUNCNAME( "cvMemStorageAlloc" );
394 CV_ERROR( CV_StsNullPtr, "NULL storage pointer" );
397 CV_ERROR( CV_StsOutOfRange, "Too large memory block is requested" );
399 assert( storage->free_space % CV_STRUCT_ALIGN == 0 );
401 if( (size_t)storage->free_space < size )
403 size_t max_free_space = cvAlignLeft(storage->block_size - sizeof(CvMemBlock), CV_STRUCT_ALIGN);
404 if( max_free_space < size )
405 CV_ERROR( CV_StsOutOfRange, "requested size is negative or too big" );
407 CV_CALL( icvGoNextMemBlock( storage ));
410 ptr = ICV_FREE_PTR(storage);
411 assert( (size_t)ptr % CV_STRUCT_ALIGN == 0 );
412 storage->free_space = cvAlignLeft(storage->free_space - (int)size, CV_STRUCT_ALIGN );
421 cvMemStorageAllocString( CvMemStorage* storage, const char* ptr, int len )
424 CV_FUNCNAME( "cvMemStorageAllocString" );
428 str.len = len >= 0 ? len : (int)strlen(ptr);
429 CV_CALL( str.ptr = (char*)cvMemStorageAlloc( storage, str.len + 1 ));
430 memcpy( str.ptr, ptr, str.len );
431 str.ptr[str.len] = '\0';
439 /****************************************************************************************\
440 * Sequence implementation *
441 \****************************************************************************************/
443 /* Create empty sequence: */
445 cvCreateSeq( int seq_flags, int header_size, int elem_size, CvMemStorage * storage )
449 CV_FUNCNAME( "cvCreateSeq" );
454 CV_ERROR( CV_StsNullPtr, "" );
455 if( header_size < (int)sizeof( CvSeq ) || elem_size <= 0 )
456 CV_ERROR( CV_StsBadSize, "" );
458 /* allocate sequence header */
459 CV_CALL( seq = (CvSeq*)cvMemStorageAlloc( storage, header_size ));
460 memset( seq, 0, header_size );
462 seq->header_size = header_size;
463 seq->flags = (seq_flags & ~CV_MAGIC_MASK) | CV_SEQ_MAGIC_VAL;
465 int elemtype = CV_MAT_TYPE(seq_flags);
466 int typesize = CV_ELEM_SIZE(elemtype);
468 if( elemtype != CV_SEQ_ELTYPE_GENERIC &&
469 typesize != 0 && typesize != elem_size )
470 CV_ERROR( CV_StsBadSize,
471 "Specified element size doesn't match to the size of the specified element type "
472 "(try to use 0 for element type)" );
474 seq->elem_size = elem_size;
475 seq->storage = storage;
477 CV_CALL( cvSetSeqBlockSize( seq, (1 << 10)/elem_size ));
485 /* adjusts <delta_elems> field of sequence. It determines how much the sequence
486 grows if there are no free space inside the sequence buffers */
488 cvSetSeqBlockSize( CvSeq *seq, int delta_elements )
491 int useful_block_size;
493 CV_FUNCNAME( "cvSetSeqBlockSize" );
497 if( !seq || !seq->storage )
498 CV_ERROR( CV_StsNullPtr, "" );
499 if( delta_elements < 0 )
500 CV_ERROR( CV_StsOutOfRange, "" );
502 useful_block_size = cvAlignLeft(seq->storage->block_size - sizeof(CvMemBlock) -
503 sizeof(CvSeqBlock), CV_STRUCT_ALIGN);
504 elem_size = seq->elem_size;
506 if( delta_elements == 0 )
508 delta_elements = (1 << 10) / elem_size;
509 delta_elements = MAX( delta_elements, 1 );
511 if( delta_elements * elem_size > useful_block_size )
513 delta_elements = useful_block_size / elem_size;
514 if( delta_elements == 0 )
515 CV_ERROR( CV_StsOutOfRange, "Storage block size is too small "
516 "to fit the sequence elements" );
519 seq->delta_elems = delta_elements;
525 /* Find a sequence element by its index: */
527 cvGetSeqElem( const CvSeq *seq, int index )
530 int count, total = seq->total;
532 if( (unsigned)index >= (unsigned)total )
534 index += index < 0 ? total : 0;
535 index -= index >= total ? total : 0;
536 if( (unsigned)index >= (unsigned)total )
541 if( index + index <= total )
543 while( index >= (count = block->count) )
554 total -= block->count;
556 while( index < total );
560 return block->data + index * seq->elem_size;
564 /* Calculate index of a sequence element: */
566 cvSeqElemIdx( const CvSeq* seq, const void* _element, CvSeqBlock** _block )
568 const schar *element = (const schar *)_element;
571 CvSeqBlock *first_block;
574 CV_FUNCNAME( "cvSeqElemIdx" );
578 if( !seq || !element )
579 CV_ERROR( CV_StsNullPtr, "" );
581 block = first_block = seq->first;
582 elem_size = seq->elem_size;
586 if( (unsigned)(element - block->data) < (unsigned) (block->count * elem_size) )
590 if( elem_size <= ICV_SHIFT_TAB_MAX && (id = icvPower2ShiftTab[elem_size - 1]) >= 0 )
591 id = (int)((size_t)(element - block->data) >> id);
593 id = (int)((size_t)(element - block->data) / elem_size);
594 id += block->start_index - seq->first->start_index;
598 if( block == first_block )
609 cvSliceLength( CvSlice slice, const CvSeq* seq )
611 int total = seq->total;
612 int length = slice.end_index - slice.start_index;
616 if( slice.start_index < 0 )
617 slice.start_index += total;
618 if( slice.end_index <= 0 )
619 slice.end_index += total;
621 length = slice.end_index - slice.start_index;
630 else if( length > total )
637 /* Copy all sequence elements into single continuous array: */
639 cvCvtSeqToArray( const CvSeq *seq, void *array, CvSlice slice )
641 CV_FUNCNAME( "cvCvtSeqToArray" );
645 int elem_size, total;
647 char *dst = (char*)array;
650 CV_ERROR( CV_StsNullPtr, "" );
652 elem_size = seq->elem_size;
653 total = cvSliceLength( slice, seq )*elem_size;
658 cvStartReadSeq( seq, &reader, 0 );
659 CV_CALL( cvSetSeqReaderPos( &reader, slice.start_index, 0 ));
663 int count = (int)(reader.block_max - reader.ptr);
667 memcpy( dst, reader.ptr, count );
669 reader.block = reader.block->next;
670 reader.ptr = reader.block->data;
671 reader.block_max = reader.ptr + reader.block->count*elem_size;
682 /* Construct a sequence from an array without copying any data.
683 NB: The resultant sequence cannot grow beyond its initial size: */
685 cvMakeSeqHeaderForArray( int seq_flags, int header_size, int elem_size,
686 void *array, int total, CvSeq *seq, CvSeqBlock * block )
690 CV_FUNCNAME( "cvMakeSeqHeaderForArray" );
694 if( elem_size <= 0 || header_size < (int)sizeof( CvSeq ) || total < 0 )
695 CV_ERROR( CV_StsBadSize, "" );
697 if( !seq || ((!array || !block) && total > 0) )
698 CV_ERROR( CV_StsNullPtr, "" );
700 memset( seq, 0, header_size );
702 seq->header_size = header_size;
703 seq->flags = (seq_flags & ~CV_MAGIC_MASK) | CV_SEQ_MAGIC_VAL;
705 int elemtype = CV_MAT_TYPE(seq_flags);
706 int typesize = CV_ELEM_SIZE(elemtype);
708 if( elemtype != CV_SEQ_ELTYPE_GENERIC &&
709 typesize != 0 && typesize != elem_size )
710 CV_ERROR( CV_StsBadSize,
711 "Element size doesn't match to the size of predefined element type "
712 "(try to use 0 for sequence element type)" );
714 seq->elem_size = elem_size;
716 seq->block_max = seq->ptr = (schar *) array + total * elem_size;
721 block->prev = block->next = block;
722 block->start_index = 0;
723 block->count = total;
724 block->data = (schar *) array;
735 /* The function allocates space for at least one more sequence element.
736 If there are free sequence blocks (seq->free_blocks != 0)
737 they are reused, otherwise the space is allocated in the storage: */
739 icvGrowSeq( CvSeq *seq, int in_front_of )
741 CV_FUNCNAME( "icvGrowSeq" );
748 CV_ERROR( CV_StsNullPtr, "" );
749 block = seq->free_blocks;
753 int elem_size = seq->elem_size;
754 int delta_elems = seq->delta_elems;
755 CvMemStorage *storage = seq->storage;
757 if( seq->total >= delta_elems*4 )
758 cvSetSeqBlockSize( seq, delta_elems*2 );
761 CV_ERROR( CV_StsNullPtr, "The sequence has NULL storage pointer" );
763 /* If there is a free space just after last allocated block
764 and it is big enough then enlarge the last block.
765 This can happen only if the new block is added to the end of sequence: */
766 if( (unsigned)(ICV_FREE_PTR(storage) - seq->block_max) < CV_STRUCT_ALIGN &&
767 storage->free_space >= seq->elem_size && !in_front_of )
769 int delta = storage->free_space / elem_size;
771 delta = MIN( delta, delta_elems ) * elem_size;
772 seq->block_max += delta;
773 storage->free_space = cvAlignLeft((int)(((schar*)storage->top + storage->block_size) -
774 seq->block_max), CV_STRUCT_ALIGN );
779 int delta = elem_size * delta_elems + ICV_ALIGNED_SEQ_BLOCK_SIZE;
781 /* Try to allocate <delta_elements> elements: */
782 if( storage->free_space < delta )
784 int small_block_size = MAX(1, delta_elems/3)*elem_size +
785 ICV_ALIGNED_SEQ_BLOCK_SIZE;
786 /* try to allocate smaller part */
787 if( storage->free_space >= small_block_size + CV_STRUCT_ALIGN )
789 delta = (storage->free_space - ICV_ALIGNED_SEQ_BLOCK_SIZE)/seq->elem_size;
790 delta = delta*seq->elem_size + ICV_ALIGNED_SEQ_BLOCK_SIZE;
794 CV_CALL( icvGoNextMemBlock( storage ));
795 assert( storage->free_space >= delta );
799 CV_CALL( block = (CvSeqBlock*)cvMemStorageAlloc( storage, delta ));
800 block->data = (schar*)cvAlignPtr( block + 1, CV_STRUCT_ALIGN );
801 block->count = delta - ICV_ALIGNED_SEQ_BLOCK_SIZE;
802 block->prev = block->next = 0;
807 seq->free_blocks = block->next;
813 block->prev = block->next = block;
817 block->prev = seq->first->prev;
818 block->next = seq->first;
819 block->prev->next = block->next->prev = block;
822 /* For free blocks the <count> field means
823 * total number of bytes in the block.
825 * For used blocks it means current number
826 * of sequence elements in the block:
828 assert( block->count % seq->elem_size == 0 && block->count > 0 );
832 seq->ptr = block->data;
833 seq->block_max = block->data + block->count;
834 block->start_index = block == block->prev ? 0 :
835 block->prev->start_index + block->prev->count;
839 int delta = block->count / seq->elem_size;
840 block->data += block->count;
842 if( block != block->prev )
844 assert( seq->first->start_index == 0 );
849 seq->block_max = seq->ptr = block->data;
852 block->start_index = 0;
856 block->start_index += delta;
858 if( block == seq->first )
868 /* Recycle a sequence block: */
870 icvFreeSeqBlock( CvSeq *seq, int in_front_of )
872 /*CV_FUNCNAME( "icvFreeSeqBlock" );*/
876 CvSeqBlock *block = seq->first;
878 assert( (in_front_of ? block : block->prev)->count == 0 );
880 if( block == block->prev ) /* single block case */
882 block->count = (int)(seq->block_max - block->data) + block->start_index * seq->elem_size;
883 block->data = seq->block_max - block->count;
885 seq->ptr = seq->block_max = 0;
893 assert( seq->ptr == block->data );
895 block->count = (int)(seq->block_max - seq->ptr);
896 seq->block_max = seq->ptr = block->prev->data +
897 block->prev->count * seq->elem_size;
901 int delta = block->start_index;
903 block->count = delta * seq->elem_size;
904 block->data -= block->count;
906 /* Update start indices of sequence blocks: */
909 block->start_index -= delta;
911 if( block == seq->first )
915 seq->first = block->next;
918 block->prev->next = block->next;
919 block->next->prev = block->prev;
922 assert( block->count > 0 && block->count % seq->elem_size == 0 );
923 block->next = seq->free_blocks;
924 seq->free_blocks = block;
930 /****************************************************************************************\
931 * Sequence Writer implementation *
932 \****************************************************************************************/
934 /* Initialize sequence writer: */
936 cvStartAppendToSeq( CvSeq *seq, CvSeqWriter * writer )
938 CV_FUNCNAME( "cvStartAppendToSeq" );
942 if( !seq || !writer )
943 CV_ERROR( CV_StsNullPtr, "" );
945 memset( writer, 0, sizeof( *writer ));
946 writer->header_size = sizeof( CvSeqWriter );
949 writer->block = seq->first ? seq->first->prev : 0;
950 writer->ptr = seq->ptr;
951 writer->block_max = seq->block_max;
957 /* Initialize sequence writer: */
959 cvStartWriteSeq( int seq_flags, int header_size,
960 int elem_size, CvMemStorage * storage, CvSeqWriter * writer )
964 CV_FUNCNAME( "cvStartWriteSeq" );
968 if( !storage || !writer )
969 CV_ERROR( CV_StsNullPtr, "" );
971 CV_CALL( seq = cvCreateSeq( seq_flags, header_size, elem_size, storage ));
972 cvStartAppendToSeq( seq, writer );
978 /* Update sequence header: */
980 cvFlushSeqWriter( CvSeqWriter * writer )
984 CV_FUNCNAME( "cvFlushSeqWriter" );
989 CV_ERROR( CV_StsNullPtr, "" );
992 seq->ptr = writer->ptr;
997 CvSeqBlock *first_block = writer->seq->first;
998 CvSeqBlock *block = first_block;
1000 writer->block->count = (int)((writer->ptr - writer->block->data) / seq->elem_size);
1001 assert( writer->block->count > 0 );
1005 total += block->count;
1006 block = block->next;
1008 while( block != first_block );
1010 writer->seq->total = total;
1017 /* Calls icvFlushSeqWriter and finishes writing process: */
1019 cvEndWriteSeq( CvSeqWriter * writer )
1023 CV_FUNCNAME( "cvEndWriteSeq" );
1028 CV_ERROR( CV_StsNullPtr, "" );
1030 CV_CALL( cvFlushSeqWriter( writer ));
1033 /* Truncate the last block: */
1034 if( writer->block && writer->seq->storage )
1036 CvMemStorage *storage = seq->storage;
1037 schar *storage_block_max = (schar *) storage->top + storage->block_size;
1039 assert( writer->block->count > 0 );
1041 if( (unsigned)((storage_block_max - storage->free_space)
1042 - seq->block_max) < CV_STRUCT_ALIGN )
1044 storage->free_space = cvAlignLeft((int)(storage_block_max - seq->ptr), CV_STRUCT_ALIGN);
1045 seq->block_max = seq->ptr;
1057 /* Create new sequence block: */
1059 cvCreateSeqBlock( CvSeqWriter * writer )
1061 CV_FUNCNAME( "cvCreateSeqBlock" );
1067 if( !writer || !writer->seq )
1068 CV_ERROR( CV_StsNullPtr, "" );
1072 cvFlushSeqWriter( writer );
1074 CV_CALL( icvGrowSeq( seq, 0 ));
1076 writer->block = seq->first->prev;
1077 writer->ptr = seq->ptr;
1078 writer->block_max = seq->block_max;
1084 /****************************************************************************************\
1085 * Sequence Reader implementation *
1086 \****************************************************************************************/
1088 /* Initialize sequence reader: */
1090 cvStartReadSeq( const CvSeq *seq, CvSeqReader * reader, int reverse )
1092 CvSeqBlock *first_block;
1093 CvSeqBlock *last_block;
1095 CV_FUNCNAME( "cvStartReadSeq" );
1101 reader->ptr = reader->block_max = reader->block_min = 0;
1106 if( !seq || !reader )
1107 CV_ERROR( CV_StsNullPtr, "" );
1109 reader->header_size = sizeof( CvSeqReader );
1110 reader->seq = (CvSeq*)seq;
1112 first_block = seq->first;
1116 last_block = first_block->prev;
1117 reader->ptr = first_block->data;
1118 reader->prev_elem = CV_GET_LAST_ELEM( seq, last_block );
1119 reader->delta_index = seq->first->start_index;
1123 schar *temp = reader->ptr;
1125 reader->ptr = reader->prev_elem;
1126 reader->prev_elem = temp;
1128 reader->block = last_block;
1132 reader->block = first_block;
1135 reader->block_min = reader->block->data;
1136 reader->block_max = reader->block_min + reader->block->count * seq->elem_size;
1140 reader->delta_index = 0;
1143 reader->ptr = reader->prev_elem = reader->block_min = reader->block_max = 0;
1150 /* Change the current reading block
1151 * to the previous or to the next:
1154 cvChangeSeqBlock( void* _reader, int direction )
1156 CV_FUNCNAME( "cvChangeSeqBlock" );
1160 CvSeqReader* reader = (CvSeqReader*)_reader;
1163 CV_ERROR( CV_StsNullPtr, "" );
1167 reader->block = reader->block->next;
1168 reader->ptr = reader->block->data;
1172 reader->block = reader->block->prev;
1173 reader->ptr = CV_GET_LAST_ELEM( reader->seq, reader->block );
1175 reader->block_min = reader->block->data;
1176 reader->block_max = reader->block_min + reader->block->count * reader->seq->elem_size;
1182 /* Return the current reader position: */
1184 cvGetSeqReaderPos( CvSeqReader* reader )
1189 CV_FUNCNAME( "cvGetSeqReaderPos" );
1193 if( !reader || !reader->ptr )
1194 CV_ERROR( CV_StsNullPtr, "" );
1196 elem_size = reader->seq->elem_size;
1197 if( elem_size <= ICV_SHIFT_TAB_MAX && (index = icvPower2ShiftTab[elem_size - 1]) >= 0 )
1198 index = (int)((reader->ptr - reader->block_min) >> index);
1200 index = (int)((reader->ptr - reader->block_min) / elem_size);
1202 index += reader->block->start_index - reader->delta_index;
1210 /* Set reader position to given position,
1211 * either absolute or relative to the
1215 cvSetSeqReaderPos( CvSeqReader* reader, int index, int is_relative )
1217 CV_FUNCNAME( "cvSetSeqReaderPos" );
1222 int elem_size, count, total;
1224 if( !reader || !reader->seq )
1225 CV_ERROR( CV_StsNullPtr, "" );
1227 total = reader->seq->total;
1228 elem_size = reader->seq->elem_size;
1234 if( index < -total )
1235 CV_ERROR( CV_StsOutOfRange, "" );
1238 else if( index >= total )
1241 if( index >= total )
1242 CV_ERROR( CV_StsOutOfRange, "" );
1245 block = reader->seq->first;
1246 if( index >= (count = block->count) )
1248 if( index + index <= total )
1252 block = block->next;
1255 while( index >= (count = block->count) );
1261 block = block->prev;
1262 total -= block->count;
1264 while( index < total );
1268 reader->ptr = block->data + index * elem_size;
1269 if( reader->block != block )
1271 reader->block = block;
1272 reader->block_min = block->data;
1273 reader->block_max = block->data + block->count * elem_size;
1278 schar* ptr = reader->ptr;
1280 block = reader->block;
1284 while( ptr + index >= reader->block_max )
1286 int delta = (int)(reader->block_max - ptr);
1288 reader->block = block = block->next;
1289 reader->block_min = ptr = block->data;
1290 reader->block_max = block->data + block->count*elem_size;
1292 reader->ptr = ptr + index;
1296 while( ptr + index < reader->block_min )
1298 int delta = (int)(ptr - reader->block_min);
1300 reader->block = block = block->prev;
1301 reader->block_min = block->data;
1302 reader->block_max = ptr = block->data + block->count*elem_size;
1304 reader->ptr = ptr + index;
1312 /* Push element onto the sequence: */
1314 cvSeqPush( CvSeq *seq, void *element )
1319 CV_FUNCNAME( "cvSeqPush" );
1324 CV_ERROR( CV_StsNullPtr, "" );
1326 elem_size = seq->elem_size;
1329 if( ptr >= seq->block_max )
1331 CV_CALL( icvGrowSeq( seq, 0 ));
1334 assert( ptr + elem_size <= seq->block_max /*&& ptr == seq->block_min */ );
1338 CV_MEMCPY_AUTO( ptr, element, elem_size );
1339 seq->first->prev->count++;
1341 seq->ptr = ptr + elem_size;
1349 /* Pop last element off of the sequence: */
1351 cvSeqPop( CvSeq *seq, void *element )
1356 CV_FUNCNAME( "cvSeqPop" );
1361 CV_ERROR( CV_StsNullPtr, "" );
1362 if( seq->total <= 0 )
1363 CV_ERROR( CV_StsBadSize, "" );
1365 elem_size = seq->elem_size;
1366 seq->ptr = ptr = seq->ptr - elem_size;
1369 CV_MEMCPY_AUTO( element, ptr, elem_size );
1373 if( --(seq->first->prev->count) == 0 )
1375 icvFreeSeqBlock( seq, 0 );
1376 assert( seq->ptr == seq->block_max );
1383 /* Push element onto the front of the sequence: */
1385 cvSeqPushFront( CvSeq *seq, void *element )
1391 CV_FUNCNAME( "cvSeqPushFront" );
1396 CV_ERROR( CV_StsNullPtr, "" );
1398 elem_size = seq->elem_size;
1401 if( !block || block->start_index == 0 )
1403 CV_CALL( icvGrowSeq( seq, 1 ));
1406 assert( block->start_index > 0 );
1409 ptr = block->data -= elem_size;
1412 CV_MEMCPY_AUTO( ptr, element, elem_size );
1414 block->start_index--;
1423 /* Shift out first element of the sequence: */
1425 cvSeqPopFront( CvSeq *seq, void *element )
1430 CV_FUNCNAME( "cvSeqPopFront" );
1435 CV_ERROR( CV_StsNullPtr, "" );
1436 if( seq->total <= 0 )
1437 CV_ERROR( CV_StsBadSize, "" );
1439 elem_size = seq->elem_size;
1443 CV_MEMCPY_AUTO( element, block->data, elem_size );
1444 block->data += elem_size;
1445 block->start_index++;
1448 if( --(block->count) == 0 )
1450 icvFreeSeqBlock( seq, 1 );
1456 /* Insert new element in middle of sequence: */
1458 cvSeqInsert( CvSeq *seq, int before_index, void *element )
1467 CV_FUNCNAME( "cvSeqInsert" );
1472 CV_ERROR( CV_StsNullPtr, "" );
1475 before_index += before_index < 0 ? total : 0;
1476 before_index -= before_index > total ? total : 0;
1478 if( (unsigned)before_index > (unsigned)total )
1479 CV_ERROR( CV_StsOutOfRange, "" );
1481 if( before_index == total )
1483 CV_CALL( ret_ptr = cvSeqPush( seq, element ));
1485 else if( before_index == 0 )
1487 CV_CALL( ret_ptr = cvSeqPushFront( seq, element ));
1491 elem_size = seq->elem_size;
1493 if( before_index >= total >> 1 )
1495 schar *ptr = seq->ptr + elem_size;
1497 if( ptr > seq->block_max )
1499 CV_CALL( icvGrowSeq( seq, 0 ));
1501 ptr = seq->ptr + elem_size;
1502 assert( ptr <= seq->block_max );
1505 delta_index = seq->first->start_index;
1506 block = seq->first->prev;
1508 block_size = (int)(ptr - block->data);
1510 while( before_index < block->start_index - delta_index )
1512 CvSeqBlock *prev_block = block->prev;
1514 memmove( block->data + elem_size, block->data, block_size - elem_size );
1515 block_size = prev_block->count * elem_size;
1516 memcpy( block->data, prev_block->data + block_size - elem_size, elem_size );
1519 /* Check that we don't fall into an infinite loop: */
1520 assert( block != seq->first->prev );
1523 before_index = (before_index - block->start_index + delta_index) * elem_size;
1524 memmove( block->data + before_index + elem_size, block->data + before_index,
1525 block_size - before_index - elem_size );
1527 ret_ptr = block->data + before_index;
1530 memcpy( ret_ptr, element, elem_size );
1537 if( block->start_index == 0 )
1539 CV_CALL( icvGrowSeq( seq, 1 ));
1544 delta_index = block->start_index;
1546 block->start_index--;
1547 block->data -= elem_size;
1549 while( before_index > block->start_index - delta_index + block->count )
1551 CvSeqBlock *next_block = block->next;
1553 block_size = block->count * elem_size;
1554 memmove( block->data, block->data + elem_size, block_size - elem_size );
1555 memcpy( block->data + block_size - elem_size, next_block->data, elem_size );
1558 /* Check that we don't fall into an infinite loop: */
1559 assert( block != seq->first );
1562 before_index = (before_index - block->start_index + delta_index) * elem_size;
1563 memmove( block->data, block->data + elem_size, before_index - elem_size );
1565 ret_ptr = block->data + before_index - elem_size;
1568 memcpy( ret_ptr, element, elem_size );
1571 seq->total = total + 1;
1580 /* Removes element from sequence: */
1582 cvSeqRemove( CvSeq *seq, int index )
1589 int total, front = 0;
1591 CV_FUNCNAME( "cvSeqRemove" );
1596 CV_ERROR( CV_StsNullPtr, "" );
1600 index += index < 0 ? total : 0;
1601 index -= index >= total ? total : 0;
1603 if( (unsigned) index >= (unsigned) total )
1604 CV_ERROR( CV_StsOutOfRange, "Invalid index" );
1606 if( index == total - 1 )
1610 else if( index == 0 )
1612 cvSeqPopFront( seq, 0 );
1617 elem_size = seq->elem_size;
1618 delta_index = block->start_index;
1619 while( block->start_index - delta_index + block->count <= index )
1620 block = block->next;
1622 ptr = block->data + (index - block->start_index + delta_index) * elem_size;
1624 front = index < total >> 1;
1627 block_size = block->count * elem_size - (int)(ptr - block->data);
1629 while( block != seq->first->prev ) /* while not the last block */
1631 CvSeqBlock *next_block = block->next;
1633 memmove( ptr, ptr + elem_size, block_size - elem_size );
1634 memcpy( ptr + block_size - elem_size, next_block->data, elem_size );
1637 block_size = block->count * elem_size;
1640 memmove( ptr, ptr + elem_size, block_size - elem_size );
1641 seq->ptr -= elem_size;
1646 block_size = (int)(ptr - block->data);
1648 while( block != seq->first )
1650 CvSeqBlock *prev_block = block->prev;
1652 memmove( block->data + elem_size, block->data, block_size - elem_size );
1653 block_size = prev_block->count * elem_size;
1654 memcpy( block->data, prev_block->data + block_size - elem_size, elem_size );
1658 memmove( block->data + elem_size, block->data, block_size - elem_size );
1659 block->data += elem_size;
1660 block->start_index++;
1663 seq->total = total - 1;
1664 if( --block->count == 0 )
1665 icvFreeSeqBlock( seq, front );
1672 /* Add several elements to the beginning or end of a sequence: */
1674 cvSeqPushMulti( CvSeq *seq, void *_elements, int count, int front )
1676 char *elements = (char *) _elements;
1678 CV_FUNCNAME( "cvSeqPushMulti" );
1684 CV_ERROR( CV_StsNullPtr, "NULL sequence pointer" );
1686 CV_ERROR( CV_StsBadSize, "number of removed elements is negative" );
1688 elem_size = seq->elem_size;
1694 int delta = (int)((seq->block_max - seq->ptr) / elem_size);
1696 delta = MIN( delta, count );
1699 seq->first->prev->count += delta;
1700 seq->total += delta;
1705 memcpy( seq->ptr, elements, delta );
1712 CV_CALL( icvGrowSeq( seq, 0 ));
1717 CvSeqBlock* block = seq->first;
1723 if( !block || block->start_index == 0 )
1725 CV_CALL( icvGrowSeq( seq, 1 ));
1728 assert( block->start_index > 0 );
1731 delta = MIN( block->start_index, count );
1733 block->start_index -= delta;
1734 block->count += delta;
1735 seq->total += delta;
1737 block->data -= delta;
1740 memcpy( block->data, elements + count*elem_size, delta );
1748 /* Remove several elements from the end of sequence: */
1750 cvSeqPopMulti( CvSeq *seq, void *_elements, int count, int front )
1752 char *elements = (char *) _elements;
1754 CV_FUNCNAME( "cvSeqPopMulti" );
1759 CV_ERROR( CV_StsNullPtr, "NULL sequence pointer" );
1761 CV_ERROR( CV_StsBadSize, "number of removed elements is negative" );
1763 count = MIN( count, seq->total );
1768 elements += count * seq->elem_size;
1772 int delta = seq->first->prev->count;
1774 delta = MIN( delta, count );
1775 assert( delta > 0 );
1777 seq->first->prev->count -= delta;
1778 seq->total -= delta;
1780 delta *= seq->elem_size;
1786 memcpy( elements, seq->ptr, delta );
1789 if( seq->first->prev->count == 0 )
1790 icvFreeSeqBlock( seq, 0 );
1797 int delta = seq->first->count;
1799 delta = MIN( delta, count );
1800 assert( delta > 0 );
1802 seq->first->count -= delta;
1803 seq->total -= delta;
1805 seq->first->start_index += delta;
1806 delta *= seq->elem_size;
1810 memcpy( elements, seq->first->data, delta );
1814 seq->first->data += delta;
1815 if( seq->first->count == 0 )
1816 icvFreeSeqBlock( seq, 1 );
1824 /* Remove all elements from a sequence: */
1826 cvClearSeq( CvSeq *seq )
1828 CV_FUNCNAME( "cvClearSeq" );
1833 CV_ERROR( CV_StsNullPtr, "" );
1834 cvSeqPopMulti( seq, 0, seq->total );
1841 cvSeqSlice( const CvSeq* seq, CvSlice slice, CvMemStorage* storage, int copy_data )
1845 CV_FUNCNAME("cvSeqSlice");
1849 int elem_size, count, length;
1851 CvSeqBlock *block, *first_block = 0, *last_block = 0;
1853 if( !CV_IS_SEQ(seq) )
1854 CV_ERROR( CV_StsBadArg, "Invalid sequence header" );
1858 storage = seq->storage;
1860 CV_ERROR( CV_StsNullPtr, "NULL storage pointer" );
1863 elem_size = seq->elem_size;
1864 length = cvSliceLength( slice, seq );
1865 if( slice.start_index < 0 )
1866 slice.start_index += seq->total;
1867 else if( slice.start_index >= seq->total )
1868 slice.start_index -= seq->total;
1869 if( (unsigned)length > (unsigned)seq->total ||
1870 ((unsigned)slice.start_index >= (unsigned)seq->total && length != 0) )
1871 CV_ERROR( CV_StsOutOfRange, "Bad sequence slice" );
1873 CV_CALL( subseq = cvCreateSeq( seq->flags, seq->header_size, elem_size, storage ));
1877 cvStartReadSeq( seq, &reader, 0 );
1878 cvSetSeqReaderPos( &reader, slice.start_index, 0 );
1879 count = (int)((reader.block_max - reader.ptr)/elem_size);
1883 int bl = MIN( count, length );
1887 block = (CvSeqBlock*)cvMemStorageAlloc( storage, sizeof(*block) );
1890 first_block = subseq->first = block->prev = block->next = block;
1891 block->start_index = 0;
1895 block->prev = last_block;
1896 block->next = first_block;
1897 last_block->next = first_block->prev = block;
1898 block->start_index = last_block->start_index + last_block->count;
1901 block->data = reader.ptr;
1903 subseq->total += bl;
1906 cvSeqPushMulti( subseq, reader.ptr, bl, 0 );
1908 reader.block = reader.block->next;
1909 reader.ptr = reader.block->data;
1910 count = reader.block->count;
1912 while( length > 0 );
1921 // Remove slice from the middle of the sequence.
1922 // !!! TODO !!! Implement more efficient algorithm
1924 cvSeqRemoveSlice( CvSeq* seq, CvSlice slice )
1926 CV_FUNCNAME("cvSeqRemoveSlice");
1932 if( !CV_IS_SEQ(seq) )
1933 CV_ERROR( CV_StsBadArg, "Invalid sequence header" );
1935 length = cvSliceLength( slice, seq );
1938 if( slice.start_index < 0 )
1939 slice.start_index += total;
1940 else if( slice.start_index >= total )
1941 slice.start_index -= total;
1943 if( (unsigned)slice.start_index >= (unsigned)total )
1944 CV_ERROR( CV_StsOutOfRange, "start slice index is out of range" );
1946 slice.end_index = slice.start_index + length;
1948 if( slice.end_index < total )
1950 CvSeqReader reader_to, reader_from;
1951 int elem_size = seq->elem_size;
1953 cvStartReadSeq( seq, &reader_to );
1954 cvStartReadSeq( seq, &reader_from );
1956 if( slice.start_index > total - slice.end_index )
1958 int i, count = seq->total - slice.end_index;
1959 cvSetSeqReaderPos( &reader_to, slice.start_index );
1960 cvSetSeqReaderPos( &reader_from, slice.end_index );
1962 for( i = 0; i < count; i++ )
1964 CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );
1965 CV_NEXT_SEQ_ELEM( elem_size, reader_to );
1966 CV_NEXT_SEQ_ELEM( elem_size, reader_from );
1969 cvSeqPopMulti( seq, 0, slice.end_index - slice.start_index );
1973 int i, count = slice.start_index;
1974 cvSetSeqReaderPos( &reader_to, slice.end_index );
1975 cvSetSeqReaderPos( &reader_from, slice.start_index );
1977 for( i = 0; i < count; i++ )
1979 CV_PREV_SEQ_ELEM( elem_size, reader_to );
1980 CV_PREV_SEQ_ELEM( elem_size, reader_from );
1982 CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );
1985 cvSeqPopMulti( seq, 0, slice.end_index - slice.start_index, 1 );
1990 cvSeqPopMulti( seq, 0, total - slice.start_index );
1991 cvSeqPopMulti( seq, 0, slice.end_index - total, 1 );
1998 // Insert a sequence into the middle of another sequence:
1999 // !!! TODO !!! Implement more efficient algorithm
2001 cvSeqInsertSlice( CvSeq* seq, int index, const CvArr* from_arr )
2003 CvSeqReader reader_to, reader_from;
2004 int i, elem_size, total, from_total;
2006 CV_FUNCNAME("cvSeqInsertSlice");
2010 CvSeq from_header, *from = (CvSeq*)from_arr;
2013 if( !CV_IS_SEQ(seq) )
2014 CV_ERROR( CV_StsBadArg, "Invalid destination sequence header" );
2016 if( !CV_IS_SEQ(from))
2018 CvMat* mat = (CvMat*)from;
2019 if( !CV_IS_MAT(mat))
2020 CV_ERROR( CV_StsBadArg, "Source is not a sequence nor matrix" );
2022 if( !CV_IS_MAT_CONT(mat->type) || (mat->rows != 1 && mat->cols != 1) )
2023 CV_ERROR( CV_StsBadArg, "The source array must be 1d coninuous vector" );
2025 CV_CALL( from = cvMakeSeqHeaderForArray( CV_SEQ_KIND_GENERIC, sizeof(from_header),
2026 CV_ELEM_SIZE(mat->type),
2027 mat->data.ptr, mat->cols + mat->rows - 1,
2028 &from_header, &block ));
2031 if( seq->elem_size != from->elem_size )
2032 CV_ERROR( CV_StsUnmatchedSizes,
2033 "Source and destination sequence element sizes are different." );
2035 from_total = from->total;
2037 if( from_total == 0 )
2041 index += index < 0 ? total : 0;
2042 index -= index > total ? total : 0;
2044 if( (unsigned)index > (unsigned)total )
2045 CV_ERROR( CV_StsOutOfRange, "" );
2047 elem_size = seq->elem_size;
2049 if( index < (total >> 1) )
2051 cvSeqPushMulti( seq, 0, from_total, 1 );
2053 cvStartReadSeq( seq, &reader_to );
2054 cvStartReadSeq( seq, &reader_from );
2055 cvSetSeqReaderPos( &reader_from, from_total );
2057 for( i = 0; i < index; i++ )
2059 CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );
2060 CV_NEXT_SEQ_ELEM( elem_size, reader_to );
2061 CV_NEXT_SEQ_ELEM( elem_size, reader_from );
2066 cvSeqPushMulti( seq, 0, from_total );
2068 cvStartReadSeq( seq, &reader_to );
2069 cvStartReadSeq( seq, &reader_from );
2070 cvSetSeqReaderPos( &reader_from, total );
2071 cvSetSeqReaderPos( &reader_to, seq->total );
2073 for( i = 0; i < total - index; i++ )
2075 CV_PREV_SEQ_ELEM( elem_size, reader_to );
2076 CV_PREV_SEQ_ELEM( elem_size, reader_from );
2077 CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );
2081 cvStartReadSeq( from, &reader_from );
2082 cvSetSeqReaderPos( &reader_to, index );
2084 for( i = 0; i < from_total; i++ )
2086 CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );
2087 CV_NEXT_SEQ_ELEM( elem_size, reader_to );
2088 CV_NEXT_SEQ_ELEM( elem_size, reader_from );
2094 // Sort the sequence using user-specified comparison function.
2095 // The semantics is similar to qsort() function.
2096 // The code is based on BSD system qsort():
2097 // * Copyright (c) 1992, 1993
2098 // * The Regents of the University of California. All rights reserved.
2100 // * Redistribution and use in source and binary forms, with or without
2101 // * modification, are permitted provided that the following conditions
2103 // * 1. Redistributions of source code must retain the above copyright
2104 // * notice, this list of conditions and the following disclaimer.
2105 // * 2. Redistributions in binary form must reproduce the above copyright
2106 // * notice, this list of conditions and the following disclaimer in the
2107 // * documentation and/or other materials provided with the distribution.
2108 // * 3. All advertising materials mentioning features or use of this software
2109 // * must display the following acknowledgement:
2110 // * This product includes software developed by the University of
2111 // * California, Berkeley and its contributors.
2112 // * 4. Neither the name of the University nor the names of its contributors
2113 // * may be used to endorse or promote products derived from this software
2114 // * without specific prior written permission.
2116 // * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
2117 // * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
2118 // * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
2119 // * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
2120 // * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
2121 // * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
2122 // * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
2123 // * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
2124 // * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
2125 // * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
2128 typedef struct CvSeqReaderPos
2137 #define CV_SAVE_READER_POS( reader, pos ) \
2139 (pos).block = (reader).block; \
2140 (pos).ptr = (reader).ptr; \
2141 (pos).block_min = (reader).block_min; \
2142 (pos).block_max = (reader).block_max; \
2145 #define CV_RESTORE_READER_POS( reader, pos )\
2147 (reader).block = (pos).block; \
2148 (reader).ptr = (pos).ptr; \
2149 (reader).block_min = (pos).block_min; \
2150 (reader).block_max = (pos).block_max; \
2154 icvMed3( schar* a, schar* b, schar* c, CvCmpFunc cmp_func, void* aux )
2156 return cmp_func(a, b, aux) < 0 ?
2157 (cmp_func(b, c, aux) < 0 ? b : cmp_func(a, c, aux) < 0 ? c : a)
2158 :(cmp_func(b, c, aux) > 0 ? b : cmp_func(a, c, aux) < 0 ? a : c);
2162 cvSeqSort( CvSeq* seq, CvCmpFunc cmp_func, void* aux )
2165 int isort_thresh = 7;
2166 CvSeqReader left, right;
2176 CV_FUNCNAME( "cvSeqSort" );
2180 if( !CV_IS_SEQ(seq) )
2181 CV_ERROR( !seq ? CV_StsNullPtr : CV_StsBadArg, "Bad input sequence" );
2184 CV_ERROR( CV_StsNullPtr, "Null compare function" );
2186 if( seq->total <= 1 )
2189 elem_size = seq->elem_size;
2190 isort_thresh *= elem_size;
2192 cvStartReadSeq( seq, &left, 0 );
2194 CV_SAVE_READER_POS( left, stack[0].lb );
2195 CV_PREV_SEQ_ELEM( elem_size, right );
2196 CV_SAVE_READER_POS( right, stack[0].ub );
2200 CV_RESTORE_READER_POS( left, stack[sp].lb );
2201 CV_RESTORE_READER_POS( right, stack[sp].ub );
2207 CvSeqReader ptr, ptr2;
2209 if( left.block == right.block )
2210 n = (int)(right.ptr - left.ptr) + elem_size;
2213 n = cvGetSeqReaderPos( &right );
2214 n = (n - cvGetSeqReaderPos( &left ) + 1)*elem_size;
2217 if( n <= isort_thresh )
2221 CV_NEXT_SEQ_ELEM( elem_size, ptr );
2222 CV_NEXT_SEQ_ELEM( elem_size, right );
2223 while( ptr.ptr != right.ptr )
2226 if( ptr2.block != ptr.block )
2228 ptr2.block = ptr.block;
2229 ptr2.block_min = ptr.block_min;
2230 ptr2.block_max = ptr.block_max;
2232 while( ptr2.ptr != left.ptr )
2234 schar* cur = ptr2.ptr;
2235 CV_PREV_SEQ_ELEM( elem_size, ptr2 );
2236 if( cmp_func( ptr2.ptr, cur, aux ) <= 0 )
2238 CV_SWAP_ELEMS( ptr2.ptr, cur, elem_size );
2240 CV_NEXT_SEQ_ELEM( elem_size, ptr );
2246 CvSeqReader left0, left1, right0, right1;
2247 CvSeqReader tmp0, tmp1;
2248 schar *m1, *m2, *m3, *pivot;
2250 int l, l0, l1, r, r0, r1;
2252 left0 = tmp0 = left;
2253 right0 = right1 = right;
2259 schar *p1, *p2, *p3;
2261 cvSetSeqReaderPos( &tmp0, d, 1 );
2263 cvSetSeqReaderPos( &tmp0, d, 1 );
2265 m1 = icvMed3( p1, p2, p3, cmp_func, aux );
2266 cvSetSeqReaderPos( &tmp0, (n/2) - d*3, 1 );
2268 cvSetSeqReaderPos( &tmp0, d, 1 );
2270 cvSetSeqReaderPos( &tmp0, d, 1 );
2272 m2 = icvMed3( p1, p2, p3, cmp_func, aux );
2273 cvSetSeqReaderPos( &tmp0, n - 1 - d*3 - n/2, 1 );
2275 cvSetSeqReaderPos( &tmp0, d, 1 );
2277 cvSetSeqReaderPos( &tmp0, d, 1 );
2279 m3 = icvMed3( p1, p2, p3, cmp_func, aux );
2284 cvSetSeqReaderPos( &tmp0, n/2, 1 );
2286 cvSetSeqReaderPos( &tmp0, n - 1 - n/2, 1 );
2290 pivot = icvMed3( m1, m2, m3, cmp_func, aux );
2292 if( pivot != left.ptr )
2294 CV_SWAP_ELEMS( pivot, left.ptr, elem_size );
2297 CV_NEXT_SEQ_ELEM( elem_size, left );
2302 while( left.ptr != right.ptr && (r = cmp_func(left.ptr, pivot, aux)) <= 0 )
2306 if( left1.ptr != left.ptr )
2307 CV_SWAP_ELEMS( left1.ptr, left.ptr, elem_size );
2309 CV_NEXT_SEQ_ELEM( elem_size, left1 );
2311 CV_NEXT_SEQ_ELEM( elem_size, left );
2314 while( left.ptr != right.ptr && (r = cmp_func(right.ptr,pivot, aux)) >= 0 )
2318 if( right1.ptr != right.ptr )
2319 CV_SWAP_ELEMS( right1.ptr, right.ptr, elem_size );
2321 CV_PREV_SEQ_ELEM( elem_size, right1 );
2323 CV_PREV_SEQ_ELEM( elem_size, right );
2326 if( left.ptr == right.ptr )
2328 r = cmp_func(left.ptr, pivot, aux);
2331 if( left1.ptr != left.ptr )
2332 CV_SWAP_ELEMS( left1.ptr, left.ptr, elem_size );
2334 CV_NEXT_SEQ_ELEM( elem_size, left1 );
2338 CV_NEXT_SEQ_ELEM( elem_size, left );
2342 CV_PREV_SEQ_ELEM( elem_size, right );
2347 CV_SWAP_ELEMS( left.ptr, right.ptr, elem_size );
2348 CV_NEXT_SEQ_ELEM( elem_size, left );
2349 r = left.ptr == right.ptr;
2350 CV_PREV_SEQ_ELEM( elem_size, right );
2358 left = left0, right = right0;
2362 l = cvGetSeqReaderPos( &left );
2365 l0 = cvGetSeqReaderPos( &left0 );
2366 l1 = cvGetSeqReaderPos( &left1 );
2370 n = MIN( l - l1, l1 - l0 );
2375 cvSetSeqReaderPos( &tmp1, 0-n, 1 );
2376 for( i = 0; i < n; i++ )
2378 CV_SWAP_ELEMS( tmp0.ptr, tmp1.ptr, elem_size );
2379 CV_NEXT_SEQ_ELEM( elem_size, tmp0 );
2380 CV_NEXT_SEQ_ELEM( elem_size, tmp1 );
2384 r = cvGetSeqReaderPos( &right );
2385 r0 = cvGetSeqReaderPos( &right0 );
2386 r1 = cvGetSeqReaderPos( &right1 );
2387 m = MIN( r0 - r1, r1 - r );
2392 cvSetSeqReaderPos( &tmp1, 1-m, 1 );
2393 for( i = 0; i < m; i++ )
2395 CV_SWAP_ELEMS( tmp0.ptr, tmp1.ptr, elem_size );
2396 CV_NEXT_SEQ_ELEM( elem_size, tmp0 );
2397 CV_NEXT_SEQ_ELEM( elem_size, tmp1 );
2410 CV_SAVE_READER_POS( left0, stack[sp].lb );
2411 cvSetSeqReaderPos( &left0, n - 1, 1 );
2412 CV_SAVE_READER_POS( left0, stack[sp].ub );
2413 left = right = right0;
2414 cvSetSeqReaderPos( &left, 1 - m, 1 );
2419 CV_SAVE_READER_POS( right0, stack[sp].ub );
2420 cvSetSeqReaderPos( &right0, 1 - m, 1 );
2421 CV_SAVE_READER_POS( right0, stack[sp].lb );
2422 left = right = left0;
2423 cvSetSeqReaderPos( &right, n - 1, 1 );
2428 left = right = left0;
2429 cvSetSeqReaderPos( &right, n - 1, 1 );
2434 left = right = right0;
2435 cvSetSeqReaderPos( &left, 1 - m, 1 );
2448 cvSeqSearch( CvSeq* seq, const void* _elem, CvCmpFunc cmp_func,
2449 int is_sorted, int* _idx, void* userdata )
2452 const schar* elem = (const schar*)_elem;
2455 CV_FUNCNAME("cvSeqSearch");
2459 int elem_size, i, j, total;
2461 if( !CV_IS_SEQ(seq) )
2462 CV_ERROR( !seq ? CV_StsNullPtr : CV_StsBadArg, "Bad input sequence" );
2465 CV_ERROR( CV_StsNullPtr, "Null element pointer" );
2467 elem_size = seq->elem_size;
2476 cvStartReadSeq( seq, &reader, 0 );
2480 for( i = 0; i < total; i++ )
2482 if( cmp_func( elem, reader.ptr, userdata ) == 0 )
2484 CV_NEXT_SEQ_ELEM( elem_size, reader );
2487 else if( (elem_size & (sizeof(int)-1)) == 0 )
2489 for( i = 0; i < total; i++ )
2491 for( j = 0; j < elem_size; j += sizeof(int) )
2493 if( *(const int*)(reader.ptr + j) != *(const int*)(elem + j) )
2496 if( j == elem_size )
2498 CV_NEXT_SEQ_ELEM( elem_size, reader );
2503 for( i = 0; i < total; i++ )
2505 for( j = 0; j < elem_size; j++ )
2507 if( reader.ptr[j] != elem[j] )
2510 if( j == elem_size )
2512 CV_NEXT_SEQ_ELEM( elem_size, reader );
2518 result = reader.ptr;
2523 CV_ERROR( CV_StsNullPtr, "Null compare function" );
2529 int k = (i+j)>>1, code;
2530 schar* ptr = cvGetSeqElem( seq, k );
2531 code = cmp_func( elem, ptr, userdata );
2556 cvSeqInvert( CvSeq* seq )
2558 CV_FUNCNAME( "cvSeqInvert" );
2562 CvSeqReader left_reader, right_reader;
2566 CV_CALL( cvStartReadSeq( seq, &left_reader, 0 ));
2567 CV_CALL( cvStartReadSeq( seq, &right_reader, 1 ));
2568 elem_size = seq->elem_size;
2569 count = seq->total >> 1;
2571 for( i = 0; i < count; i++ )
2573 CV_SWAP_ELEMS( left_reader.ptr, right_reader.ptr, elem_size );
2574 CV_NEXT_SEQ_ELEM( elem_size, left_reader );
2575 CV_PREV_SEQ_ELEM( elem_size, right_reader );
2582 typedef struct CvPTreeNode
2584 struct CvPTreeNode* parent;
2591 // This function splits the input sequence or set into one or more equivalence classes.
2592 // is_equal(a,b,...) returns non-zero if the two sequence elements
2593 // belong to the same class. The function returns sequence of integers -
2594 // 0-based class indexes for each element.
2596 // The algorithm is described in "Introduction to Algorithms"
2597 // by Cormen, Leiserson and Rivest, chapter "Data structures for disjoint sets"
2599 cvSeqPartition( const CvSeq* seq, CvMemStorage* storage, CvSeq** labels,
2600 CvCmpFunc is_equal, void* userdata )
2603 CvMemStorage* temp_storage = 0;
2606 CV_FUNCNAME( "cvSeqPartition" );
2611 CvSeqReader reader, reader0;
2617 CV_ERROR( CV_StsNullPtr, "" );
2619 if( !seq || !is_equal )
2620 CV_ERROR( CV_StsNullPtr, "" );
2623 storage = seq->storage;
2626 CV_ERROR( CV_StsNullPtr, "" );
2628 is_set = CV_IS_SET(seq);
2630 temp_storage = cvCreateChildMemStorage( storage );
2632 nodes = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvPTreeNode), temp_storage );
2634 cvStartReadSeq( seq, &reader );
2635 memset( &writer, 0, sizeof(writer));
2636 cvStartAppendToSeq( nodes, &writer );
2638 // Initial O(N) pass. Make a forest of single-vertex trees.
2639 for( i = 0; i < seq->total; i++ )
2641 CvPTreeNode node = { 0, 0, 0 };
2642 if( !is_set || CV_IS_SET_ELEM( reader.ptr ))
2643 node.element = reader.ptr;
2644 CV_WRITE_SEQ_ELEM( node, writer );
2645 CV_NEXT_SEQ_ELEM( seq->elem_size, reader );
2648 cvEndWriteSeq( &writer );
2650 // Because in the next loop we will iterate
2651 // through all the sequence nodes each time,
2652 // we do not need to initialize reader every time:
2653 cvStartReadSeq( nodes, &reader );
2654 cvStartReadSeq( nodes, &reader0 );
2656 // The main O(N^2) pass. Merge connected components.
2657 for( i = 0; i < nodes->total; i++ )
2659 CvPTreeNode* node = (CvPTreeNode*)(reader0.ptr);
2660 CvPTreeNode* root = node;
2661 CV_NEXT_SEQ_ELEM( nodes->elem_size, reader0 );
2663 if( !node->element )
2667 while( root->parent )
2668 root = root->parent;
2670 for( j = 0; j < nodes->total; j++ )
2672 CvPTreeNode* node2 = (CvPTreeNode*)reader.ptr;
2674 if( node2->element && node2 != node &&
2675 is_equal( node->element, node2->element, userdata ))
2677 CvPTreeNode* root2 = node2;
2680 while( root2->parent )
2681 root2 = root2->parent;
2685 if( root->rank > root2->rank )
2686 root2->parent = root;
2689 root->parent = root2;
2690 root2->rank += root->rank == root2->rank;
2693 assert( root->parent == 0 );
2695 // Compress path from node2 to the root:
2696 while( node2->parent )
2698 CvPTreeNode* temp = node2;
2699 node2 = node2->parent;
2700 temp->parent = root;
2703 // Compress path from node to the root:
2705 while( node2->parent )
2707 CvPTreeNode* temp = node2;
2708 node2 = node2->parent;
2709 temp->parent = root;
2714 CV_NEXT_SEQ_ELEM( sizeof(*node), reader );
2718 // Final O(N) pass (Enumerate classes)
2719 // Reuse reader one more time
2720 result = cvCreateSeq( 0, sizeof(CvSeq), sizeof(int), storage );
2721 cvStartAppendToSeq( result, &writer );
2723 for( i = 0; i < nodes->total; i++ )
2725 CvPTreeNode* node = (CvPTreeNode*)reader.ptr;
2730 while( node->parent )
2731 node = node->parent;
2732 if( node->rank >= 0 )
2733 node->rank = ~class_idx++;
2737 CV_NEXT_SEQ_ELEM( sizeof(*node), reader );
2738 CV_WRITE_SEQ_ELEM( idx, writer );
2741 cvEndWriteSeq( &writer );
2748 cvReleaseMemStorage( &temp_storage );
2753 /****************************************************************************************\
2754 * Set implementation *
2755 \****************************************************************************************/
2757 /* Creates empty set: */
2759 cvCreateSet( int set_flags, int header_size, int elem_size, CvMemStorage * storage )
2763 CV_FUNCNAME( "cvCreateSet" );
2768 CV_ERROR( CV_StsNullPtr, "" );
2769 if( header_size < (int)sizeof( CvSet ) ||
2770 elem_size < (int)sizeof(void*)*2 ||
2771 (elem_size & (sizeof(void*)-1)) != 0 )
2772 CV_ERROR( CV_StsBadSize, "" );
2774 set = (CvSet*) cvCreateSeq( set_flags, header_size, elem_size, storage );
2775 set->flags = (set->flags & ~CV_MAGIC_MASK) | CV_SET_MAGIC_VAL;
2783 /* Add new element to the set: */
2785 cvSetAdd( CvSet* set, CvSetElem* element, CvSetElem** inserted_element )
2789 CV_FUNCNAME( "cvSetAdd" );
2793 CvSetElem *free_elem;
2796 CV_ERROR( CV_StsNullPtr, "" );
2798 if( !(set->free_elems) )
2800 int count = set->total;
2801 int elem_size = set->elem_size;
2803 CV_CALL( icvGrowSeq( (CvSeq *) set, 0 ));
2805 set->free_elems = (CvSetElem*) (ptr = set->ptr);
2806 for( ; ptr + elem_size <= set->block_max; ptr += elem_size, count++ )
2808 ((CvSetElem*)ptr)->flags = count | CV_SET_ELEM_FREE_FLAG;
2809 ((CvSetElem*)ptr)->next_free = (CvSetElem*)(ptr + elem_size);
2811 assert( count <= CV_SET_ELEM_IDX_MASK+1 );
2812 ((CvSetElem*)(ptr - elem_size))->next_free = 0;
2813 set->first->prev->count += count - set->total;
2815 set->ptr = set->block_max;
2818 free_elem = set->free_elems;
2819 set->free_elems = free_elem->next_free;
2821 id = free_elem->flags & CV_SET_ELEM_IDX_MASK;
2823 CV_MEMCPY_INT( free_elem, element, (size_t)set->elem_size/sizeof(int) );
2825 free_elem->flags = id;
2826 set->active_count++;
2828 if( inserted_element )
2829 *inserted_element = free_elem;
2837 /* Remove element from a set given element index: */
2839 cvSetRemove( CvSet* set, int index )
2841 CV_FUNCNAME( "cvSetRemove" );
2845 CvSetElem* elem = cvGetSetElem( set, index );
2847 cvSetRemoveByPtr( set, elem );
2849 CV_ERROR( CV_StsNullPtr, "" );
2855 /* Remove all elements from a set: */
2857 cvClearSet( CvSet* set )
2859 CV_FUNCNAME( "cvClearSet" );
2863 CV_CALL( cvClearSeq( (CvSeq*)set ));
2864 set->free_elems = 0;
2865 set->active_count = 0;
2871 /****************************************************************************************\
2872 * Graph implementation *
2873 \****************************************************************************************/
2875 /* Create a new graph: */
2877 cvCreateGraph( int graph_type, int header_size,
2878 int vtx_size, int edge_size, CvMemStorage * storage )
2883 CV_FUNCNAME( "cvCleateGraph" );
2887 CvSet *vertices = 0;
2889 if( header_size < (int) sizeof( CvGraph )
2890 || edge_size < (int) sizeof( CvGraphEdge )
2891 || vtx_size < (int) sizeof( CvGraphVtx )
2893 CV_ERROR( CV_StsBadSize, "" );
2896 CV_CALL( vertices = cvCreateSet( graph_type, header_size, vtx_size, storage ));
2897 CV_CALL( edges = cvCreateSet( CV_SEQ_KIND_GENERIC | CV_SEQ_ELTYPE_GRAPH_EDGE,
2898 sizeof( CvSet ), edge_size, storage ));
2900 graph = (CvGraph*)vertices;
2901 graph->edges = edges;
2909 /* Remove all vertices and edges from a graph: */
2911 cvClearGraph( CvGraph * graph )
2913 CV_FUNCNAME( "cvClearGraph" );
2918 CV_ERROR( CV_StsNullPtr, "" );
2920 cvClearSet( graph->edges );
2921 cvClearSet( (CvSet*)graph );
2927 /* Add a vertex to a graph: */
2929 cvGraphAddVtx( CvGraph* graph, const CvGraphVtx* _vertex, CvGraphVtx** _inserted_vertex )
2931 CvGraphVtx *vertex = 0;
2934 CV_FUNCNAME( "cvGraphAddVtx" );
2939 CV_ERROR( CV_StsNullPtr, "" );
2941 vertex = (CvGraphVtx*)cvSetNew((CvSet*)graph);
2945 CV_MEMCPY_INT( vertex + 1, _vertex + 1,
2946 (size_t)(graph->elem_size - sizeof(CvGraphVtx))/sizeof(int) );
2948 index = vertex->flags;
2951 if( _inserted_vertex )
2952 *_inserted_vertex = vertex;
2960 /* Remove a vertex from the graph together with its incident edges: */
2962 cvGraphRemoveVtxByPtr( CvGraph* graph, CvGraphVtx* vtx )
2966 CV_FUNCNAME( "cvGraphRemoveVtxByPtr" );
2970 if( !graph || !vtx )
2971 CV_ERROR( CV_StsNullPtr, "" );
2973 if( !CV_IS_SET_ELEM(vtx))
2974 CV_ERROR( CV_StsBadArg, "The vertex does not belong to the graph" );
2976 count = graph->edges->active_count;
2979 CvGraphEdge *edge = vtx->first;
2982 cvGraphRemoveEdgeByPtr( graph, edge->vtx[0], edge->vtx[1] );
2984 count -= graph->edges->active_count;
2985 cvSetRemoveByPtr( (CvSet*)graph, vtx );
2993 /* Remove a vertex from the graph together with its incident edges: */
2995 cvGraphRemoveVtx( CvGraph* graph, int index )
2998 CvGraphVtx *vtx = 0;
3000 CV_FUNCNAME( "cvGraphRemoveVtx" );
3005 CV_ERROR( CV_StsNullPtr, "" );
3007 vtx = cvGetGraphVtx( graph, index );
3009 CV_ERROR( CV_StsBadArg, "The vertex is not found" );
3011 count = graph->edges->active_count;
3014 CvGraphEdge *edge = vtx->first;
3019 cvGraphRemoveEdgeByPtr( graph, edge->vtx[0], edge->vtx[1] );
3021 count -= graph->edges->active_count;
3022 cvSetRemoveByPtr( (CvSet*)graph, vtx );
3030 /* Find a graph edge given pointers to the ending vertices: */
3031 CV_IMPL CvGraphEdge*
3032 cvFindGraphEdgeByPtr( const CvGraph* graph,
3033 const CvGraphVtx* start_vtx,
3034 const CvGraphVtx* end_vtx )
3036 CvGraphEdge *edge = 0;
3037 CV_FUNCNAME( "cvFindGraphEdgeByPtr" );
3043 if( !graph || !start_vtx || !end_vtx )
3044 CV_ERROR( CV_StsNullPtr, "" );
3046 if( start_vtx == end_vtx )
3049 if( !CV_IS_GRAPH_ORIENTED( graph ) &&
3050 (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) )
3052 const CvGraphVtx* t;
3053 CV_SWAP( start_vtx, end_vtx, t );
3056 edge = start_vtx->first;
3057 for( ; edge; edge = edge->next[ofs] )
3059 ofs = start_vtx == edge->vtx[1];
3060 assert( ofs == 1 || start_vtx == edge->vtx[0] );
3061 if( edge->vtx[1] == end_vtx )
3071 /* Find an edge in the graph given indices of the ending vertices: */
3072 CV_IMPL CvGraphEdge *
3073 cvFindGraphEdge( const CvGraph* graph, int start_idx, int end_idx )
3075 CvGraphEdge *edge = 0;
3076 CvGraphVtx *start_vtx;
3077 CvGraphVtx *end_vtx;
3079 CV_FUNCNAME( "cvFindGraphEdge" );
3084 CV_ERROR( CV_StsNullPtr, "graph pointer is NULL" );
3086 start_vtx = cvGetGraphVtx( graph, start_idx );
3087 end_vtx = cvGetGraphVtx( graph, end_idx );
3089 edge = cvFindGraphEdgeByPtr( graph, start_vtx, end_vtx );
3097 /* Given two vertices, return the edge
3098 * connecting them, creating it if it
3099 * did not already exist:
3102 cvGraphAddEdgeByPtr( CvGraph* graph,
3103 CvGraphVtx* start_vtx, CvGraphVtx* end_vtx,
3104 const CvGraphEdge* _edge,
3105 CvGraphEdge ** _inserted_edge )
3107 CvGraphEdge *edge = 0;
3110 CV_FUNCNAME( "cvGraphAddEdgeByPtr" );
3117 CV_ERROR( CV_StsNullPtr, "graph pointer is NULL" );
3119 if( !CV_IS_GRAPH_ORIENTED( graph ) &&
3120 (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) )
3123 CV_SWAP( start_vtx, end_vtx, t );
3126 CV_CALL( edge = cvFindGraphEdgeByPtr( graph, start_vtx, end_vtx ));
3133 if( start_vtx == end_vtx )
3134 CV_ERROR( start_vtx ? CV_StsBadArg : CV_StsNullPtr,
3135 "vertex pointers coinside (or set to NULL)" );
3137 CV_CALL( edge = (CvGraphEdge*)cvSetNew( (CvSet*)(graph->edges) ));
3138 assert( edge->flags >= 0 );
3140 edge->vtx[0] = start_vtx;
3141 edge->vtx[1] = end_vtx;
3142 edge->next[0] = start_vtx->first;
3143 edge->next[1] = end_vtx->first;
3144 start_vtx->first = end_vtx->first = edge;
3146 delta = (graph->edges->elem_size - sizeof(*edge))/sizeof(int);
3150 CV_MEMCPY_INT( edge + 1, _edge + 1, delta );
3151 edge->weight = _edge->weight;
3156 CV_ZERO_INT( edge + 1, delta );
3164 if( _inserted_edge )
3165 *_inserted_edge = edge;
3170 /* Given two vertices, return the edge
3171 * connecting them, creating it if it
3172 * did not already exist:
3175 cvGraphAddEdge( CvGraph* graph,
3176 int start_idx, int end_idx,
3177 const CvGraphEdge* _edge,
3178 CvGraphEdge ** _inserted_edge )
3180 CvGraphVtx *start_vtx;
3181 CvGraphVtx *end_vtx;
3184 CV_FUNCNAME( "cvGraphAddEdge" );
3189 CV_ERROR( CV_StsNullPtr, "" );
3191 start_vtx = cvGetGraphVtx( graph, start_idx );
3192 end_vtx = cvGetGraphVtx( graph, end_idx );
3194 result = cvGraphAddEdgeByPtr( graph, start_vtx, end_vtx, _edge, _inserted_edge );
3202 /* Remove the graph edge connecting two given vertices: */
3204 cvGraphRemoveEdgeByPtr( CvGraph* graph, CvGraphVtx* start_vtx, CvGraphVtx* end_vtx )
3206 CV_FUNCNAME( "cvGraphRemoveEdgeByPtr" );
3211 CvGraphEdge *edge, *next_edge, *prev_edge;
3213 if( !graph || !start_vtx || !end_vtx )
3214 CV_ERROR( CV_StsNullPtr, "" );
3216 if( start_vtx == end_vtx )
3219 if( !CV_IS_GRAPH_ORIENTED( graph ) &&
3220 (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) )
3223 CV_SWAP( start_vtx, end_vtx, t );
3226 for( ofs = prev_ofs = 0, prev_edge = 0, edge = start_vtx->first; edge != 0;
3227 prev_ofs = ofs, prev_edge = edge, edge = edge->next[ofs] )
3229 ofs = start_vtx == edge->vtx[1];
3230 assert( ofs == 1 || start_vtx == edge->vtx[0] );
3231 if( edge->vtx[1] == end_vtx )
3238 next_edge = edge->next[ofs];
3240 prev_edge->next[prev_ofs] = next_edge;
3242 start_vtx->first = next_edge;
3244 for( ofs = prev_ofs = 0, prev_edge = 0, edge = end_vtx->first; edge != 0;
3245 prev_ofs = ofs, prev_edge = edge, edge = edge->next[ofs] )
3247 ofs = end_vtx == edge->vtx[1];
3248 assert( ofs == 1 || end_vtx == edge->vtx[0] );
3249 if( edge->vtx[0] == start_vtx )
3253 assert( edge != 0 );
3255 next_edge = edge->next[ofs];
3257 prev_edge->next[prev_ofs] = next_edge;
3259 end_vtx->first = next_edge;
3261 cvSetRemoveByPtr( graph->edges, edge );
3267 /* Remove the graph edge connecting two given vertices: */
3269 cvGraphRemoveEdge( CvGraph* graph, int start_idx, int end_idx )
3271 CvGraphVtx *start_vtx;
3272 CvGraphVtx *end_vtx;
3274 CV_FUNCNAME( "cvGraphRemoveEdge" );
3279 CV_ERROR( CV_StsNullPtr, "" );
3281 start_vtx = cvGetGraphVtx( graph, start_idx );
3282 end_vtx = cvGetGraphVtx( graph, end_idx );
3284 cvGraphRemoveEdgeByPtr( graph, start_vtx, end_vtx );
3290 /* Count number of edges incident to a given vertex: */
3292 cvGraphVtxDegreeByPtr( const CvGraph* graph, const CvGraphVtx* vertex )
3297 CV_FUNCNAME( "cvGraphVtxDegreeByPtr" );
3301 if( !graph || !vertex )
3302 CV_ERROR( CV_StsNullPtr, "" );
3304 for( edge = vertex->first, count = 0; edge; )
3307 edge = CV_NEXT_GRAPH_EDGE( edge, vertex );
3316 /* Count number of edges incident to a given vertex: */
3318 cvGraphVtxDegree( const CvGraph* graph, int vtx_idx )
3324 CV_FUNCNAME( "cvGraphVtxDegree" );
3329 CV_ERROR( CV_StsNullPtr, "" );
3331 vertex = cvGetGraphVtx( graph, vtx_idx );
3333 CV_ERROR( CV_StsObjectNotFound, "" );
3335 for( edge = vertex->first, count = 0; edge; )
3338 edge = CV_NEXT_GRAPH_EDGE( edge, vertex );
3347 typedef struct CvGraphItem
3356 icvSeqElemsClearFlags( CvSeq* seq, int offset, int clear_mask )
3358 CV_FUNCNAME("icvStartScanGraph");
3363 int i, total, elem_size;
3366 CV_ERROR( CV_StsNullPtr, "" );
3368 elem_size = seq->elem_size;
3371 if( (unsigned)offset > (unsigned)elem_size )
3372 CV_ERROR( CV_StsBadArg, "" );
3374 CV_CALL( cvStartReadSeq( seq, &reader ));
3376 for( i = 0; i < total; i++ )
3378 int* flag_ptr = (int*)(reader.ptr + offset);
3379 *flag_ptr &= ~clear_mask;
3381 CV_NEXT_SEQ_ELEM( elem_size, reader );
3389 icvSeqFindNextElem( CvSeq* seq, int offset, int mask,
3390 int value, int* start_index )
3392 schar* elem_ptr = 0;
3394 CV_FUNCNAME("icvStartScanGraph");
3399 int total, elem_size, index;
3401 if( !seq || !start_index )
3402 CV_ERROR( CV_StsNullPtr, "" );
3404 elem_size = seq->elem_size;
3406 index = *start_index;
3408 if( (unsigned)offset > (unsigned)elem_size )
3409 CV_ERROR( CV_StsBadArg, "" );
3414 if( (unsigned)index >= (unsigned)total )
3417 index += index < 0 ? total : 0;
3420 CV_CALL( cvStartReadSeq( seq, &reader ));
3423 CV_CALL( cvSetSeqReaderPos( &reader, index ));
3425 for( index = 0; index < total; index++ )
3427 int* flag_ptr = (int*)(reader.ptr + offset);
3428 if( (*flag_ptr & mask) == value )
3431 CV_NEXT_SEQ_ELEM( elem_size, reader );
3436 elem_ptr = reader.ptr;
3437 *start_index = index;
3445 #define CV_FIELD_OFFSET( field, structtype ) ((int)(size_t)&((structtype*)0)->field)
3447 CV_IMPL CvGraphScanner*
3448 cvCreateGraphScanner( CvGraph* graph, CvGraphVtx* vtx, int mask )
3450 CvGraphScanner* scanner = 0;
3451 CvMemStorage* child_storage = 0;
3453 CV_FUNCNAME("cvCreateGraphScanner");
3458 CV_ERROR( CV_StsNullPtr, "Null graph pointer" );
3460 CV_ASSERT( graph->storage != 0 );
3462 CV_CALL( scanner = (CvGraphScanner*)cvAlloc( sizeof(*scanner) ));
3463 memset( scanner, 0, sizeof(*scanner));
3465 scanner->graph = graph;
3466 scanner->mask = mask;
3468 scanner->index = vtx == 0 ? 0 : -1;
3470 CV_CALL( child_storage = cvCreateChildMemStorage( graph->storage ));
3472 CV_CALL( scanner->stack = cvCreateSeq( 0, sizeof(CvSet),
3473 sizeof(CvGraphItem), child_storage ));
3475 CV_CALL( icvSeqElemsClearFlags( (CvSeq*)graph,
3476 CV_FIELD_OFFSET( flags, CvGraphVtx),
3477 CV_GRAPH_ITEM_VISITED_FLAG|
3478 CV_GRAPH_SEARCH_TREE_NODE_FLAG ));
3480 CV_CALL( icvSeqElemsClearFlags( (CvSeq*)(graph->edges),
3481 CV_FIELD_OFFSET( flags, CvGraphEdge),
3482 CV_GRAPH_ITEM_VISITED_FLAG ));
3486 if( cvGetErrStatus() < 0 )
3488 cvReleaseMemStorage( &child_storage );
3497 cvReleaseGraphScanner( CvGraphScanner** scanner )
3499 CV_FUNCNAME("cvReleaseGraphScanner");
3504 CV_ERROR( CV_StsNullPtr, "Null double pointer to graph scanner" );
3508 if( (*scanner)->stack )
3509 CV_CALL( cvReleaseMemStorage( &((*scanner)->stack->storage)));
3518 cvNextGraphItem( CvGraphScanner* scanner )
3522 CV_FUNCNAME("cvNextGraphItem");
3531 if( !scanner || !(scanner->stack))
3532 CV_ERROR( CV_StsNullPtr, "Null graph scanner" );
3536 edge = scanner->edge;
3542 if( dst && !CV_IS_GRAPH_VERTEX_VISITED(dst) )
3544 scanner->vtx = vtx = dst;
3546 dst->flags |= CV_GRAPH_ITEM_VISITED_FLAG;
3548 if((scanner->mask & CV_GRAPH_VERTEX))
3551 scanner->edge = vtx->first;
3553 code = CV_GRAPH_VERTEX;
3560 dst = edge->vtx[vtx == edge->vtx[0]];
3562 if( !CV_IS_GRAPH_EDGE_VISITED(edge) )
3564 // Check that the edge is outgoing:
3565 if( !CV_IS_GRAPH_ORIENTED( scanner->graph ) || dst != edge->vtx[0] )
3567 edge->flags |= CV_GRAPH_ITEM_VISITED_FLAG;
3569 if( !CV_IS_GRAPH_VERTEX_VISITED(dst) )
3574 vtx->flags |= CV_GRAPH_SEARCH_TREE_NODE_FLAG;
3576 cvSeqPush( scanner->stack, &item );
3578 if( scanner->mask & CV_GRAPH_TREE_EDGE )
3580 code = CV_GRAPH_TREE_EDGE;
3583 scanner->edge = edge;
3590 if( scanner->mask & (CV_GRAPH_BACK_EDGE|
3591 CV_GRAPH_CROSS_EDGE|
3592 CV_GRAPH_FORWARD_EDGE) )
3594 code = (dst->flags & CV_GRAPH_SEARCH_TREE_NODE_FLAG) ?
3595 CV_GRAPH_BACK_EDGE :
3596 (edge->flags & CV_GRAPH_FORWARD_EDGE_FLAG) ?
3597 CV_GRAPH_FORWARD_EDGE : CV_GRAPH_CROSS_EDGE;
3598 edge->flags &= ~CV_GRAPH_FORWARD_EDGE_FLAG;
3599 if( scanner->mask & code )
3603 scanner->edge = edge;
3609 else if( (dst->flags & (CV_GRAPH_ITEM_VISITED_FLAG|
3610 CV_GRAPH_SEARCH_TREE_NODE_FLAG)) ==
3611 (CV_GRAPH_ITEM_VISITED_FLAG|
3612 CV_GRAPH_SEARCH_TREE_NODE_FLAG))
3614 edge->flags |= CV_GRAPH_FORWARD_EDGE_FLAG;
3618 edge = CV_NEXT_GRAPH_EDGE( edge, vtx );
3621 if( !edge ) /* need to backtrack */
3623 if( scanner->stack->total == 0 )
3625 if( scanner->index >= 0 )
3631 cvSeqPop( scanner->stack, &item );
3633 vtx->flags &= ~CV_GRAPH_SEARCH_TREE_NODE_FLAG;
3637 if( scanner->mask & CV_GRAPH_BACKTRACKING )
3640 scanner->edge = edge;
3641 scanner->dst = edge->vtx[vtx == edge->vtx[0]];
3642 code = CV_GRAPH_BACKTRACKING;
3650 vtx = (CvGraphVtx*)icvSeqFindNextElem( (CvSeq*)(scanner->graph),
3651 CV_FIELD_OFFSET( flags, CvGraphVtx ), CV_GRAPH_ITEM_VISITED_FLAG|INT_MIN,
3652 0, &(scanner->index) );
3656 code = CV_GRAPH_OVER;
3662 if( scanner->mask & CV_GRAPH_NEW_TREE )
3667 code = CV_GRAPH_NEW_TREE;
3679 cvCloneGraph( const CvGraph* graph, CvMemStorage* storage )
3681 int* flag_buffer = 0;
3682 CvGraphVtx** ptr_buffer = 0;
3683 CvGraph* result = 0;
3685 CV_FUNCNAME( "cvCloneGraph" );
3690 int vtx_size, edge_size;
3693 if( !CV_IS_GRAPH(graph))
3694 CV_ERROR( CV_StsBadArg, "Invalid graph pointer" );
3697 storage = graph->storage;
3700 CV_ERROR( CV_StsNullPtr, "NULL storage pointer" );
3702 vtx_size = graph->elem_size;
3703 edge_size = graph->edges->elem_size;
3705 CV_CALL( flag_buffer = (int*)cvAlloc( graph->total*sizeof(flag_buffer[0])));
3706 CV_CALL( ptr_buffer = (CvGraphVtx**)cvAlloc( graph->total*sizeof(ptr_buffer[0])));
3707 CV_CALL( result = cvCreateGraph( graph->flags, graph->header_size,
3708 vtx_size, edge_size, storage ));
3709 memcpy( result + sizeof(CvGraph), graph + sizeof(CvGraph),
3710 graph->header_size - sizeof(CvGraph));
3712 // Pass 1. Save flags, copy vertices:
3713 cvStartReadSeq( (CvSeq*)graph, &reader );
3714 for( i = 0, k = 0; i < graph->total; i++ )
3716 if( CV_IS_SET_ELEM( reader.ptr ))
3718 CvGraphVtx* vtx = (CvGraphVtx*)reader.ptr;
3719 CvGraphVtx* dstvtx = 0;
3720 CV_CALL( cvGraphAddVtx( result, vtx, &dstvtx ));
3721 flag_buffer[k] = dstvtx->flags = vtx->flags;
3723 ptr_buffer[k++] = dstvtx;
3725 CV_NEXT_SEQ_ELEM( vtx_size, reader );
3728 // Pass 2. Copy edges:
3729 cvStartReadSeq( (CvSeq*)graph->edges, &reader );
3730 for( i = 0; i < graph->edges->total; i++ )
3732 if( CV_IS_SET_ELEM( reader.ptr ))
3734 CvGraphEdge* edge = (CvGraphEdge*)reader.ptr;
3735 CvGraphEdge* dstedge = 0;
3736 CvGraphVtx* new_org = ptr_buffer[edge->vtx[0]->flags];
3737 CvGraphVtx* new_dst = ptr_buffer[edge->vtx[1]->flags];
3738 CV_CALL( cvGraphAddEdgeByPtr( result, new_org, new_dst, edge, &dstedge ));
3739 dstedge->flags = edge->flags;
3741 CV_NEXT_SEQ_ELEM( edge_size, reader );
3744 // Pass 3. Restore flags:
3745 cvStartReadSeq( (CvSeq*)graph, &reader );
3746 for( i = 0, k = 0; i < graph->edges->total; i++ )
3748 if( CV_IS_SET_ELEM( reader.ptr ))
3750 CvGraphVtx* vtx = (CvGraphVtx*)reader.ptr;
3751 vtx->flags = flag_buffer[k++];
3753 CV_NEXT_SEQ_ELEM( vtx_size, reader );
3758 cvFree( &flag_buffer );
3759 cvFree( &ptr_buffer );
3761 if( cvGetErrStatus() < 0 )
3768 /****************************************************************************************\
3769 * Working with sequence tree *
3770 \****************************************************************************************/
3772 // Gather pointers to all the sequences, accessible from the <first>, to the single sequence.
3774 cvTreeToNodeSeq( const void* first, int header_size, CvMemStorage* storage )
3778 CV_FUNCNAME("cvTreeToNodeSeq");
3782 CvTreeNodeIterator iterator;
3785 CV_ERROR( CV_StsNullPtr, "NULL storage pointer" );
3787 CV_CALL( allseq = cvCreateSeq( 0, header_size, sizeof(first), storage ));
3791 CV_CALL( cvInitTreeNodeIterator( &iterator, first, INT_MAX ));
3795 void* node = cvNextTreeNode( &iterator );
3798 cvSeqPush( allseq, &node );
3808 typedef struct CvTreeNode
3810 int flags; /* micsellaneous flags */
3811 int header_size; /* size of sequence header */
3812 struct CvTreeNode* h_prev; /* previous sequence */
3813 struct CvTreeNode* h_next; /* next sequence */
3814 struct CvTreeNode* v_prev; /* 2nd previous sequence */
3815 struct CvTreeNode* v_next; /* 2nd next sequence */
3821 // Insert contour into tree given certain parent sequence.
3822 // If parent is equal to frame (the most external contour),
3823 // then added contour will have null pointer to parent:
3825 cvInsertNodeIntoTree( void* _node, void* _parent, void* _frame )
3827 CV_FUNCNAME( "cvInsertNodeIntoTree" );
3831 CvTreeNode* node = (CvTreeNode*)_node;
3832 CvTreeNode* parent = (CvTreeNode*)_parent;
3834 if( !node || !parent )
3835 CV_ERROR( CV_StsNullPtr, "" );
3837 node->v_prev = _parent != _frame ? parent : 0;
3838 node->h_next = parent->v_next;
3840 assert( parent->v_next != node );
3842 if( parent->v_next )
3843 parent->v_next->h_prev = node;
3844 parent->v_next = node;
3850 // Remove contour from tree, together with the contour's children:
3852 cvRemoveNodeFromTree( void* _node, void* _frame )
3854 CV_FUNCNAME( "cvRemoveNodeFromTree" );
3858 CvTreeNode* node = (CvTreeNode*)_node;
3859 CvTreeNode* frame = (CvTreeNode*)_frame;
3862 CV_ERROR_FROM_CODE( CV_StsNullPtr );
3865 CV_ERROR( CV_StsBadArg, "frame node could not be deleted" );
3868 node->h_next->h_prev = node->h_prev;
3871 node->h_prev->h_next = node->h_next;
3874 CvTreeNode* parent = node->v_prev;
3880 assert( parent->v_next == node );
3881 parent->v_next = node->h_next;
3890 cvInitTreeNodeIterator( CvTreeNodeIterator* treeIterator,
3891 const void* first, int max_level )
3893 CV_FUNCNAME("icvInitTreeNodeIterator");
3897 if( !treeIterator || !first )
3898 CV_ERROR( CV_StsNullPtr, "" );
3901 CV_ERROR( CV_StsOutOfRange, "" );
3903 treeIterator->node = (void*)first;
3904 treeIterator->level = 0;
3905 treeIterator->max_level = max_level;
3912 cvNextTreeNode( CvTreeNodeIterator* treeIterator )
3914 CvTreeNode* prevNode = 0;
3916 CV_FUNCNAME("cvNextTreeNode");
3924 CV_ERROR( CV_StsNullPtr, "NULL iterator pointer" );
3926 prevNode = node = (CvTreeNode*)treeIterator->node;
3927 level = treeIterator->level;
3931 if( node->v_next && level+1 < treeIterator->max_level )
3933 node = node->v_next;
3938 while( node->h_next == 0 )
3940 node = node->v_prev;
3947 node = node && treeIterator->max_level != 0 ? node->h_next : 0;
3951 treeIterator->node = node;
3952 treeIterator->level = level;
3961 cvPrevTreeNode( CvTreeNodeIterator* treeIterator )
3963 CvTreeNode* prevNode = 0;
3965 CV_FUNCNAME("cvPrevTreeNode");
3973 CV_ERROR( CV_StsNullPtr, "" );
3975 prevNode = node = (CvTreeNode*)treeIterator->node;
3976 level = treeIterator->level;
3982 node = node->v_prev;
3988 node = node->h_prev;
3990 while( node->v_next && level < treeIterator->max_level )
3992 node = node->v_next;
3995 while( node->h_next )
3996 node = node->h_next;
4001 treeIterator->node = node;
4002 treeIterator->level = level;