Point Cloud Library (PCL)  1.8.0
opennurbs_qsort_template.h
1 // NOTE: 14 April 2011 Dale Lear:
2 // Replace this code with Mikko's "quacksort", once "quacksort" is fully debugged
3 // This code is based ont the VC 2010 crt qsort.c file and must not be released
4 // with public opennurbs.
5 
6 #if !defined(ON_COMPILING_OPENNURBS_QSORT_FUNCTIONS)
7 /*
8 See opennurbs_sort.cpp for examples of using openurbs_qsort_template.c
9 to define type specific quick sort functions.
10 */
11 #error Do not compile openurbs_qsort_template.c directly.
12 #endif
13 
14 #define ON_QSORT_CUTOFF 8 /* testing shows that this is good value */
15 
16 /* Note: the theoretical number of stack entries required is
17  no more than 1 + log2(num). But we switch to insertion
18  sort for CUTOFF elements or less, so we really only need
19  1 + log2(num) - log2(CUTOFF) stack entries. For a CUTOFF
20  of 8, that means we need no more than 30 stack entries for
21  32 bit platforms, and 62 for 64-bit platforms. */
22 #define ON_QSORT_STKSIZ (8*sizeof(void*) - 2)
23 
24 
25 // ON_SORT_TEMPLATE_TYPE -> double, int, ....
26 #if !defined(ON_SORT_TEMPLATE_TYPE)
27 #error Define ON_SORT_TEMPLATE_TYPE macro before including opennurbs_qsort_template.c
28 #endif
29 
30 #if !defined(ON_QSORT_FNAME)
31 #error Define ON_QSORT_FNAME macro before including opennurbs_qsort_template.c
32 #endif
33 
34 #if defined(ON_SORT_TEMPLATE_COMPARE)
35 // use a compare function like strcmp for char* strings
36 #define ON_QSORT_GT(A,B) ON_SORT_TEMPLATE_COMPARE(A,B) > 0
37 #define ON_QSORT_LE(A,B) ON_SORT_TEMPLATE_COMPARE(A,B) <= 0
38 #define ON_QSORT_EQ(A,B) ON_SORT_TEMPLATE_COMPARE(A,B) == 0
39 #else
40 // use type compares
41 #define ON_QSORT_GT(A,B) *A > *B
42 #define ON_QSORT_LE(A,B) *A <= *B
43 #define ON_QSORT_EQ(A,B) *A == *B
44 #endif
45 
46 #if defined(ON_SORT_TEMPLATE_USE_MEMCPY)
47 #define ON_QSORT_SWAP(A,B) memcpy(&tmp,A,sizeof(tmp));memcpy(A,B,sizeof(tmp));memcpy(B,&tmp,sizeof(tmp))
48 #else
49 #define ON_QSORT_SWAP(A,B) tmp = *A; *A = *B; *B = tmp
50 #endif
51 
52 static void ON_shortsort(ON_SORT_TEMPLATE_TYPE *, ON_SORT_TEMPLATE_TYPE *);
53 static void ON_shortsort(ON_SORT_TEMPLATE_TYPE *lo, ON_SORT_TEMPLATE_TYPE *hi)
54 {
55  ON_SORT_TEMPLATE_TYPE *p;
56  ON_SORT_TEMPLATE_TYPE *max;
57  ON_SORT_TEMPLATE_TYPE tmp;
58 
59  /* Note: in assertions below, i and j are alway inside original bound of
60  array to sort. */
61 
62  while (hi > lo)
63  {
64  /* A[i] <= A[j] for i <= j, j > hi */
65  max = lo;
66  for (p = lo+1; p <= hi; p++)
67  {
68  /* A[i] <= A[max] for lo <= i < p */
69  if ( ON_QSORT_GT(p,max) )
70  {
71  max = p;
72  }
73  /* A[i] <= A[max] for lo <= i <= p */
74  }
75 
76  /* A[i] <= A[max] for lo <= i <= hi */
77 
78  ON_QSORT_SWAP(max,hi);
79 
80  /* A[i] <= A[hi] for i <= hi, so A[i] <= A[j] for i <= j, j >= hi */
81 
82  hi--;
83 
84  /* A[i] <= A[j] for i <= j, j > hi, loop top condition established */
85  }
86  /* A[i] <= A[j] for i <= j, j > lo, which implies A[i] <= A[j] for i < j,
87  so array is sorted */
88 }
89 
90 /* this parameter defines the cutoff between using quick sort and
91  insertion sort for arrays; arrays with lengths shorter or equal to the
92  below value use insertion sort */
93 
94 #if defined(ON_SORT_TEMPLATE_STATIC_FUNCTION)
95 static
96 #endif
97 void
98 ON_QSORT_FNAME (
99  ON_SORT_TEMPLATE_TYPE *base,
100  size_t num
101  )
102 {
103  ON_SORT_TEMPLATE_TYPE *lo; /* start of sub-array currently sorting */
104  ON_SORT_TEMPLATE_TYPE *hi; /* end of sub-array currently sorting */
105  ON_SORT_TEMPLATE_TYPE *mid; /* points to middle of subarray */
106  ON_SORT_TEMPLATE_TYPE *loguy; /* traveling pointers for partition step */
107  ON_SORT_TEMPLATE_TYPE *higuy; /* traveling pointers for partition step */
108  ON_SORT_TEMPLATE_TYPE *lostk[ON_QSORT_STKSIZ];
109  ON_SORT_TEMPLATE_TYPE *histk[ON_QSORT_STKSIZ];
110  size_t size; /* size of the sub-array */
111  int stkptr; /* stack for saving sub-array to be processed */
112  ON_SORT_TEMPLATE_TYPE tmp;
113 
114  if ( 0 == base || num < 2 )
115  return;
116 
117  stkptr = 0; /* initialize stack */
118 
119  lo = base;
120  hi = base + (num-1); /* initialize limits */
121 
122  /* this entry point is for pseudo-recursion calling: setting
123  lo and hi and jumping to here is like recursion, but stkptr is
124  preserved, locals aren't, so we preserve stuff on the stack */
125 recurse:
126 
127  size = (hi - lo) + 1; /* number of el's to sort */
128 
129  /* below a certain size, it is faster to use a O(n^2) sorting method */
130  if (size <= ON_QSORT_CUTOFF)
131  {
132  ON_shortsort(lo, hi);
133  }
134  else {
135  /* First we pick a partitioning element. The efficiency of the
136  algorithm demands that we find one that is approximately the median
137  of the values, but also that we select one fast. We choose the
138  median of the first, middle, and last elements, to avoid bad
139  performance in the face of already sorted data, or data that is made
140  up of multiple sorted runs appended together. Testing shows that a
141  median-of-three algorithm provides better performance than simply
142  picking the middle element for the latter case. */
143 
144  mid = lo + (size / 2); /* find middle element */
145 
146  /* Sort the first, middle, last elements into order */
147  if ( ON_QSORT_GT(lo,mid) ) {ON_QSORT_SWAP(lo,mid);}
148  if ( ON_QSORT_GT(lo,hi) ) {ON_QSORT_SWAP(lo,hi);}
149  if ( ON_QSORT_GT(mid,hi) ) {ON_QSORT_SWAP(mid,hi);}
150 
151  /* We now wish to partition the array into three pieces, one consisting
152  of elements <= partition element, one of elements equal to the
153  partition element, and one of elements > than it. This is done
154  below; comments indicate conditions established at every step. */
155 
156  loguy = lo;
157  higuy = hi;
158 
159  /* Note that higuy decreases and loguy increases on every iteration,
160  so loop must terminate. */
161  for (;;)
162  {
163  /* lo <= loguy < hi, lo < higuy <= hi,
164  A[i] <= A[mid] for lo <= i <= loguy,
165  A[i] > A[mid] for higuy <= i < hi,
166  A[hi] >= A[mid] */
167 
168  /* The doubled loop is to avoid calling comp(mid,mid), since some
169  existing comparison funcs don't work when passed the same
170  value for both pointers. */
171 
172  if (mid > loguy)
173  {
174  do {
175  loguy++;
176  } while (loguy < mid && ON_QSORT_LE(loguy,mid));
177  }
178  if (mid <= loguy)
179  {
180  do {
181  loguy++;
182  } while (loguy <= hi && ON_QSORT_LE(loguy,mid));
183  }
184 
185  /* lo < loguy <= hi+1, A[i] <= A[mid] for lo <= i < loguy,
186  either loguy > hi or A[loguy] > A[mid] */
187 
188  do {
189  higuy--;
190  } while (higuy > mid && ON_QSORT_GT(higuy,mid));
191 
192  /* lo <= higuy < hi, A[i] > A[mid] for higuy < i < hi,
193  either higuy == lo or A[higuy] <= A[mid] */
194 
195  if (higuy < loguy)
196  break;
197 
198  /* if loguy > hi or higuy == lo, then we would have exited, so
199  A[loguy] > A[mid], A[higuy] <= A[mid],
200  loguy <= hi, higuy > lo */
201 
202  ON_QSORT_SWAP(loguy,higuy);
203 
204  /* If the partition element was moved, follow it. Only need
205  to check for mid == higuy, since before the swap,
206  A[loguy] > A[mid] implies loguy != mid. */
207 
208  if (mid == higuy)
209  mid = loguy;
210 
211  /* A[loguy] <= A[mid], A[higuy] > A[mid]; so condition at top
212  of loop is re-established */
213  }
214 
215  /* A[i] <= A[mid] for lo <= i < loguy,
216  A[i] > A[mid] for higuy < i < hi,
217  A[hi] >= A[mid]
218  higuy < loguy
219  implying:
220  higuy == loguy-1
221  or higuy == hi - 1, loguy == hi + 1, A[hi] == A[mid] */
222 
223  /* Find adjacent elements equal to the partition element. The
224  doubled loop is to avoid calling comp(mid,mid), since some
225  existing comparison funcs don't work when passed the same value
226  for both pointers. */
227 
228  higuy++;
229  if (mid < higuy) {
230  do {
231  higuy--;
232  } while (higuy > mid && ON_QSORT_EQ(higuy,mid));
233  }
234  if (mid >= higuy) {
235  do {
236  higuy--;
237  } while (higuy > lo && ON_QSORT_EQ(higuy,mid));
238  }
239 
240  /* OK, now we have the following:
241  higuy < loguy
242  lo <= higuy <= hi
243  A[i] <= A[mid] for lo <= i <= higuy
244  A[i] == A[mid] for higuy < i < loguy
245  A[i] > A[mid] for loguy <= i < hi
246  A[hi] >= A[mid] */
247 
248  /* We've finished the partition, now we want to sort the subarrays
249  [lo, higuy] and [loguy, hi].
250  We do the smaller one first to minimize stack usage.
251  We only sort arrays of length 2 or more.*/
252 
253  if ( higuy - lo >= hi - loguy ) {
254  if (lo < higuy) {
255  lostk[stkptr] = lo;
256  histk[stkptr] = higuy;
257  ++stkptr;
258  } /* save big recursion for later */
259 
260  if (loguy < hi) {
261  lo = loguy;
262  goto recurse; /* do small recursion */
263  }
264  }
265  else {
266  if (loguy < hi) {
267  lostk[stkptr] = loguy;
268  histk[stkptr] = hi;
269  ++stkptr; /* save big recursion for later */
270  }
271 
272  if (lo < higuy) {
273  hi = higuy;
274  goto recurse; /* do small recursion */
275  }
276  }
277  }
278 
279  /* We have sorted the array, except for any pending sorts on the stack.
280  Check if there are any, and do them. */
281 
282  --stkptr;
283  if (stkptr >= 0) {
284  lo = lostk[stkptr];
285  hi = histk[stkptr];
286  goto recurse; /* pop subarray from stack */
287  }
288  else
289  return; /* all subarrays done */
290 }
291 
292 #undef ON_QSORT_GT
293 #undef ON_QSORT_LE
294 #undef ON_QSORT_EQ
295 #undef ON_QSORT_SWAP
296 #undef ON_QSORT_CUTOFF
297 #undef ON_QSORT_STKSIZ