imagecore 0.0.1

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