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.
41 #include "precomp.hpp"
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 )
86 CV_Error( CV_StsNullPtr, "" );
89 block_size = CV_STORAGE_BLOCK_SIZE;
91 block_size = cvAlign( block_size, CV_STRUCT_ALIGN );
92 assert( sizeof(CvMemBlock) % CV_STRUCT_ALIGN == 0 );
94 memset( storage, 0, sizeof( *storage ));
95 storage->signature = CV_STORAGE_MAGIC_VAL;
96 storage->block_size = block_size;
100 /* Create root memory storage: */
101 CV_IMPL CvMemStorage*
102 cvCreateMemStorage( int block_size )
104 CvMemStorage* storage = (CvMemStorage *)cvAlloc( sizeof( CvMemStorage ));
105 icvInitMemStorage( storage, block_size );
110 /* Create child memory storage: */
111 CV_IMPL CvMemStorage *
112 cvCreateChildMemStorage( CvMemStorage * parent )
115 CV_Error( CV_StsNullPtr, "" );
117 CvMemStorage* storage = cvCreateMemStorage(parent->block_size);
118 storage->parent = parent;
124 /* Release all blocks of the storage (or return them to parent, if any): */
126 icvDestroyMemStorage( CvMemStorage* storage )
131 CvMemBlock *dst_top = 0;
134 CV_Error( CV_StsNullPtr, "" );
136 if( storage->parent )
137 dst_top = storage->parent->top;
139 for( block = storage->bottom; block != 0; k++ )
141 CvMemBlock *temp = block;
144 if( storage->parent )
148 temp->prev = dst_top;
149 temp->next = dst_top->next;
151 temp->next->prev = temp;
152 dst_top = dst_top->next = temp;
156 dst_top = storage->parent->bottom = storage->parent->top = temp;
157 temp->prev = temp->next = 0;
158 storage->free_space = storage->block_size - sizeof( *temp );
167 storage->top = storage->bottom = 0;
168 storage->free_space = 0;
172 /* Release memory storage: */
174 cvReleaseMemStorage( CvMemStorage** storage )
177 CV_Error( CV_StsNullPtr, "" );
179 CvMemStorage* st = *storage;
183 icvDestroyMemStorage( st );
189 /* Clears memory storage (return blocks to the parent, if any): */
191 cvClearMemStorage( CvMemStorage * storage )
194 CV_Error( CV_StsNullPtr, "" );
196 if( storage->parent )
197 icvDestroyMemStorage( storage );
200 storage->top = storage->bottom;
201 storage->free_space = storage->bottom ? storage->block_size - sizeof(CvMemBlock) : 0;
206 /* Moves stack pointer to next block.
207 If no blocks, allocate new one and link it to the storage: */
209 icvGoNextMemBlock( CvMemStorage * storage )
212 CV_Error( CV_StsNullPtr, "" );
214 if( !storage->top || !storage->top->next )
218 if( !(storage->parent) )
220 block = (CvMemBlock *)cvAlloc( storage->block_size );
224 CvMemStorage *parent = storage->parent;
225 CvMemStoragePos parent_pos;
227 cvSaveMemStoragePos( parent, &parent_pos );
228 icvGoNextMemBlock( parent );
231 cvRestoreMemStoragePos( parent, &parent_pos );
233 if( block == parent->top ) /* the single allocated block */
235 assert( parent->bottom == block );
236 parent->top = parent->bottom = 0;
237 parent->free_space = 0;
241 /* cut the block from the parent's list of blocks */
242 parent->top->next = block->next;
244 block->next->prev = parent->top;
250 block->prev = storage->top;
253 storage->top->next = block;
255 storage->top = storage->bottom = block;
258 if( storage->top->next )
259 storage->top = storage->top->next;
260 storage->free_space = storage->block_size - sizeof(CvMemBlock);
261 assert( storage->free_space % CV_STRUCT_ALIGN == 0 );
265 /* Remember memory storage position: */
267 cvSaveMemStoragePos( const CvMemStorage * storage, CvMemStoragePos * pos )
269 if( !storage || !pos )
270 CV_Error( CV_StsNullPtr, "" );
272 pos->top = storage->top;
273 pos->free_space = storage->free_space;
277 /* Restore memory storage position: */
279 cvRestoreMemStoragePos( CvMemStorage * storage, CvMemStoragePos * pos )
281 if( !storage || !pos )
282 CV_Error( CV_StsNullPtr, "" );
283 if( pos->free_space > storage->block_size )
284 CV_Error( CV_StsBadSize, "" );
287 // this breaks icvGoNextMemBlock, so comment it off for now
288 if( storage->parent && (!pos->top || pos->top->next) )
290 CvMemBlock* save_bottom;
295 save_bottom = storage->bottom;
296 storage->bottom = pos->top->next;
298 storage->bottom->prev = 0;
300 icvDestroyMemStorage( storage );
301 storage->bottom = save_bottom;
304 storage->top = pos->top;
305 storage->free_space = pos->free_space;
309 storage->top = storage->bottom;
310 storage->free_space = storage->top ? storage->block_size - sizeof(CvMemBlock) : 0;
315 /* Allocate continuous buffer of the specified size in the storage: */
317 cvMemStorageAlloc( CvMemStorage* storage, size_t size )
321 CV_Error( CV_StsNullPtr, "NULL storage pointer" );
324 CV_Error( CV_StsOutOfRange, "Too large memory block is requested" );
326 assert( storage->free_space % CV_STRUCT_ALIGN == 0 );
328 if( (size_t)storage->free_space < size )
330 size_t max_free_space = cvAlignLeft(storage->block_size - sizeof(CvMemBlock), CV_STRUCT_ALIGN);
331 if( max_free_space < size )
332 CV_Error( CV_StsOutOfRange, "requested size is negative or too big" );
334 icvGoNextMemBlock( storage );
337 ptr = ICV_FREE_PTR(storage);
338 assert( (size_t)ptr % CV_STRUCT_ALIGN == 0 );
339 storage->free_space = cvAlignLeft(storage->free_space - (int)size, CV_STRUCT_ALIGN );
346 cvMemStorageAllocString( CvMemStorage* storage, const char* ptr, int len )
350 str.len = len >= 0 ? len : (int)strlen(ptr);
351 str.ptr = (char*)cvMemStorageAlloc( storage, str.len + 1 );
352 memcpy( str.ptr, ptr, str.len );
353 str.ptr[str.len] = '\0';
359 /****************************************************************************************\
360 * Sequence implementation *
361 \****************************************************************************************/
363 /* Create empty sequence: */
365 cvCreateSeq( int seq_flags, int header_size, int elem_size, CvMemStorage * storage )
370 CV_Error( CV_StsNullPtr, "" );
371 if( header_size < (int)sizeof( CvSeq ) || elem_size <= 0 )
372 CV_Error( CV_StsBadSize, "" );
374 /* allocate sequence header */
375 seq = (CvSeq*)cvMemStorageAlloc( storage, header_size );
376 memset( seq, 0, header_size );
378 seq->header_size = header_size;
379 seq->flags = (seq_flags & ~CV_MAGIC_MASK) | CV_SEQ_MAGIC_VAL;
381 int elemtype = CV_MAT_TYPE(seq_flags);
382 int typesize = CV_ELEM_SIZE(elemtype);
384 if( elemtype != CV_SEQ_ELTYPE_GENERIC && elemtype != CV_USRTYPE1 &&
385 typesize != 0 && typesize != elem_size )
386 CV_Error( CV_StsBadSize,
387 "Specified element size doesn't match to the size of the specified element type "
388 "(try to use 0 for element type)" );
390 seq->elem_size = elem_size;
391 seq->storage = storage;
393 cvSetSeqBlockSize( seq, (1 << 10)/elem_size );
399 /* adjusts <delta_elems> field of sequence. It determines how much the sequence
400 grows if there are no free space inside the sequence buffers */
402 cvSetSeqBlockSize( CvSeq *seq, int delta_elements )
405 int useful_block_size;
407 if( !seq || !seq->storage )
408 CV_Error( CV_StsNullPtr, "" );
409 if( delta_elements < 0 )
410 CV_Error( CV_StsOutOfRange, "" );
412 useful_block_size = cvAlignLeft(seq->storage->block_size - sizeof(CvMemBlock) -
413 sizeof(CvSeqBlock), CV_STRUCT_ALIGN);
414 elem_size = seq->elem_size;
416 if( delta_elements == 0 )
418 delta_elements = (1 << 10) / elem_size;
419 delta_elements = MAX( delta_elements, 1 );
421 if( delta_elements * elem_size > useful_block_size )
423 delta_elements = useful_block_size / elem_size;
424 if( delta_elements == 0 )
425 CV_Error( CV_StsOutOfRange, "Storage block size is too small "
426 "to fit the sequence elements" );
429 seq->delta_elems = delta_elements;
433 /* Find a sequence element by its index: */
435 cvGetSeqElem( const CvSeq *seq, int index )
438 int count, total = seq->total;
440 if( (unsigned)index >= (unsigned)total )
442 index += index < 0 ? total : 0;
443 index -= index >= total ? total : 0;
444 if( (unsigned)index >= (unsigned)total )
449 if( index + index <= total )
451 while( index >= (count = block->count) )
462 total -= block->count;
464 while( index < total );
468 return block->data + index * seq->elem_size;
472 /* Calculate index of a sequence element: */
474 cvSeqElemIdx( const CvSeq* seq, const void* _element, CvSeqBlock** _block )
476 const schar *element = (const schar *)_element;
479 CvSeqBlock *first_block;
482 if( !seq || !element )
483 CV_Error( CV_StsNullPtr, "" );
485 block = first_block = seq->first;
486 elem_size = seq->elem_size;
490 if( (unsigned)(element - block->data) < (unsigned) (block->count * elem_size) )
494 if( elem_size <= ICV_SHIFT_TAB_MAX && (id = icvPower2ShiftTab[elem_size - 1]) >= 0 )
495 id = (int)((size_t)(element - block->data) >> id);
497 id = (int)((size_t)(element - block->data) / elem_size);
498 id += block->start_index - seq->first->start_index;
502 if( block == first_block )
511 cvSliceLength( CvSlice slice, const CvSeq* seq )
513 int total = seq->total;
514 int length = slice.end_index - slice.start_index;
518 if( slice.start_index < 0 )
519 slice.start_index += total;
520 if( slice.end_index <= 0 )
521 slice.end_index += total;
523 length = slice.end_index - slice.start_index;
532 else if( length > total )
539 /* Copy all sequence elements into single continuous array: */
541 cvCvtSeqToArray( const CvSeq *seq, void *array, CvSlice slice )
543 int elem_size, total;
545 char *dst = (char*)array;
548 CV_Error( CV_StsNullPtr, "" );
550 elem_size = seq->elem_size;
551 total = cvSliceLength( slice, seq )*elem_size;
556 cvStartReadSeq( seq, &reader, 0 );
557 cvSetSeqReaderPos( &reader, slice.start_index, 0 );
561 int count = (int)(reader.block_max - reader.ptr);
565 memcpy( dst, reader.ptr, count );
567 reader.block = reader.block->next;
568 reader.ptr = reader.block->data;
569 reader.block_max = reader.ptr + reader.block->count*elem_size;
578 /* Construct a sequence from an array without copying any data.
579 NB: The resultant sequence cannot grow beyond its initial size: */
581 cvMakeSeqHeaderForArray( int seq_flags, int header_size, int elem_size,
582 void *array, int total, CvSeq *seq, CvSeqBlock * block )
586 if( elem_size <= 0 || header_size < (int)sizeof( CvSeq ) || total < 0 )
587 CV_Error( CV_StsBadSize, "" );
589 if( !seq || ((!array || !block) && total > 0) )
590 CV_Error( CV_StsNullPtr, "" );
592 memset( seq, 0, header_size );
594 seq->header_size = header_size;
595 seq->flags = (seq_flags & ~CV_MAGIC_MASK) | CV_SEQ_MAGIC_VAL;
597 int elemtype = CV_MAT_TYPE(seq_flags);
598 int typesize = CV_ELEM_SIZE(elemtype);
600 if( elemtype != CV_SEQ_ELTYPE_GENERIC &&
601 typesize != 0 && typesize != elem_size )
602 CV_Error( CV_StsBadSize,
603 "Element size doesn't match to the size of predefined element type "
604 "(try to use 0 for sequence element type)" );
606 seq->elem_size = elem_size;
608 seq->block_max = seq->ptr = (schar *) array + total * elem_size;
613 block->prev = block->next = block;
614 block->start_index = 0;
615 block->count = total;
616 block->data = (schar *) array;
625 /* The function allocates space for at least one more sequence element.
626 If there are free sequence blocks (seq->free_blocks != 0)
627 they are reused, otherwise the space is allocated in the storage: */
629 icvGrowSeq( CvSeq *seq, int in_front_of )
634 CV_Error( CV_StsNullPtr, "" );
635 block = seq->free_blocks;
639 int elem_size = seq->elem_size;
640 int delta_elems = seq->delta_elems;
641 CvMemStorage *storage = seq->storage;
643 if( seq->total >= delta_elems*4 )
644 cvSetSeqBlockSize( seq, delta_elems*2 );
647 CV_Error( CV_StsNullPtr, "The sequence has NULL storage pointer" );
649 /* If there is a free space just after last allocated block
650 and it is big enough then enlarge the last block.
651 This can happen only if the new block is added to the end of sequence: */
652 if( (unsigned)(ICV_FREE_PTR(storage) - seq->block_max) < CV_STRUCT_ALIGN &&
653 storage->free_space >= seq->elem_size && !in_front_of )
655 int delta = storage->free_space / elem_size;
657 delta = MIN( delta, delta_elems ) * elem_size;
658 seq->block_max += delta;
659 storage->free_space = cvAlignLeft((int)(((schar*)storage->top + storage->block_size) -
660 seq->block_max), CV_STRUCT_ALIGN );
665 int delta = elem_size * delta_elems + ICV_ALIGNED_SEQ_BLOCK_SIZE;
667 /* Try to allocate <delta_elements> elements: */
668 if( storage->free_space < delta )
670 int small_block_size = MAX(1, delta_elems/3)*elem_size +
671 ICV_ALIGNED_SEQ_BLOCK_SIZE;
672 /* try to allocate smaller part */
673 if( storage->free_space >= small_block_size + CV_STRUCT_ALIGN )
675 delta = (storage->free_space - ICV_ALIGNED_SEQ_BLOCK_SIZE)/seq->elem_size;
676 delta = delta*seq->elem_size + ICV_ALIGNED_SEQ_BLOCK_SIZE;
680 icvGoNextMemBlock( storage );
681 assert( storage->free_space >= delta );
685 block = (CvSeqBlock*)cvMemStorageAlloc( storage, delta );
686 block->data = (schar*)cvAlignPtr( block + 1, CV_STRUCT_ALIGN );
687 block->count = delta - ICV_ALIGNED_SEQ_BLOCK_SIZE;
688 block->prev = block->next = 0;
693 seq->free_blocks = block->next;
699 block->prev = block->next = block;
703 block->prev = seq->first->prev;
704 block->next = seq->first;
705 block->prev->next = block->next->prev = block;
708 /* For free blocks the <count> field means
709 * total number of bytes in the block.
711 * For used blocks it means current number
712 * of sequence elements in the block:
714 assert( block->count % seq->elem_size == 0 && block->count > 0 );
718 seq->ptr = block->data;
719 seq->block_max = block->data + block->count;
720 block->start_index = block == block->prev ? 0 :
721 block->prev->start_index + block->prev->count;
725 int delta = block->count / seq->elem_size;
726 block->data += block->count;
728 if( block != block->prev )
730 assert( seq->first->start_index == 0 );
735 seq->block_max = seq->ptr = block->data;
738 block->start_index = 0;
742 block->start_index += delta;
744 if( block == seq->first )
752 /* Recycle a sequence block: */
754 icvFreeSeqBlock( CvSeq *seq, int in_front_of )
756 CvSeqBlock *block = seq->first;
758 assert( (in_front_of ? block : block->prev)->count == 0 );
760 if( block == block->prev ) /* single block case */
762 block->count = (int)(seq->block_max - block->data) + block->start_index * seq->elem_size;
763 block->data = seq->block_max - block->count;
765 seq->ptr = seq->block_max = 0;
773 assert( seq->ptr == block->data );
775 block->count = (int)(seq->block_max - seq->ptr);
776 seq->block_max = seq->ptr = block->prev->data +
777 block->prev->count * seq->elem_size;
781 int delta = block->start_index;
783 block->count = delta * seq->elem_size;
784 block->data -= block->count;
786 /* Update start indices of sequence blocks: */
789 block->start_index -= delta;
791 if( block == seq->first )
795 seq->first = block->next;
798 block->prev->next = block->next;
799 block->next->prev = block->prev;
802 assert( block->count > 0 && block->count % seq->elem_size == 0 );
803 block->next = seq->free_blocks;
804 seq->free_blocks = block;
808 /****************************************************************************************\
809 * Sequence Writer implementation *
810 \****************************************************************************************/
812 /* Initialize sequence writer: */
814 cvStartAppendToSeq( CvSeq *seq, CvSeqWriter * writer )
816 if( !seq || !writer )
817 CV_Error( CV_StsNullPtr, "" );
819 memset( writer, 0, sizeof( *writer ));
820 writer->header_size = sizeof( CvSeqWriter );
823 writer->block = seq->first ? seq->first->prev : 0;
824 writer->ptr = seq->ptr;
825 writer->block_max = seq->block_max;
829 /* Initialize sequence writer: */
831 cvStartWriteSeq( int seq_flags, int header_size,
832 int elem_size, CvMemStorage * storage, CvSeqWriter * writer )
834 if( !storage || !writer )
835 CV_Error( CV_StsNullPtr, "" );
837 CvSeq* seq = cvCreateSeq( seq_flags, header_size, elem_size, storage );
838 cvStartAppendToSeq( seq, writer );
842 /* Update sequence header: */
844 cvFlushSeqWriter( CvSeqWriter * writer )
847 CV_Error( CV_StsNullPtr, "" );
849 CvSeq* seq = writer->seq;
850 seq->ptr = writer->ptr;
855 CvSeqBlock *first_block = writer->seq->first;
856 CvSeqBlock *block = first_block;
858 writer->block->count = (int)((writer->ptr - writer->block->data) / seq->elem_size);
859 assert( writer->block->count > 0 );
863 total += block->count;
866 while( block != first_block );
868 writer->seq->total = total;
873 /* Calls icvFlushSeqWriter and finishes writing process: */
875 cvEndWriteSeq( CvSeqWriter * writer )
878 CV_Error( CV_StsNullPtr, "" );
880 cvFlushSeqWriter( writer );
881 CvSeq* seq = writer->seq;
883 /* Truncate the last block: */
884 if( writer->block && writer->seq->storage )
886 CvMemStorage *storage = seq->storage;
887 schar *storage_block_max = (schar *) storage->top + storage->block_size;
889 assert( writer->block->count > 0 );
891 if( (unsigned)((storage_block_max - storage->free_space)
892 - seq->block_max) < CV_STRUCT_ALIGN )
894 storage->free_space = cvAlignLeft((int)(storage_block_max - seq->ptr), CV_STRUCT_ALIGN);
895 seq->block_max = seq->ptr;
904 /* Create new sequence block: */
906 cvCreateSeqBlock( CvSeqWriter * writer )
908 if( !writer || !writer->seq )
909 CV_Error( CV_StsNullPtr, "" );
911 CvSeq* seq = writer->seq;
913 cvFlushSeqWriter( writer );
915 icvGrowSeq( seq, 0 );
917 writer->block = seq->first->prev;
918 writer->ptr = seq->ptr;
919 writer->block_max = seq->block_max;
923 /****************************************************************************************\
924 * Sequence Reader implementation *
925 \****************************************************************************************/
927 /* Initialize sequence reader: */
929 cvStartReadSeq( const CvSeq *seq, CvSeqReader * reader, int reverse )
931 CvSeqBlock *first_block;
932 CvSeqBlock *last_block;
938 reader->ptr = reader->block_max = reader->block_min = 0;
941 if( !seq || !reader )
942 CV_Error( CV_StsNullPtr, "" );
944 reader->header_size = sizeof( CvSeqReader );
945 reader->seq = (CvSeq*)seq;
947 first_block = seq->first;
951 last_block = first_block->prev;
952 reader->ptr = first_block->data;
953 reader->prev_elem = CV_GET_LAST_ELEM( seq, last_block );
954 reader->delta_index = seq->first->start_index;
958 schar *temp = reader->ptr;
960 reader->ptr = reader->prev_elem;
961 reader->prev_elem = temp;
963 reader->block = last_block;
967 reader->block = first_block;
970 reader->block_min = reader->block->data;
971 reader->block_max = reader->block_min + reader->block->count * seq->elem_size;
975 reader->delta_index = 0;
978 reader->ptr = reader->prev_elem = reader->block_min = reader->block_max = 0;
983 /* Change the current reading block
984 * to the previous or to the next:
987 cvChangeSeqBlock( void* _reader, int direction )
989 CvSeqReader* reader = (CvSeqReader*)_reader;
992 CV_Error( CV_StsNullPtr, "" );
996 reader->block = reader->block->next;
997 reader->ptr = reader->block->data;
1001 reader->block = reader->block->prev;
1002 reader->ptr = CV_GET_LAST_ELEM( reader->seq, reader->block );
1004 reader->block_min = reader->block->data;
1005 reader->block_max = reader->block_min + reader->block->count * reader->seq->elem_size;
1009 /* Return the current reader position: */
1011 cvGetSeqReaderPos( CvSeqReader* reader )
1016 if( !reader || !reader->ptr )
1017 CV_Error( CV_StsNullPtr, "" );
1019 elem_size = reader->seq->elem_size;
1020 if( elem_size <= ICV_SHIFT_TAB_MAX && (index = icvPower2ShiftTab[elem_size - 1]) >= 0 )
1021 index = (int)((reader->ptr - reader->block_min) >> index);
1023 index = (int)((reader->ptr - reader->block_min) / elem_size);
1025 index += reader->block->start_index - reader->delta_index;
1031 /* Set reader position to given position,
1032 * either absolute or relative to the
1036 cvSetSeqReaderPos( CvSeqReader* reader, int index, int is_relative )
1039 int elem_size, count, total;
1041 if( !reader || !reader->seq )
1042 CV_Error( CV_StsNullPtr, "" );
1044 total = reader->seq->total;
1045 elem_size = reader->seq->elem_size;
1051 if( index < -total )
1052 CV_Error( CV_StsOutOfRange, "" );
1055 else if( index >= total )
1058 if( index >= total )
1059 CV_Error( CV_StsOutOfRange, "" );
1062 block = reader->seq->first;
1063 if( index >= (count = block->count) )
1065 if( index + index <= total )
1069 block = block->next;
1072 while( index >= (count = block->count) );
1078 block = block->prev;
1079 total -= block->count;
1081 while( index < total );
1085 reader->ptr = block->data + index * elem_size;
1086 if( reader->block != block )
1088 reader->block = block;
1089 reader->block_min = block->data;
1090 reader->block_max = block->data + block->count * elem_size;
1095 schar* ptr = reader->ptr;
1097 block = reader->block;
1101 while( ptr + index >= reader->block_max )
1103 int delta = (int)(reader->block_max - ptr);
1105 reader->block = block = block->next;
1106 reader->block_min = ptr = block->data;
1107 reader->block_max = block->data + block->count*elem_size;
1109 reader->ptr = ptr + index;
1113 while( ptr + index < reader->block_min )
1115 int delta = (int)(ptr - reader->block_min);
1117 reader->block = block = block->prev;
1118 reader->block_min = block->data;
1119 reader->block_max = ptr = block->data + block->count*elem_size;
1121 reader->ptr = ptr + index;
1127 /* Push element onto the sequence: */
1129 cvSeqPush( CvSeq *seq, const void *element )
1135 CV_Error( CV_StsNullPtr, "" );
1137 elem_size = seq->elem_size;
1140 if( ptr >= seq->block_max )
1142 icvGrowSeq( seq, 0 );
1145 assert( ptr + elem_size <= seq->block_max /*&& ptr == seq->block_min */ );
1149 CV_MEMCPY_AUTO( ptr, element, elem_size );
1150 seq->first->prev->count++;
1152 seq->ptr = ptr + elem_size;
1158 /* Pop last element off of the sequence: */
1160 cvSeqPop( CvSeq *seq, void *element )
1166 CV_Error( CV_StsNullPtr, "" );
1167 if( seq->total <= 0 )
1168 CV_Error( CV_StsBadSize, "" );
1170 elem_size = seq->elem_size;
1171 seq->ptr = ptr = seq->ptr - elem_size;
1174 CV_MEMCPY_AUTO( element, ptr, elem_size );
1178 if( --(seq->first->prev->count) == 0 )
1180 icvFreeSeqBlock( seq, 0 );
1181 assert( seq->ptr == seq->block_max );
1186 /* Push element onto the front of the sequence: */
1188 cvSeqPushFront( CvSeq *seq, const void *element )
1195 CV_Error( CV_StsNullPtr, "" );
1197 elem_size = seq->elem_size;
1200 if( !block || block->start_index == 0 )
1202 icvGrowSeq( seq, 1 );
1205 assert( block->start_index > 0 );
1208 ptr = block->data -= elem_size;
1211 CV_MEMCPY_AUTO( ptr, element, elem_size );
1213 block->start_index--;
1220 /* Shift out first element of the sequence: */
1222 cvSeqPopFront( CvSeq *seq, void *element )
1228 CV_Error( CV_StsNullPtr, "" );
1229 if( seq->total <= 0 )
1230 CV_Error( CV_StsBadSize, "" );
1232 elem_size = seq->elem_size;
1236 CV_MEMCPY_AUTO( element, block->data, elem_size );
1237 block->data += elem_size;
1238 block->start_index++;
1241 if( --(block->count) == 0 )
1242 icvFreeSeqBlock( seq, 1 );
1245 /* Insert new element in middle of sequence: */
1247 cvSeqInsert( CvSeq *seq, int before_index, const void *element )
1257 CV_Error( CV_StsNullPtr, "" );
1260 before_index += before_index < 0 ? total : 0;
1261 before_index -= before_index > total ? total : 0;
1263 if( (unsigned)before_index > (unsigned)total )
1264 CV_Error( CV_StsOutOfRange, "" );
1266 if( before_index == total )
1268 ret_ptr = cvSeqPush( seq, element );
1270 else if( before_index == 0 )
1272 ret_ptr = cvSeqPushFront( seq, element );
1276 elem_size = seq->elem_size;
1278 if( before_index >= total >> 1 )
1280 schar *ptr = seq->ptr + elem_size;
1282 if( ptr > seq->block_max )
1284 icvGrowSeq( seq, 0 );
1286 ptr = seq->ptr + elem_size;
1287 assert( ptr <= seq->block_max );
1290 delta_index = seq->first->start_index;
1291 block = seq->first->prev;
1293 block_size = (int)(ptr - block->data);
1295 while( before_index < block->start_index - delta_index )
1297 CvSeqBlock *prev_block = block->prev;
1299 memmove( block->data + elem_size, block->data, block_size - elem_size );
1300 block_size = prev_block->count * elem_size;
1301 memcpy( block->data, prev_block->data + block_size - elem_size, elem_size );
1304 /* Check that we don't fall into an infinite loop: */
1305 assert( block != seq->first->prev );
1308 before_index = (before_index - block->start_index + delta_index) * elem_size;
1309 memmove( block->data + before_index + elem_size, block->data + before_index,
1310 block_size - before_index - elem_size );
1312 ret_ptr = block->data + before_index;
1315 memcpy( ret_ptr, element, elem_size );
1322 if( block->start_index == 0 )
1324 icvGrowSeq( seq, 1 );
1329 delta_index = block->start_index;
1331 block->start_index--;
1332 block->data -= elem_size;
1334 while( before_index > block->start_index - delta_index + block->count )
1336 CvSeqBlock *next_block = block->next;
1338 block_size = block->count * elem_size;
1339 memmove( block->data, block->data + elem_size, block_size - elem_size );
1340 memcpy( block->data + block_size - elem_size, next_block->data, elem_size );
1343 /* Check that we don't fall into an infinite loop: */
1344 assert( block != seq->first );
1347 before_index = (before_index - block->start_index + delta_index) * elem_size;
1348 memmove( block->data, block->data + elem_size, before_index - elem_size );
1350 ret_ptr = block->data + before_index - elem_size;
1353 memcpy( ret_ptr, element, elem_size );
1356 seq->total = total + 1;
1363 /* Removes element from sequence: */
1365 cvSeqRemove( CvSeq *seq, int index )
1372 int total, front = 0;
1375 CV_Error( CV_StsNullPtr, "" );
1379 index += index < 0 ? total : 0;
1380 index -= index >= total ? total : 0;
1382 if( (unsigned) index >= (unsigned) total )
1383 CV_Error( CV_StsOutOfRange, "Invalid index" );
1385 if( index == total - 1 )
1389 else if( index == 0 )
1391 cvSeqPopFront( seq, 0 );
1396 elem_size = seq->elem_size;
1397 delta_index = block->start_index;
1398 while( block->start_index - delta_index + block->count <= index )
1399 block = block->next;
1401 ptr = block->data + (index - block->start_index + delta_index) * elem_size;
1403 front = index < total >> 1;
1406 block_size = block->count * elem_size - (int)(ptr - block->data);
1408 while( block != seq->first->prev ) /* while not the last block */
1410 CvSeqBlock *next_block = block->next;
1412 memmove( ptr, ptr + elem_size, block_size - elem_size );
1413 memcpy( ptr + block_size - elem_size, next_block->data, elem_size );
1416 block_size = block->count * elem_size;
1419 memmove( ptr, ptr + elem_size, block_size - elem_size );
1420 seq->ptr -= elem_size;
1425 block_size = (int)(ptr - block->data);
1427 while( block != seq->first )
1429 CvSeqBlock *prev_block = block->prev;
1431 memmove( block->data + elem_size, block->data, block_size - elem_size );
1432 block_size = prev_block->count * elem_size;
1433 memcpy( block->data, prev_block->data + block_size - elem_size, elem_size );
1437 memmove( block->data + elem_size, block->data, block_size - elem_size );
1438 block->data += elem_size;
1439 block->start_index++;
1442 seq->total = total - 1;
1443 if( --block->count == 0 )
1444 icvFreeSeqBlock( seq, front );
1449 /* Add several elements to the beginning or end of a sequence: */
1451 cvSeqPushMulti( CvSeq *seq, const void *_elements, int count, int front )
1453 char *elements = (char *) _elements;
1456 CV_Error( CV_StsNullPtr, "NULL sequence pointer" );
1458 CV_Error( CV_StsBadSize, "number of removed elements is negative" );
1460 int elem_size = seq->elem_size;
1466 int delta = (int)((seq->block_max - seq->ptr) / elem_size);
1468 delta = MIN( delta, count );
1471 seq->first->prev->count += delta;
1472 seq->total += delta;
1477 memcpy( seq->ptr, elements, delta );
1484 icvGrowSeq( seq, 0 );
1489 CvSeqBlock* block = seq->first;
1495 if( !block || block->start_index == 0 )
1497 icvGrowSeq( seq, 1 );
1500 assert( block->start_index > 0 );
1503 delta = MIN( block->start_index, count );
1505 block->start_index -= delta;
1506 block->count += delta;
1507 seq->total += delta;
1509 block->data -= delta;
1512 memcpy( block->data, elements + count*elem_size, delta );
1518 /* Remove several elements from the end of sequence: */
1520 cvSeqPopMulti( CvSeq *seq, void *_elements, int count, int front )
1522 char *elements = (char *) _elements;
1525 CV_Error( CV_StsNullPtr, "NULL sequence pointer" );
1527 CV_Error( CV_StsBadSize, "number of removed elements is negative" );
1529 count = MIN( count, seq->total );
1534 elements += count * seq->elem_size;
1538 int delta = seq->first->prev->count;
1540 delta = MIN( delta, count );
1541 assert( delta > 0 );
1543 seq->first->prev->count -= delta;
1544 seq->total -= delta;
1546 delta *= seq->elem_size;
1552 memcpy( elements, seq->ptr, delta );
1555 if( seq->first->prev->count == 0 )
1556 icvFreeSeqBlock( seq, 0 );
1563 int delta = seq->first->count;
1565 delta = MIN( delta, count );
1566 assert( delta > 0 );
1568 seq->first->count -= delta;
1569 seq->total -= delta;
1571 seq->first->start_index += delta;
1572 delta *= seq->elem_size;
1576 memcpy( elements, seq->first->data, delta );
1580 seq->first->data += delta;
1581 if( seq->first->count == 0 )
1582 icvFreeSeqBlock( seq, 1 );
1588 /* Remove all elements from a sequence: */
1590 cvClearSeq( CvSeq *seq )
1593 CV_Error( CV_StsNullPtr, "" );
1594 cvSeqPopMulti( seq, 0, seq->total );
1599 cvSeqSlice( const CvSeq* seq, CvSlice slice, CvMemStorage* storage, int copy_data )
1602 int elem_size, count, length;
1604 CvSeqBlock *block, *first_block = 0, *last_block = 0;
1606 if( !CV_IS_SEQ(seq) )
1607 CV_Error( CV_StsBadArg, "Invalid sequence header" );
1611 storage = seq->storage;
1613 CV_Error( CV_StsNullPtr, "NULL storage pointer" );
1616 elem_size = seq->elem_size;
1617 length = cvSliceLength( slice, seq );
1618 if( slice.start_index < 0 )
1619 slice.start_index += seq->total;
1620 else if( slice.start_index >= seq->total )
1621 slice.start_index -= seq->total;
1622 if( (unsigned)length > (unsigned)seq->total ||
1623 ((unsigned)slice.start_index >= (unsigned)seq->total && length != 0) )
1624 CV_Error( CV_StsOutOfRange, "Bad sequence slice" );
1626 subseq = cvCreateSeq( seq->flags, seq->header_size, elem_size, storage );
1630 cvStartReadSeq( seq, &reader, 0 );
1631 cvSetSeqReaderPos( &reader, slice.start_index, 0 );
1632 count = (int)((reader.block_max - reader.ptr)/elem_size);
1636 int bl = MIN( count, length );
1640 block = (CvSeqBlock*)cvMemStorageAlloc( storage, sizeof(*block) );
1643 first_block = subseq->first = block->prev = block->next = block;
1644 block->start_index = 0;
1648 block->prev = last_block;
1649 block->next = first_block;
1650 last_block->next = first_block->prev = block;
1651 block->start_index = last_block->start_index + last_block->count;
1654 block->data = reader.ptr;
1656 subseq->total += bl;
1659 cvSeqPushMulti( subseq, reader.ptr, bl, 0 );
1661 reader.block = reader.block->next;
1662 reader.ptr = reader.block->data;
1663 count = reader.block->count;
1665 while( length > 0 );
1672 // Remove slice from the middle of the sequence.
1673 // !!! TODO !!! Implement more efficient algorithm
1675 cvSeqRemoveSlice( CvSeq* seq, CvSlice slice )
1679 if( !CV_IS_SEQ(seq) )
1680 CV_Error( CV_StsBadArg, "Invalid sequence header" );
1682 length = cvSliceLength( slice, seq );
1685 if( slice.start_index < 0 )
1686 slice.start_index += total;
1687 else if( slice.start_index >= total )
1688 slice.start_index -= total;
1690 if( (unsigned)slice.start_index >= (unsigned)total )
1691 CV_Error( CV_StsOutOfRange, "start slice index is out of range" );
1693 slice.end_index = slice.start_index + length;
1695 if( slice.end_index < total )
1697 CvSeqReader reader_to, reader_from;
1698 int elem_size = seq->elem_size;
1700 cvStartReadSeq( seq, &reader_to );
1701 cvStartReadSeq( seq, &reader_from );
1703 if( slice.start_index > total - slice.end_index )
1705 int i, count = seq->total - slice.end_index;
1706 cvSetSeqReaderPos( &reader_to, slice.start_index );
1707 cvSetSeqReaderPos( &reader_from, slice.end_index );
1709 for( i = 0; i < count; i++ )
1711 CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );
1712 CV_NEXT_SEQ_ELEM( elem_size, reader_to );
1713 CV_NEXT_SEQ_ELEM( elem_size, reader_from );
1716 cvSeqPopMulti( seq, 0, slice.end_index - slice.start_index );
1720 int i, count = slice.start_index;
1721 cvSetSeqReaderPos( &reader_to, slice.end_index );
1722 cvSetSeqReaderPos( &reader_from, slice.start_index );
1724 for( i = 0; i < count; i++ )
1726 CV_PREV_SEQ_ELEM( elem_size, reader_to );
1727 CV_PREV_SEQ_ELEM( elem_size, reader_from );
1729 CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );
1732 cvSeqPopMulti( seq, 0, slice.end_index - slice.start_index, 1 );
1737 cvSeqPopMulti( seq, 0, total - slice.start_index );
1738 cvSeqPopMulti( seq, 0, slice.end_index - total, 1 );
1743 // Insert a sequence into the middle of another sequence:
1744 // !!! TODO !!! Implement more efficient algorithm
1746 cvSeqInsertSlice( CvSeq* seq, int index, const CvArr* from_arr )
1748 CvSeqReader reader_to, reader_from;
1749 int i, elem_size, total, from_total;
1750 CvSeq from_header, *from = (CvSeq*)from_arr;
1753 if( !CV_IS_SEQ(seq) )
1754 CV_Error( CV_StsBadArg, "Invalid destination sequence header" );
1756 if( !CV_IS_SEQ(from))
1758 CvMat* mat = (CvMat*)from;
1759 if( !CV_IS_MAT(mat))
1760 CV_Error( CV_StsBadArg, "Source is not a sequence nor matrix" );
1762 if( !CV_IS_MAT_CONT(mat->type) || (mat->rows != 1 && mat->cols != 1) )
1763 CV_Error( CV_StsBadArg, "The source array must be 1d coninuous vector" );
1765 from = cvMakeSeqHeaderForArray( CV_SEQ_KIND_GENERIC, sizeof(from_header),
1766 CV_ELEM_SIZE(mat->type),
1767 mat->data.ptr, mat->cols + mat->rows - 1,
1768 &from_header, &block );
1771 if( seq->elem_size != from->elem_size )
1772 CV_Error( CV_StsUnmatchedSizes,
1773 "Source and destination sequence element sizes are different." );
1775 from_total = from->total;
1777 if( from_total == 0 )
1781 index += index < 0 ? total : 0;
1782 index -= index > total ? total : 0;
1784 if( (unsigned)index > (unsigned)total )
1785 CV_Error( CV_StsOutOfRange, "" );
1787 elem_size = seq->elem_size;
1789 if( index < (total >> 1) )
1791 cvSeqPushMulti( seq, 0, from_total, 1 );
1793 cvStartReadSeq( seq, &reader_to );
1794 cvStartReadSeq( seq, &reader_from );
1795 cvSetSeqReaderPos( &reader_from, from_total );
1797 for( i = 0; i < index; i++ )
1799 CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );
1800 CV_NEXT_SEQ_ELEM( elem_size, reader_to );
1801 CV_NEXT_SEQ_ELEM( elem_size, reader_from );
1806 cvSeqPushMulti( seq, 0, from_total );
1808 cvStartReadSeq( seq, &reader_to );
1809 cvStartReadSeq( seq, &reader_from );
1810 cvSetSeqReaderPos( &reader_from, total );
1811 cvSetSeqReaderPos( &reader_to, seq->total );
1813 for( i = 0; i < total - index; i++ )
1815 CV_PREV_SEQ_ELEM( elem_size, reader_to );
1816 CV_PREV_SEQ_ELEM( elem_size, reader_from );
1817 CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );
1821 cvStartReadSeq( from, &reader_from );
1822 cvSetSeqReaderPos( &reader_to, index );
1824 for( i = 0; i < from_total; i++ )
1826 CV_MEMCPY_AUTO( reader_to.ptr, reader_from.ptr, elem_size );
1827 CV_NEXT_SEQ_ELEM( elem_size, reader_to );
1828 CV_NEXT_SEQ_ELEM( elem_size, reader_from );
1832 // Sort the sequence using user-specified comparison function.
1833 // The semantics is similar to qsort() function.
1834 // The code is based on BSD system qsort():
1835 // * Copyright (c) 1992, 1993
1836 // * The Regents of the University of California. All rights reserved.
1838 // * Redistribution and use in source and binary forms, with or without
1839 // * modification, are permitted provided that the following conditions
1841 // * 1. Redistributions of source code must retain the above copyright
1842 // * notice, this list of conditions and the following disclaimer.
1843 // * 2. Redistributions in binary form must reproduce the above copyright
1844 // * notice, this list of conditions and the following disclaimer in the
1845 // * documentation and/or other materials provided with the distribution.
1846 // * 3. All advertising materials mentioning features or use of this software
1847 // * must display the following acknowledgement:
1848 // * This product includes software developed by the University of
1849 // * California, Berkeley and its contributors.
1850 // * 4. Neither the name of the University nor the names of its contributors
1851 // * may be used to endorse or promote products derived from this software
1852 // * without specific prior written permission.
1854 // * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
1855 // * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
1856 // * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
1857 // * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
1858 // * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
1859 // * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
1860 // * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
1861 // * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
1862 // * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
1863 // * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
1866 typedef struct CvSeqReaderPos
1875 #define CV_SAVE_READER_POS( reader, pos ) \
1877 (pos).block = (reader).block; \
1878 (pos).ptr = (reader).ptr; \
1879 (pos).block_min = (reader).block_min; \
1880 (pos).block_max = (reader).block_max; \
1883 #define CV_RESTORE_READER_POS( reader, pos )\
1885 (reader).block = (pos).block; \
1886 (reader).ptr = (pos).ptr; \
1887 (reader).block_min = (pos).block_min; \
1888 (reader).block_max = (pos).block_max; \
1892 icvMed3( schar* a, schar* b, schar* c, CvCmpFunc cmp_func, void* aux )
1894 return cmp_func(a, b, aux) < 0 ?
1895 (cmp_func(b, c, aux) < 0 ? b : cmp_func(a, c, aux) < 0 ? c : a)
1896 :(cmp_func(b, c, aux) > 0 ? b : cmp_func(a, c, aux) < 0 ? a : c);
1900 cvSeqSort( CvSeq* seq, CvCmpFunc cmp_func, void* aux )
1903 int isort_thresh = 7;
1904 CvSeqReader left, right;
1914 if( !CV_IS_SEQ(seq) )
1915 CV_Error( !seq ? CV_StsNullPtr : CV_StsBadArg, "Bad input sequence" );
1918 CV_Error( CV_StsNullPtr, "Null compare function" );
1920 if( seq->total <= 1 )
1923 elem_size = seq->elem_size;
1924 isort_thresh *= elem_size;
1926 cvStartReadSeq( seq, &left, 0 );
1928 CV_SAVE_READER_POS( left, stack[0].lb );
1929 CV_PREV_SEQ_ELEM( elem_size, right );
1930 CV_SAVE_READER_POS( right, stack[0].ub );
1934 CV_RESTORE_READER_POS( left, stack[sp].lb );
1935 CV_RESTORE_READER_POS( right, stack[sp].ub );
1941 CvSeqReader ptr, ptr2;
1943 if( left.block == right.block )
1944 n = (int)(right.ptr - left.ptr) + elem_size;
1947 n = cvGetSeqReaderPos( &right );
1948 n = (n - cvGetSeqReaderPos( &left ) + 1)*elem_size;
1951 if( n <= isort_thresh )
1955 CV_NEXT_SEQ_ELEM( elem_size, ptr );
1956 CV_NEXT_SEQ_ELEM( elem_size, right );
1957 while( ptr.ptr != right.ptr )
1960 if( ptr2.block != ptr.block )
1962 ptr2.block = ptr.block;
1963 ptr2.block_min = ptr.block_min;
1964 ptr2.block_max = ptr.block_max;
1966 while( ptr2.ptr != left.ptr )
1968 schar* cur = ptr2.ptr;
1969 CV_PREV_SEQ_ELEM( elem_size, ptr2 );
1970 if( cmp_func( ptr2.ptr, cur, aux ) <= 0 )
1972 CV_SWAP_ELEMS( ptr2.ptr, cur, elem_size );
1974 CV_NEXT_SEQ_ELEM( elem_size, ptr );
1980 CvSeqReader left0, left1, right0, right1;
1981 CvSeqReader tmp0, tmp1;
1982 schar *m1, *m2, *m3, *pivot;
1984 int l, l0, l1, r, r0, r1;
1986 left0 = tmp0 = left;
1987 right0 = right1 = right;
1993 schar *p1, *p2, *p3;
1995 cvSetSeqReaderPos( &tmp0, d, 1 );
1997 cvSetSeqReaderPos( &tmp0, d, 1 );
1999 m1 = icvMed3( p1, p2, p3, cmp_func, aux );
2000 cvSetSeqReaderPos( &tmp0, (n/2) - d*3, 1 );
2002 cvSetSeqReaderPos( &tmp0, d, 1 );
2004 cvSetSeqReaderPos( &tmp0, d, 1 );
2006 m2 = icvMed3( p1, p2, p3, cmp_func, aux );
2007 cvSetSeqReaderPos( &tmp0, n - 1 - d*3 - n/2, 1 );
2009 cvSetSeqReaderPos( &tmp0, d, 1 );
2011 cvSetSeqReaderPos( &tmp0, d, 1 );
2013 m3 = icvMed3( p1, p2, p3, cmp_func, aux );
2018 cvSetSeqReaderPos( &tmp0, n/2, 1 );
2020 cvSetSeqReaderPos( &tmp0, n - 1 - n/2, 1 );
2024 pivot = icvMed3( m1, m2, m3, cmp_func, aux );
2026 if( pivot != left.ptr )
2028 CV_SWAP_ELEMS( pivot, left.ptr, elem_size );
2031 CV_NEXT_SEQ_ELEM( elem_size, left );
2036 while( left.ptr != right.ptr && (r = cmp_func(left.ptr, pivot, aux)) <= 0 )
2040 if( left1.ptr != left.ptr )
2041 CV_SWAP_ELEMS( left1.ptr, left.ptr, elem_size );
2043 CV_NEXT_SEQ_ELEM( elem_size, left1 );
2045 CV_NEXT_SEQ_ELEM( elem_size, left );
2048 while( left.ptr != right.ptr && (r = cmp_func(right.ptr,pivot, aux)) >= 0 )
2052 if( right1.ptr != right.ptr )
2053 CV_SWAP_ELEMS( right1.ptr, right.ptr, elem_size );
2055 CV_PREV_SEQ_ELEM( elem_size, right1 );
2057 CV_PREV_SEQ_ELEM( elem_size, right );
2060 if( left.ptr == right.ptr )
2062 r = cmp_func(left.ptr, pivot, aux);
2065 if( left1.ptr != left.ptr )
2066 CV_SWAP_ELEMS( left1.ptr, left.ptr, elem_size );
2068 CV_NEXT_SEQ_ELEM( elem_size, left1 );
2072 CV_NEXT_SEQ_ELEM( elem_size, left );
2076 CV_PREV_SEQ_ELEM( elem_size, right );
2081 CV_SWAP_ELEMS( left.ptr, right.ptr, elem_size );
2082 CV_NEXT_SEQ_ELEM( elem_size, left );
2083 r = left.ptr == right.ptr;
2084 CV_PREV_SEQ_ELEM( elem_size, right );
2092 left = left0, right = right0;
2096 l = cvGetSeqReaderPos( &left );
2099 l0 = cvGetSeqReaderPos( &left0 );
2100 l1 = cvGetSeqReaderPos( &left1 );
2104 n = MIN( l - l1, l1 - l0 );
2109 cvSetSeqReaderPos( &tmp1, 0-n, 1 );
2110 for( i = 0; i < n; i++ )
2112 CV_SWAP_ELEMS( tmp0.ptr, tmp1.ptr, elem_size );
2113 CV_NEXT_SEQ_ELEM( elem_size, tmp0 );
2114 CV_NEXT_SEQ_ELEM( elem_size, tmp1 );
2118 r = cvGetSeqReaderPos( &right );
2119 r0 = cvGetSeqReaderPos( &right0 );
2120 r1 = cvGetSeqReaderPos( &right1 );
2121 m = MIN( r0 - r1, r1 - r );
2126 cvSetSeqReaderPos( &tmp1, 1-m, 1 );
2127 for( i = 0; i < m; i++ )
2129 CV_SWAP_ELEMS( tmp0.ptr, tmp1.ptr, elem_size );
2130 CV_NEXT_SEQ_ELEM( elem_size, tmp0 );
2131 CV_NEXT_SEQ_ELEM( elem_size, tmp1 );
2144 CV_SAVE_READER_POS( left0, stack[sp].lb );
2145 cvSetSeqReaderPos( &left0, n - 1, 1 );
2146 CV_SAVE_READER_POS( left0, stack[sp].ub );
2147 left = right = right0;
2148 cvSetSeqReaderPos( &left, 1 - m, 1 );
2153 CV_SAVE_READER_POS( right0, stack[sp].ub );
2154 cvSetSeqReaderPos( &right0, 1 - m, 1 );
2155 CV_SAVE_READER_POS( right0, stack[sp].lb );
2156 left = right = left0;
2157 cvSetSeqReaderPos( &right, n - 1, 1 );
2162 left = right = left0;
2163 cvSetSeqReaderPos( &right, n - 1, 1 );
2168 left = right = right0;
2169 cvSetSeqReaderPos( &left, 1 - m, 1 );
2180 cvSeqSearch( CvSeq* seq, const void* _elem, CvCmpFunc cmp_func,
2181 int is_sorted, int* _idx, void* userdata )
2184 const schar* elem = (const schar*)_elem;
2191 if( !CV_IS_SEQ(seq) )
2192 CV_Error( !seq ? CV_StsNullPtr : CV_StsBadArg, "Bad input sequence" );
2195 CV_Error( CV_StsNullPtr, "Null element pointer" );
2197 int elem_size = seq->elem_size;
2198 int total = seq->total;
2206 cvStartReadSeq( seq, &reader, 0 );
2210 for( i = 0; i < total; i++ )
2212 if( cmp_func( elem, reader.ptr, userdata ) == 0 )
2214 CV_NEXT_SEQ_ELEM( elem_size, reader );
2217 else if( (elem_size & (sizeof(int)-1)) == 0 )
2219 for( i = 0; i < total; i++ )
2221 for( j = 0; j < elem_size; j += sizeof(int) )
2223 if( *(const int*)(reader.ptr + j) != *(const int*)(elem + j) )
2226 if( j == elem_size )
2228 CV_NEXT_SEQ_ELEM( elem_size, reader );
2233 for( i = 0; i < total; i++ )
2235 for( j = 0; j < elem_size; j++ )
2237 if( reader.ptr[j] != elem[j] )
2240 if( j == elem_size )
2242 CV_NEXT_SEQ_ELEM( elem_size, reader );
2248 result = reader.ptr;
2253 CV_Error( CV_StsNullPtr, "Null compare function" );
2259 int k = (i+j)>>1, code;
2260 schar* ptr = cvGetSeqElem( seq, k );
2261 code = cmp_func( elem, ptr, userdata );
2286 cvSeqInvert( CvSeq* seq )
2288 CvSeqReader left_reader, right_reader;
2292 cvStartReadSeq( seq, &left_reader, 0 );
2293 cvStartReadSeq( seq, &right_reader, 1 );
2294 elem_size = seq->elem_size;
2295 count = seq->total >> 1;
2297 for( i = 0; i < count; i++ )
2299 CV_SWAP_ELEMS( left_reader.ptr, right_reader.ptr, elem_size );
2300 CV_NEXT_SEQ_ELEM( elem_size, left_reader );
2301 CV_PREV_SEQ_ELEM( elem_size, right_reader );
2306 typedef struct CvPTreeNode
2308 struct CvPTreeNode* parent;
2315 // This function splits the input sequence or set into one or more equivalence classes.
2316 // is_equal(a,b,...) returns non-zero if the two sequence elements
2317 // belong to the same class. The function returns sequence of integers -
2318 // 0-based class indexes for each element.
2320 // The algorithm is described in "Introduction to Algorithms"
2321 // by Cormen, Leiserson and Rivest, chapter "Data structures for disjoint sets"
2323 cvSeqPartition( const CvSeq* seq, CvMemStorage* storage, CvSeq** labels,
2324 CvCmpFunc is_equal, void* userdata )
2327 CvMemStorage* temp_storage = 0;
2331 CvSeqReader reader, reader0;
2337 CV_Error( CV_StsNullPtr, "" );
2339 if( !seq || !is_equal )
2340 CV_Error( CV_StsNullPtr, "" );
2343 storage = seq->storage;
2346 CV_Error( CV_StsNullPtr, "" );
2348 is_set = CV_IS_SET(seq);
2350 temp_storage = cvCreateChildMemStorage( storage );
2352 nodes = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvPTreeNode), temp_storage );
2354 cvStartReadSeq( seq, &reader );
2355 memset( &writer, 0, sizeof(writer));
2356 cvStartAppendToSeq( nodes, &writer );
2358 // Initial O(N) pass. Make a forest of single-vertex trees.
2359 for( i = 0; i < seq->total; i++ )
2361 CvPTreeNode node = { 0, 0, 0 };
2362 if( !is_set || CV_IS_SET_ELEM( reader.ptr ))
2363 node.element = reader.ptr;
2364 CV_WRITE_SEQ_ELEM( node, writer );
2365 CV_NEXT_SEQ_ELEM( seq->elem_size, reader );
2368 cvEndWriteSeq( &writer );
2370 // Because in the next loop we will iterate
2371 // through all the sequence nodes each time,
2372 // we do not need to initialize reader every time:
2373 cvStartReadSeq( nodes, &reader );
2374 cvStartReadSeq( nodes, &reader0 );
2376 // The main O(N^2) pass. Merge connected components.
2377 for( i = 0; i < nodes->total; i++ )
2379 CvPTreeNode* node = (CvPTreeNode*)(reader0.ptr);
2380 CvPTreeNode* root = node;
2381 CV_NEXT_SEQ_ELEM( nodes->elem_size, reader0 );
2383 if( !node->element )
2387 while( root->parent )
2388 root = root->parent;
2390 for( j = 0; j < nodes->total; j++ )
2392 CvPTreeNode* node2 = (CvPTreeNode*)reader.ptr;
2394 if( node2->element && node2 != node &&
2395 is_equal( node->element, node2->element, userdata ))
2397 CvPTreeNode* root2 = node2;
2400 while( root2->parent )
2401 root2 = root2->parent;
2405 if( root->rank > root2->rank )
2406 root2->parent = root;
2409 root->parent = root2;
2410 root2->rank += root->rank == root2->rank;
2413 assert( root->parent == 0 );
2415 // Compress path from node2 to the root:
2416 while( node2->parent )
2418 CvPTreeNode* temp = node2;
2419 node2 = node2->parent;
2420 temp->parent = root;
2423 // Compress path from node to the root:
2425 while( node2->parent )
2427 CvPTreeNode* temp = node2;
2428 node2 = node2->parent;
2429 temp->parent = root;
2434 CV_NEXT_SEQ_ELEM( sizeof(*node), reader );
2438 // Final O(N) pass (Enumerate classes)
2439 // Reuse reader one more time
2440 result = cvCreateSeq( 0, sizeof(CvSeq), sizeof(int), storage );
2441 cvStartAppendToSeq( result, &writer );
2443 for( i = 0; i < nodes->total; i++ )
2445 CvPTreeNode* node = (CvPTreeNode*)reader.ptr;
2450 while( node->parent )
2451 node = node->parent;
2452 if( node->rank >= 0 )
2453 node->rank = ~class_idx++;
2457 CV_NEXT_SEQ_ELEM( sizeof(*node), reader );
2458 CV_WRITE_SEQ_ELEM( idx, writer );
2461 cvEndWriteSeq( &writer );
2466 cvReleaseMemStorage( &temp_storage );
2471 /****************************************************************************************\
2472 * Set implementation *
2473 \****************************************************************************************/
2475 /* Creates empty set: */
2477 cvCreateSet( int set_flags, int header_size, int elem_size, CvMemStorage * storage )
2480 CV_Error( CV_StsNullPtr, "" );
2481 if( header_size < (int)sizeof( CvSet ) ||
2482 elem_size < (int)sizeof(void*)*2 ||
2483 (elem_size & (sizeof(void*)-1)) != 0 )
2484 CV_Error( CV_StsBadSize, "" );
2486 CvSet* set = (CvSet*) cvCreateSeq( set_flags, header_size, elem_size, storage );
2487 set->flags = (set->flags & ~CV_MAGIC_MASK) | CV_SET_MAGIC_VAL;
2493 /* Add new element to the set: */
2495 cvSetAdd( CvSet* set, CvSetElem* element, CvSetElem** inserted_element )
2498 CvSetElem *free_elem;
2501 CV_Error( CV_StsNullPtr, "" );
2503 if( !(set->free_elems) )
2505 int count = set->total;
2506 int elem_size = set->elem_size;
2508 icvGrowSeq( (CvSeq *) set, 0 );
2510 set->free_elems = (CvSetElem*) (ptr = set->ptr);
2511 for( ; ptr + elem_size <= set->block_max; ptr += elem_size, count++ )
2513 ((CvSetElem*)ptr)->flags = count | CV_SET_ELEM_FREE_FLAG;
2514 ((CvSetElem*)ptr)->next_free = (CvSetElem*)(ptr + elem_size);
2516 assert( count <= CV_SET_ELEM_IDX_MASK+1 );
2517 ((CvSetElem*)(ptr - elem_size))->next_free = 0;
2518 set->first->prev->count += count - set->total;
2520 set->ptr = set->block_max;
2523 free_elem = set->free_elems;
2524 set->free_elems = free_elem->next_free;
2526 id = free_elem->flags & CV_SET_ELEM_IDX_MASK;
2528 CV_MEMCPY_INT( free_elem, element, (size_t)set->elem_size/sizeof(int) );
2530 free_elem->flags = id;
2531 set->active_count++;
2533 if( inserted_element )
2534 *inserted_element = free_elem;
2540 /* Remove element from a set given element index: */
2542 cvSetRemove( CvSet* set, int index )
2544 CvSetElem* elem = cvGetSetElem( set, index );
2546 cvSetRemoveByPtr( set, elem );
2548 CV_Error( CV_StsNullPtr, "" );
2552 /* Remove all elements from a set: */
2554 cvClearSet( CvSet* set )
2556 cvClearSeq( (CvSeq*)set );
2557 set->free_elems = 0;
2558 set->active_count = 0;
2562 /****************************************************************************************\
2563 * Graph implementation *
2564 \****************************************************************************************/
2566 /* Create a new graph: */
2568 cvCreateGraph( int graph_type, int header_size,
2569 int vtx_size, int edge_size, CvMemStorage * storage )
2573 CvSet *vertices = 0;
2575 if( header_size < (int) sizeof( CvGraph )
2576 || edge_size < (int) sizeof( CvGraphEdge )
2577 || vtx_size < (int) sizeof( CvGraphVtx )
2579 CV_Error( CV_StsBadSize, "" );
2582 vertices = cvCreateSet( graph_type, header_size, vtx_size, storage );
2583 edges = cvCreateSet( CV_SEQ_KIND_GENERIC | CV_SEQ_ELTYPE_GRAPH_EDGE,
2584 sizeof( CvSet ), edge_size, storage );
2586 graph = (CvGraph*)vertices;
2587 graph->edges = edges;
2593 /* Remove all vertices and edges from a graph: */
2595 cvClearGraph( CvGraph * graph )
2598 CV_Error( CV_StsNullPtr, "" );
2600 cvClearSet( graph->edges );
2601 cvClearSet( (CvSet*)graph );
2605 /* Add a vertex to a graph: */
2607 cvGraphAddVtx( CvGraph* graph, const CvGraphVtx* _vertex, CvGraphVtx** _inserted_vertex )
2609 CvGraphVtx *vertex = 0;
2613 CV_Error( CV_StsNullPtr, "" );
2615 vertex = (CvGraphVtx*)cvSetNew((CvSet*)graph);
2619 CV_MEMCPY_INT( vertex + 1, _vertex + 1,
2620 (size_t)(graph->elem_size - sizeof(CvGraphVtx))/sizeof(int) );
2622 index = vertex->flags;
2625 if( _inserted_vertex )
2626 *_inserted_vertex = vertex;
2632 /* Remove a vertex from the graph together with its incident edges: */
2634 cvGraphRemoveVtxByPtr( CvGraph* graph, CvGraphVtx* vtx )
2638 if( !graph || !vtx )
2639 CV_Error( CV_StsNullPtr, "" );
2641 if( !CV_IS_SET_ELEM(vtx))
2642 CV_Error( CV_StsBadArg, "The vertex does not belong to the graph" );
2644 count = graph->edges->active_count;
2647 CvGraphEdge *edge = vtx->first;
2650 cvGraphRemoveEdgeByPtr( graph, edge->vtx[0], edge->vtx[1] );
2652 count -= graph->edges->active_count;
2653 cvSetRemoveByPtr( (CvSet*)graph, vtx );
2659 /* Remove a vertex from the graph together with its incident edges: */
2661 cvGraphRemoveVtx( CvGraph* graph, int index )
2664 CvGraphVtx *vtx = 0;
2667 CV_Error( CV_StsNullPtr, "" );
2669 vtx = cvGetGraphVtx( graph, index );
2671 CV_Error( CV_StsBadArg, "The vertex is not found" );
2673 count = graph->edges->active_count;
2676 CvGraphEdge *edge = vtx->first;
2681 cvGraphRemoveEdgeByPtr( graph, edge->vtx[0], edge->vtx[1] );
2683 count -= graph->edges->active_count;
2684 cvSetRemoveByPtr( (CvSet*)graph, vtx );
2690 /* Find a graph edge given pointers to the ending vertices: */
2691 CV_IMPL CvGraphEdge*
2692 cvFindGraphEdgeByPtr( const CvGraph* graph,
2693 const CvGraphVtx* start_vtx,
2694 const CvGraphVtx* end_vtx )
2698 if( !graph || !start_vtx || !end_vtx )
2699 CV_Error( CV_StsNullPtr, "" );
2701 if( start_vtx == end_vtx )
2704 if( !CV_IS_GRAPH_ORIENTED( graph ) &&
2705 (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) )
2707 const CvGraphVtx* t;
2708 CV_SWAP( start_vtx, end_vtx, t );
2711 CvGraphEdge* edge = start_vtx->first;
2712 for( ; edge; edge = edge->next[ofs] )
2714 ofs = start_vtx == edge->vtx[1];
2715 assert( ofs == 1 || start_vtx == edge->vtx[0] );
2716 if( edge->vtx[1] == end_vtx )
2724 /* Find an edge in the graph given indices of the ending vertices: */
2725 CV_IMPL CvGraphEdge *
2726 cvFindGraphEdge( const CvGraph* graph, int start_idx, int end_idx )
2728 CvGraphVtx *start_vtx;
2729 CvGraphVtx *end_vtx;
2732 CV_Error( CV_StsNullPtr, "graph pointer is NULL" );
2734 start_vtx = cvGetGraphVtx( graph, start_idx );
2735 end_vtx = cvGetGraphVtx( graph, end_idx );
2737 return cvFindGraphEdgeByPtr( graph, start_vtx, end_vtx );
2741 /* Given two vertices, return the edge
2742 * connecting them, creating it if it
2743 * did not already exist:
2746 cvGraphAddEdgeByPtr( CvGraph* graph,
2747 CvGraphVtx* start_vtx, CvGraphVtx* end_vtx,
2748 const CvGraphEdge* _edge,
2749 CvGraphEdge ** _inserted_edge )
2751 CvGraphEdge *edge = 0;
2756 CV_Error( CV_StsNullPtr, "graph pointer is NULL" );
2758 if( !CV_IS_GRAPH_ORIENTED( graph ) &&
2759 (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) )
2762 CV_SWAP( start_vtx, end_vtx, t );
2765 edge = cvFindGraphEdgeByPtr( graph, start_vtx, end_vtx );
2769 if( _inserted_edge )
2770 *_inserted_edge = edge;
2774 if( start_vtx == end_vtx )
2775 CV_Error( start_vtx ? CV_StsBadArg : CV_StsNullPtr,
2776 "vertex pointers coinside (or set to NULL)" );
2778 edge = (CvGraphEdge*)cvSetNew( (CvSet*)(graph->edges) );
2779 assert( edge->flags >= 0 );
2781 edge->vtx[0] = start_vtx;
2782 edge->vtx[1] = end_vtx;
2783 edge->next[0] = start_vtx->first;
2784 edge->next[1] = end_vtx->first;
2785 start_vtx->first = end_vtx->first = edge;
2787 delta = (graph->edges->elem_size - sizeof(*edge))/sizeof(int);
2791 CV_MEMCPY_INT( edge + 1, _edge + 1, delta );
2792 edge->weight = _edge->weight;
2797 CV_ZERO_INT( edge + 1, delta );
2803 if( _inserted_edge )
2804 *_inserted_edge = edge;
2809 /* Given two vertices, return the edge
2810 * connecting them, creating it if it
2811 * did not already exist:
2814 cvGraphAddEdge( CvGraph* graph,
2815 int start_idx, int end_idx,
2816 const CvGraphEdge* _edge,
2817 CvGraphEdge ** _inserted_edge )
2819 CvGraphVtx *start_vtx;
2820 CvGraphVtx *end_vtx;
2823 CV_Error( CV_StsNullPtr, "" );
2825 start_vtx = cvGetGraphVtx( graph, start_idx );
2826 end_vtx = cvGetGraphVtx( graph, end_idx );
2828 return cvGraphAddEdgeByPtr( graph, start_vtx, end_vtx, _edge, _inserted_edge );
2832 /* Remove the graph edge connecting two given vertices: */
2834 cvGraphRemoveEdgeByPtr( CvGraph* graph, CvGraphVtx* start_vtx, CvGraphVtx* end_vtx )
2837 CvGraphEdge *edge, *next_edge, *prev_edge;
2839 if( !graph || !start_vtx || !end_vtx )
2840 CV_Error( CV_StsNullPtr, "" );
2842 if( start_vtx == end_vtx )
2845 if( !CV_IS_GRAPH_ORIENTED( graph ) &&
2846 (start_vtx->flags & CV_SET_ELEM_IDX_MASK) > (end_vtx->flags & CV_SET_ELEM_IDX_MASK) )
2849 CV_SWAP( start_vtx, end_vtx, t );
2852 for( ofs = prev_ofs = 0, prev_edge = 0, edge = start_vtx->first; edge != 0;
2853 prev_ofs = ofs, prev_edge = edge, edge = edge->next[ofs] )
2855 ofs = start_vtx == edge->vtx[1];
2856 assert( ofs == 1 || start_vtx == edge->vtx[0] );
2857 if( edge->vtx[1] == end_vtx )
2864 next_edge = edge->next[ofs];
2866 prev_edge->next[prev_ofs] = next_edge;
2868 start_vtx->first = next_edge;
2870 for( ofs = prev_ofs = 0, prev_edge = 0, edge = end_vtx->first; edge != 0;
2871 prev_ofs = ofs, prev_edge = edge, edge = edge->next[ofs] )
2873 ofs = end_vtx == edge->vtx[1];
2874 assert( ofs == 1 || end_vtx == edge->vtx[0] );
2875 if( edge->vtx[0] == start_vtx )
2879 assert( edge != 0 );
2881 next_edge = edge->next[ofs];
2883 prev_edge->next[prev_ofs] = next_edge;
2885 end_vtx->first = next_edge;
2887 cvSetRemoveByPtr( graph->edges, edge );
2891 /* Remove the graph edge connecting two given vertices: */
2893 cvGraphRemoveEdge( CvGraph* graph, int start_idx, int end_idx )
2895 CvGraphVtx *start_vtx;
2896 CvGraphVtx *end_vtx;
2899 CV_Error( CV_StsNullPtr, "" );
2901 start_vtx = cvGetGraphVtx( graph, start_idx );
2902 end_vtx = cvGetGraphVtx( graph, end_idx );
2904 cvGraphRemoveEdgeByPtr( graph, start_vtx, end_vtx );
2908 /* Count number of edges incident to a given vertex: */
2910 cvGraphVtxDegreeByPtr( const CvGraph* graph, const CvGraphVtx* vertex )
2915 if( !graph || !vertex )
2916 CV_Error( CV_StsNullPtr, "" );
2918 for( edge = vertex->first, count = 0; edge; )
2921 edge = CV_NEXT_GRAPH_EDGE( edge, vertex );
2928 /* Count number of edges incident to a given vertex: */
2930 cvGraphVtxDegree( const CvGraph* graph, int vtx_idx )
2937 CV_Error( CV_StsNullPtr, "" );
2939 vertex = cvGetGraphVtx( graph, vtx_idx );
2941 CV_Error( CV_StsObjectNotFound, "" );
2943 for( edge = vertex->first, count = 0; edge; )
2946 edge = CV_NEXT_GRAPH_EDGE( edge, vertex );
2953 typedef struct CvGraphItem
2962 icvSeqElemsClearFlags( CvSeq* seq, int offset, int clear_mask )
2965 int i, total, elem_size;
2968 CV_Error( CV_StsNullPtr, "" );
2970 elem_size = seq->elem_size;
2973 if( (unsigned)offset > (unsigned)elem_size )
2974 CV_Error( CV_StsBadArg, "" );
2976 cvStartReadSeq( seq, &reader );
2978 for( i = 0; i < total; i++ )
2980 int* flag_ptr = (int*)(reader.ptr + offset);
2981 *flag_ptr &= ~clear_mask;
2983 CV_NEXT_SEQ_ELEM( elem_size, reader );
2989 icvSeqFindNextElem( CvSeq* seq, int offset, int mask,
2990 int value, int* start_index )
2992 schar* elem_ptr = 0;
2995 int total, elem_size, index;
2997 if( !seq || !start_index )
2998 CV_Error( CV_StsNullPtr, "" );
3000 elem_size = seq->elem_size;
3002 index = *start_index;
3004 if( (unsigned)offset > (unsigned)elem_size )
3005 CV_Error( CV_StsBadArg, "" );
3010 if( (unsigned)index >= (unsigned)total )
3013 index += index < 0 ? total : 0;
3016 cvStartReadSeq( seq, &reader );
3019 cvSetSeqReaderPos( &reader, index );
3021 for( index = 0; index < total; index++ )
3023 int* flag_ptr = (int*)(reader.ptr + offset);
3024 if( (*flag_ptr & mask) == value )
3027 CV_NEXT_SEQ_ELEM( elem_size, reader );
3032 elem_ptr = reader.ptr;
3033 *start_index = index;
3039 #define CV_FIELD_OFFSET( field, structtype ) ((int)(size_t)&((structtype*)0)->field)
3041 CV_IMPL CvGraphScanner*
3042 cvCreateGraphScanner( CvGraph* graph, CvGraphVtx* vtx, int mask )
3045 CV_Error( CV_StsNullPtr, "Null graph pointer" );
3047 CV_Assert( graph->storage != 0 );
3049 CvGraphScanner* scanner = (CvGraphScanner*)cvAlloc( sizeof(*scanner) );
3050 memset( scanner, 0, sizeof(*scanner));
3052 scanner->graph = graph;
3053 scanner->mask = mask;
3055 scanner->index = vtx == 0 ? 0 : -1;
3057 CvMemStorage* child_storage = cvCreateChildMemStorage( graph->storage );
3059 scanner->stack = cvCreateSeq( 0, sizeof(CvSet),
3060 sizeof(CvGraphItem), child_storage );
3062 icvSeqElemsClearFlags( (CvSeq*)graph,
3063 CV_FIELD_OFFSET( flags, CvGraphVtx),
3064 CV_GRAPH_ITEM_VISITED_FLAG|
3065 CV_GRAPH_SEARCH_TREE_NODE_FLAG );
3067 icvSeqElemsClearFlags( (CvSeq*)(graph->edges),
3068 CV_FIELD_OFFSET( flags, CvGraphEdge),
3069 CV_GRAPH_ITEM_VISITED_FLAG );
3076 cvReleaseGraphScanner( CvGraphScanner** scanner )
3079 CV_Error( CV_StsNullPtr, "Null double pointer to graph scanner" );
3083 if( (*scanner)->stack )
3084 cvReleaseMemStorage( &((*scanner)->stack->storage));
3091 cvNextGraphItem( CvGraphScanner* scanner )
3099 if( !scanner || !(scanner->stack))
3100 CV_Error( CV_StsNullPtr, "Null graph scanner" );
3104 edge = scanner->edge;
3110 if( dst && !CV_IS_GRAPH_VERTEX_VISITED(dst) )
3112 scanner->vtx = vtx = dst;
3114 dst->flags |= CV_GRAPH_ITEM_VISITED_FLAG;
3116 if((scanner->mask & CV_GRAPH_VERTEX))
3119 scanner->edge = vtx->first;
3121 code = CV_GRAPH_VERTEX;
3128 dst = edge->vtx[vtx == edge->vtx[0]];
3130 if( !CV_IS_GRAPH_EDGE_VISITED(edge) )
3132 // Check that the edge is outgoing:
3133 if( !CV_IS_GRAPH_ORIENTED( scanner->graph ) || dst != edge->vtx[0] )
3135 edge->flags |= CV_GRAPH_ITEM_VISITED_FLAG;
3137 if( !CV_IS_GRAPH_VERTEX_VISITED(dst) )
3142 vtx->flags |= CV_GRAPH_SEARCH_TREE_NODE_FLAG;
3144 cvSeqPush( scanner->stack, &item );
3146 if( scanner->mask & CV_GRAPH_TREE_EDGE )
3148 code = CV_GRAPH_TREE_EDGE;
3151 scanner->edge = edge;
3158 if( scanner->mask & (CV_GRAPH_BACK_EDGE|
3159 CV_GRAPH_CROSS_EDGE|
3160 CV_GRAPH_FORWARD_EDGE) )
3162 code = (dst->flags & CV_GRAPH_SEARCH_TREE_NODE_FLAG) ?
3163 CV_GRAPH_BACK_EDGE :
3164 (edge->flags & CV_GRAPH_FORWARD_EDGE_FLAG) ?
3165 CV_GRAPH_FORWARD_EDGE : CV_GRAPH_CROSS_EDGE;
3166 edge->flags &= ~CV_GRAPH_FORWARD_EDGE_FLAG;
3167 if( scanner->mask & code )
3171 scanner->edge = edge;
3177 else if( (dst->flags & (CV_GRAPH_ITEM_VISITED_FLAG|
3178 CV_GRAPH_SEARCH_TREE_NODE_FLAG)) ==
3179 (CV_GRAPH_ITEM_VISITED_FLAG|
3180 CV_GRAPH_SEARCH_TREE_NODE_FLAG))
3182 edge->flags |= CV_GRAPH_FORWARD_EDGE_FLAG;
3186 edge = CV_NEXT_GRAPH_EDGE( edge, vtx );
3189 if( !edge ) /* need to backtrack */
3191 if( scanner->stack->total == 0 )
3193 if( scanner->index >= 0 )
3199 cvSeqPop( scanner->stack, &item );
3201 vtx->flags &= ~CV_GRAPH_SEARCH_TREE_NODE_FLAG;
3205 if( scanner->mask & CV_GRAPH_BACKTRACKING )
3208 scanner->edge = edge;
3209 scanner->dst = edge->vtx[vtx == edge->vtx[0]];
3210 code = CV_GRAPH_BACKTRACKING;
3218 vtx = (CvGraphVtx*)icvSeqFindNextElem( (CvSeq*)(scanner->graph),
3219 CV_FIELD_OFFSET( flags, CvGraphVtx ), CV_GRAPH_ITEM_VISITED_FLAG|INT_MIN,
3220 0, &(scanner->index) );
3224 code = CV_GRAPH_OVER;
3230 if( scanner->mask & CV_GRAPH_NEW_TREE )
3235 code = CV_GRAPH_NEW_TREE;
3245 cvCloneGraph( const CvGraph* graph, CvMemStorage* storage )
3247 int* flag_buffer = 0;
3248 CvGraphVtx** ptr_buffer = 0;
3249 CvGraph* result = 0;
3252 int vtx_size, edge_size;
3255 if( !CV_IS_GRAPH(graph))
3256 CV_Error( CV_StsBadArg, "Invalid graph pointer" );
3259 storage = graph->storage;
3262 CV_Error( CV_StsNullPtr, "NULL storage pointer" );
3264 vtx_size = graph->elem_size;
3265 edge_size = graph->edges->elem_size;
3267 flag_buffer = (int*)cvAlloc( graph->total*sizeof(flag_buffer[0]));
3268 ptr_buffer = (CvGraphVtx**)cvAlloc( graph->total*sizeof(ptr_buffer[0]));
3269 result = cvCreateGraph( graph->flags, graph->header_size,
3270 vtx_size, edge_size, storage );
3271 memcpy( result + sizeof(CvGraph), graph + sizeof(CvGraph),
3272 graph->header_size - sizeof(CvGraph));
3274 // Pass 1. Save flags, copy vertices:
3275 cvStartReadSeq( (CvSeq*)graph, &reader );
3276 for( i = 0, k = 0; i < graph->total; i++ )
3278 if( CV_IS_SET_ELEM( reader.ptr ))
3280 CvGraphVtx* vtx = (CvGraphVtx*)reader.ptr;
3281 CvGraphVtx* dstvtx = 0;
3282 cvGraphAddVtx( result, vtx, &dstvtx );
3283 flag_buffer[k] = dstvtx->flags = vtx->flags;
3285 ptr_buffer[k++] = dstvtx;
3287 CV_NEXT_SEQ_ELEM( vtx_size, reader );
3290 // Pass 2. Copy edges:
3291 cvStartReadSeq( (CvSeq*)graph->edges, &reader );
3292 for( i = 0; i < graph->edges->total; i++ )
3294 if( CV_IS_SET_ELEM( reader.ptr ))
3296 CvGraphEdge* edge = (CvGraphEdge*)reader.ptr;
3297 CvGraphEdge* dstedge = 0;
3298 CvGraphVtx* new_org = ptr_buffer[edge->vtx[0]->flags];
3299 CvGraphVtx* new_dst = ptr_buffer[edge->vtx[1]->flags];
3300 cvGraphAddEdgeByPtr( result, new_org, new_dst, edge, &dstedge );
3301 dstedge->flags = edge->flags;
3303 CV_NEXT_SEQ_ELEM( edge_size, reader );
3306 // Pass 3. Restore flags:
3307 cvStartReadSeq( (CvSeq*)graph, &reader );
3308 for( i = 0, k = 0; i < graph->edges->total; i++ )
3310 if( CV_IS_SET_ELEM( reader.ptr ))
3312 CvGraphVtx* vtx = (CvGraphVtx*)reader.ptr;
3313 vtx->flags = flag_buffer[k++];
3315 CV_NEXT_SEQ_ELEM( vtx_size, reader );
3318 cvFree( &flag_buffer );
3319 cvFree( &ptr_buffer );
3321 if( cvGetErrStatus() < 0 )
3328 /****************************************************************************************\
3329 * Working with sequence tree *
3330 \****************************************************************************************/
3332 // Gather pointers to all the sequences, accessible from the <first>, to the single sequence.
3334 cvTreeToNodeSeq( const void* first, int header_size, CvMemStorage* storage )
3337 CvTreeNodeIterator iterator;
3340 CV_Error( CV_StsNullPtr, "NULL storage pointer" );
3342 allseq = cvCreateSeq( 0, header_size, sizeof(first), storage );
3346 cvInitTreeNodeIterator( &iterator, first, INT_MAX );
3350 void* node = cvNextTreeNode( &iterator );
3353 cvSeqPush( allseq, &node );
3363 typedef struct CvTreeNode
3365 int flags; /* micsellaneous flags */
3366 int header_size; /* size of sequence header */
3367 struct CvTreeNode* h_prev; /* previous sequence */
3368 struct CvTreeNode* h_next; /* next sequence */
3369 struct CvTreeNode* v_prev; /* 2nd previous sequence */
3370 struct CvTreeNode* v_next; /* 2nd next sequence */
3376 // Insert contour into tree given certain parent sequence.
3377 // If parent is equal to frame (the most external contour),
3378 // then added contour will have null pointer to parent:
3380 cvInsertNodeIntoTree( void* _node, void* _parent, void* _frame )
3382 CvTreeNode* node = (CvTreeNode*)_node;
3383 CvTreeNode* parent = (CvTreeNode*)_parent;
3385 if( !node || !parent )
3386 CV_Error( CV_StsNullPtr, "" );
3388 node->v_prev = _parent != _frame ? parent : 0;
3389 node->h_next = parent->v_next;
3391 assert( parent->v_next != node );
3393 if( parent->v_next )
3394 parent->v_next->h_prev = node;
3395 parent->v_next = node;
3399 // Remove contour from tree, together with the contour's children:
3401 cvRemoveNodeFromTree( void* _node, void* _frame )
3403 CvTreeNode* node = (CvTreeNode*)_node;
3404 CvTreeNode* frame = (CvTreeNode*)_frame;
3407 CV_Error( CV_StsNullPtr, "" );
3410 CV_Error( CV_StsBadArg, "frame node could not be deleted" );
3413 node->h_next->h_prev = node->h_prev;
3416 node->h_prev->h_next = node->h_next;
3419 CvTreeNode* parent = node->v_prev;
3425 assert( parent->v_next == node );
3426 parent->v_next = node->h_next;
3433 cvInitTreeNodeIterator( CvTreeNodeIterator* treeIterator,
3434 const void* first, int max_level )
3436 if( !treeIterator || !first )
3437 CV_Error( CV_StsNullPtr, "" );
3440 CV_Error( CV_StsOutOfRange, "" );
3442 treeIterator->node = (void*)first;
3443 treeIterator->level = 0;
3444 treeIterator->max_level = max_level;
3449 cvNextTreeNode( CvTreeNodeIterator* treeIterator )
3451 CvTreeNode* prevNode = 0;
3456 CV_Error( CV_StsNullPtr, "NULL iterator pointer" );
3458 prevNode = node = (CvTreeNode*)treeIterator->node;
3459 level = treeIterator->level;
3463 if( node->v_next && level+1 < treeIterator->max_level )
3465 node = node->v_next;
3470 while( node->h_next == 0 )
3472 node = node->v_prev;
3479 node = node && treeIterator->max_level != 0 ? node->h_next : 0;
3483 treeIterator->node = node;
3484 treeIterator->level = level;
3490 cvPrevTreeNode( CvTreeNodeIterator* treeIterator )
3492 CvTreeNode* prevNode = 0;
3497 CV_Error( CV_StsNullPtr, "" );
3499 prevNode = node = (CvTreeNode*)treeIterator->node;
3500 level = treeIterator->level;
3506 node = node->v_prev;
3512 node = node->h_prev;
3514 while( node->v_next && level < treeIterator->max_level )
3516 node = node->v_next;
3519 while( node->h_next )
3520 node = node->h_next;
3525 treeIterator->node = node;
3526 treeIterator->level = level;
3534 // This is reimplementation of kd-trees from cvkdtree*.* by Xavier Delacour, cleaned-up and
3535 // adopted to work with the new OpenCV data structures. It's in cxcore to be shared by
3536 // both cv (CvFeatureTree) and ml (kNN).
3538 // The algorithm is taken from:
3539 // J.S. Beis and D.G. Lowe. Shape indexing using approximate nearest-neighbor search
3540 // in highdimensional spaces. In Proc. IEEE Conf. Comp. Vision Patt. Recog.,
3541 // pages 1000--1006, 1997. http://citeseer.ist.psu.edu/beis97shape.html
3543 const int MAX_TREE_DEPTH = 32;
3551 KDTree::KDTree(const Mat& _points, bool _copyData)
3555 build(_points, _copyData);
3558 KDTree::KDTree(const Mat& _points, const Mat& _labels, bool _copyData)
3562 build(_points, _labels, _copyData);
3567 SubTree() : first(0), last(0), nodeIdx(0), depth(0) {}
3568 SubTree(int _first, int _last, int _nodeIdx, int _depth)
3569 : first(_first), last(_last), nodeIdx(_nodeIdx), depth(_depth) {}
3578 medianPartition( size_t* ofs, int a, int b, const float* vals )
3580 int k, a0 = a, b0 = b;
3581 int middle = (a + b)/2;
3584 int i0 = a, i1 = (a+b)/2, i2 = b;
3585 float v0 = vals[ofs[i0]], v1 = vals[ofs[i1]], v2 = vals[ofs[i2]];
3586 int ip = v0 < v1 ? (v1 < v2 ? i1 : v0 < v2 ? i2 : i0) :
3587 v0 < v2 ? i0 : (v1 < v2 ? i2 : i1);
3588 float pivot = vals[ofs[ip]];
3589 std::swap(ofs[ip], ofs[i2]);
3591 for( i1 = i0, i0--; i1 <= i2; i1++ )
3592 if( vals[ofs[i1]] <= pivot )
3595 std::swap(ofs[i0], ofs[i1]);
3605 float pivot = vals[ofs[middle]];
3606 int less = 0, more = 0;
3607 for( k = a0; k < middle; k++ )
3609 CV_Assert(vals[ofs[k]] <= pivot);
3610 less += vals[ofs[k]] < pivot;
3612 for( k = b0; k > middle; k-- )
3614 CV_Assert(vals[ofs[k]] >= pivot);
3615 more += vals[ofs[k]] > pivot;
3617 CV_Assert(std::abs(more - less) <= 1);
3619 return vals[ofs[middle]];
3623 computeSums( const Mat& points, const size_t* ofs, int a, int b, double* sums )
3625 int i, j, dims = points.cols;
3626 const float* data = points.ptr<float>(0);
3627 for( j = 0; j < dims; j++ )
3628 sums[j*2] = sums[j*2+1] = 0;
3629 for( i = a; i <= b; i++ )
3631 const float* row = data + ofs[i];
3632 for( j = 0; j < dims; j++ )
3634 double t = row[j], s = sums[j*2] + t, s2 = sums[j*2+1] + t*t;
3635 sums[j*2] = s; sums[j*2+1] = s2;
3641 void KDTree::build(const Mat& _points, bool _copyData)
3643 build(_points, Mat(), _copyData);
3647 void KDTree::build(const Mat& _points, const Mat& _labels, bool _copyData)
3649 CV_Assert(_points.type() == CV_32F && !_points.empty());
3650 vector<KDTree::Node>().swap(nodes);
3657 points.create(_points.size(), _points.type());
3660 int i, j, n = _points.rows, dims = _points.cols, top = 0;
3661 const float* data = _points.ptr<float>(0);
3662 float* dstdata = points.ptr<float>(0);
3663 size_t step = _points.step1();
3664 size_t dstep = points.step1();
3667 const int* _labels_data = 0;
3669 if( !_labels.empty() )
3671 int nlabels = _labels.checkVector(1, CV_32S, true);
3672 CV_Assert(nlabels == n);
3673 _labels_data = (const int*)_labels.data;
3676 Mat sumstack(MAX_TREE_DEPTH*2, dims*2, CV_64F);
3677 SubTree stack[MAX_TREE_DEPTH*2];
3679 vector<size_t> _ptofs(n);
3680 size_t* ptofs = &_ptofs[0];
3682 for( i = 0; i < n; i++ )
3685 nodes.push_back(Node());
3686 computeSums(points, ptofs, 0, n-1, sumstack.ptr<double>(top));
3687 stack[top++] = SubTree(0, n-1, 0, 0);
3692 int first = stack[top].first, last = stack[top].last;
3693 int depth = stack[top].depth, nidx = stack[top].nodeIdx;
3694 int count = last - first + 1, dim = -1;
3695 const double* sums = sumstack.ptr<double>(top);
3696 double invCount = 1./count, maxVar = -1.;
3700 int idx0 = (int)(ptofs[first]/step);
3701 int idx = _copyData ? ptpos++ : idx0;
3702 nodes[nidx].idx = ~idx;
3705 const float* src = data + ptofs[first];
3706 float* dst = dstdata + idx*dstep;
3707 for( j = 0; j < dims; j++ )
3710 labels[idx] = _labels_data ? _labels_data[idx0] : idx0;
3711 _maxDepth = std::max(_maxDepth, depth);
3715 // find the dimensionality with the biggest variance
3716 for( j = 0; j < dims; j++ )
3718 double m = sums[j*2]*invCount;
3719 double varj = sums[j*2+1]*invCount - m*m;
3727 int left = (int)nodes.size(), right = left + 1;
3728 nodes.push_back(Node());
3729 nodes.push_back(Node());
3730 nodes[nidx].idx = dim;
3731 nodes[nidx].left = left;
3732 nodes[nidx].right = right;
3733 nodes[nidx].boundary = medianPartition(ptofs, first, last, data + dim);
3735 int middle = (first + last)/2;
3736 double *lsums = (double*)sums, *rsums = lsums + dims*2;
3737 computeSums(points, ptofs, middle+1, last, rsums);
3738 for( j = 0; j < dims*2; j++ )
3739 lsums[j] = sums[j] - rsums[j];
3740 stack[top++] = SubTree(first, middle, left, depth+1);
3741 stack[top++] = SubTree(middle+1, last, right, depth+1);
3743 maxDepth = _maxDepth;
3747 int KDTree::findNearest(const float* vec, int K, int emax,
3748 vector<int>* neighborsIdx,
3750 vector<float>* dist,
3751 vector<int>* labels) const
3753 K = std::min(K, points.rows);
3756 neighborsIdx->resize(K);
3761 K = findNearest(vec, K, emax, neighborsIdx ? &(*neighborsIdx)[0] : 0,
3762 neighbors, dist ? &(*dist)[0] : 0, labels ? &(*labels)[0] : 0);
3764 neighborsIdx->resize(K);
3772 int KDTree::findNearest(const vector<float>& vec, int K, int emax,
3773 vector<int>* neighborsIdx,
3775 vector<float>* dist,
3776 vector<int>* labels) const
3778 CV_Assert((int)vec.size() == points.cols);
3779 K = std::min(K, points.rows);
3782 neighborsIdx->resize(K);
3787 K = findNearest(&vec[0], K, emax, neighborsIdx ? &(*neighborsIdx)[0] : 0,
3788 neighbors, dist ? &(*dist)[0] : 0, labels ? &(*labels)[0] : 0);
3790 neighborsIdx->resize(K);
3800 PQueueElem() : dist(0), idx(0) {}
3801 PQueueElem(float _dist, int _idx) : dist(_dist), idx(_idx) {}
3807 int KDTree::findNearest(const float* vec, int K, int emax,
3808 int* _neighborsIdx, Mat* _neighbors,
3809 float* _dist, int* _labels) const
3812 K = std::min(K, points.rows);
3813 int dims = points.cols;
3815 CV_Assert(K > 0 && (normType == NORM_L2 || normType == NORM_L1));
3817 AutoBuffer<uchar> _buf((K+1)*(sizeof(float) + sizeof(int)));
3818 int* idx = (int*)(uchar*)_buf;
3819 float* dist = (float*)(idx + K + 1);
3820 int i, j, ncount = 0, e = 0;
3822 int qsize = 0, maxqsize = 1 << 10;
3823 AutoBuffer<uchar> _pqueue(maxqsize*sizeof(PQueueElem));
3824 PQueueElem* pqueue = (PQueueElem*)(uchar*)_pqueue;
3825 emax = std::max(emax, 1);
3827 for( e = 0; e < emax; )
3829 float d, alt_d = 0.f;
3836 // take the next node from the priority queue
3839 nidx = pqueue[0].idx;
3840 alt_d = pqueue[0].dist;
3843 std::swap(pqueue[0], pqueue[qsize]);
3847 int left = i*2 + 1, right = i*2 + 2;
3850 if( right < qsize && pqueue[right].dist < pqueue[left].dist )
3852 if( pqueue[left].dist >= d )
3854 std::swap(pqueue[i], pqueue[left]);
3859 if( ncount == K && alt_d > dist[ncount-1] )
3867 const Node& n = nodes[nidx];
3872 const float* row = points.ptr<float>(i);
3873 if( normType == NORM_L2 )
3874 for( j = 0, d = 0.f; j < dims; j++ )
3876 float t = vec[j] - row[j];
3880 for( j = 0, d = 0.f; j < dims; j++ )
3881 d += std::abs(vec[j] - row[j]);
3885 for( i = ncount-1; i >= 0; i-- )
3889 std::swap(dist[i], dist[i+1]);
3890 std::swap(idx[i], idx[i+1]);
3892 ncount += ncount < K;
3898 if( vec[n.idx] <= n.boundary )
3909 d = vec[n.idx] - n.boundary;
3910 if( normType == NORM_L2 )
3913 d = std::abs(d) + alt_d;
3915 if( ncount == K && d > dist[ncount-1] )
3917 // add alternative subtree to the priority queue
3918 pqueue[qsize] = PQueueElem(d, alt);
3919 for( i = qsize; i > 0; )
3921 int parent = (i-1)/2;
3922 if( parent < 0 || pqueue[parent].dist <= d )
3924 std::swap(pqueue[i], pqueue[parent]);
3927 qsize += qsize+1 < maxqsize;
3931 K = std::min(K, ncount);
3934 for( i = 0; i < K; i++ )
3935 _neighborsIdx[i] = idx[i];
3939 for( i = 0; i < K; i++ )
3940 _dist[i] = std::sqrt(dist[i]);
3944 for( i = 0; i < K; i++ )
3945 _labels[i] = labels[idx[i]];
3949 getPoints(idx, K, *_neighbors);
3954 void KDTree::findOrthoRange(const float* L, const float* R,
3955 vector<int>* neighborsIdx,
3956 Mat* neighbors, vector<int>* _labels) const
3958 int dims = points.cols;
3960 CV_Assert( L && R );
3962 vector<int> _idx, *idx = neighborsIdx ? neighborsIdx : &_idx;
3963 AutoBuffer<int> _stack(MAX_TREE_DEPTH*2 + 1);
3964 int* stack = _stack;
3972 int nidx = stack[top];
3975 const Node& n = nodes[nidx];
3979 const float* row = points.ptr<float>(i);
3980 for( j = 0; j < dims; j++ )
3981 if( row[j] < L[j] || row[j] >= R[j] )
3987 if( L[n.idx] <= n.boundary )
3988 stack[top++] = n.left;
3989 if( R[n.idx] > n.boundary )
3990 stack[top++] = n.right;
3994 getPoints( &(*idx)[0], idx->size(), *neighbors, _labels );
3998 void KDTree::findOrthoRange(const vector<float>& L, const vector<float>& R,
3999 vector<int>* neighborsIdx, Mat* neighbors, vector<int>* _labels) const
4001 size_t dims = points.cols;
4002 CV_Assert(L.size() == dims && R.size() == dims);
4003 findOrthoRange(&L[0], &R[0], neighborsIdx, neighbors, _labels);
4007 void KDTree::getPoints(const int* idx, size_t nidx, Mat& pts, vector<int>* _labels) const
4009 int dims = points.cols, n = (int)nidx;
4010 pts.create( n, dims, points.type());
4012 _labels->resize(nidx);
4014 for( int i = 0; i < n; i++ )
4017 CV_Assert( (unsigned)k < (unsigned)points.rows );
4018 const float* src = points.ptr<float>(k);
4019 std::copy(src, src + dims, pts.ptr<float>(i));
4021 (*_labels)[i] = labels[k];
4026 void KDTree::getPoints(const vector<int>& idx, Mat& pts, vector<int>* _labels) const
4028 int dims = points.cols;
4029 int i, nidx = (int)idx.size();
4030 pts.create( nidx, dims, points.type());
4033 _labels->resize(nidx);
4035 for( i = 0; i < nidx; i++ )
4038 CV_Assert( (unsigned)k < (unsigned)points.rows );
4039 const float* src = points.ptr<float>(k);
4040 std::copy(src, src + dims, pts.ptr<float>(i));
4041 if(_labels) (*_labels)[i] = labels[k];
4046 const float* KDTree::getPoint(int ptidx, int* label) const
4048 CV_Assert( (unsigned)ptidx < (unsigned)points.rows);
4050 *label = label[ptidx];
4051 return points.ptr<float>(ptidx);
4055 int KDTree::dims() const
4057 return !points.empty() ? points.cols : 0;
4060 ////////////////////////////////////////////////////////////////////////////////
4062 schar* seqPush( CvSeq* seq, const void* element )
4064 return cvSeqPush(seq, element);
4067 schar* seqPushFront( CvSeq* seq, const void* element )
4069 return cvSeqPushFront(seq, element);
4072 void seqPop( CvSeq* seq, void* element )
4074 cvSeqPop(seq, element);
4077 void seqPopFront( CvSeq* seq, void* element )
4079 cvSeqPopFront(seq, element);
4082 void seqRemove( CvSeq* seq, int index )
4084 cvSeqRemove(seq, index);
4087 void clearSeq( CvSeq* seq )
4092 schar* getSeqElem( const CvSeq* seq, int index )
4094 return cvGetSeqElem(seq, index);
4097 void seqRemoveSlice( CvSeq* seq, CvSlice slice )
4099 return cvSeqRemoveSlice(seq, slice);
4102 void seqInsertSlice( CvSeq* seq, int before_index, const CvArr* from_arr )
4104 cvSeqInsertSlice(seq, before_index, from_arr);