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