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