Point Cloud Library (PCL)
1.7.0
|
00001 // NOTE: 14 April 2011 Dale Lear: 00002 // Replace this code with Mikko's "quacksort", once "quacksort" is fully debugged 00003 // This code is based ont the VC 2010 crt qsort.c file and must not be released 00004 // with public opennurbs. 00005 00006 #if !defined(ON_COMPILING_OPENNURBS_QSORT_FUNCTIONS) 00007 /* 00008 See opennurbs_sort.cpp for examples of using openurbs_qsort_template.c 00009 to define type specific quick sort functions. 00010 */ 00011 #error Do not compile openurbs_qsort_template.c directly. 00012 #endif 00013 00014 #define ON_QSORT_CUTOFF 8 /* testing shows that this is good value */ 00015 00016 /* Note: the theoretical number of stack entries required is 00017 no more than 1 + log2(num). But we switch to insertion 00018 sort for CUTOFF elements or less, so we really only need 00019 1 + log2(num) - log2(CUTOFF) stack entries. For a CUTOFF 00020 of 8, that means we need no more than 30 stack entries for 00021 32 bit platforms, and 62 for 64-bit platforms. */ 00022 #define ON_QSORT_STKSIZ (8*sizeof(void*) - 2) 00023 00024 00025 // ON_SORT_TEMPLATE_TYPE -> double, int, .... 00026 #if !defined(ON_SORT_TEMPLATE_TYPE) 00027 #error Define ON_SORT_TEMPLATE_TYPE macro before including opennurbs_qsort_template.c 00028 #endif 00029 00030 #if !defined(ON_QSORT_FNAME) 00031 #error Define ON_QSORT_FNAME macro before including opennurbs_qsort_template.c 00032 #endif 00033 00034 #if defined(ON_SORT_TEMPLATE_COMPARE) 00035 // use a compare function like strcmp for char* strings 00036 #define ON_QSORT_GT(A,B) ON_SORT_TEMPLATE_COMPARE(A,B) > 0 00037 #define ON_QSORT_LE(A,B) ON_SORT_TEMPLATE_COMPARE(A,B) <= 0 00038 #define ON_QSORT_EQ(A,B) ON_SORT_TEMPLATE_COMPARE(A,B) == 0 00039 #else 00040 // use type compares 00041 #define ON_QSORT_GT(A,B) *A > *B 00042 #define ON_QSORT_LE(A,B) *A <= *B 00043 #define ON_QSORT_EQ(A,B) *A == *B 00044 #endif 00045 00046 #if defined(ON_SORT_TEMPLATE_USE_MEMCPY) 00047 #define ON_QSORT_SWAP(A,B) memcpy(&tmp,A,sizeof(tmp));memcpy(A,B,sizeof(tmp));memcpy(B,&tmp,sizeof(tmp)) 00048 #else 00049 #define ON_QSORT_SWAP(A,B) tmp = *A; *A = *B; *B = tmp 00050 #endif 00051 00052 static void ON_shortsort(ON_SORT_TEMPLATE_TYPE *, ON_SORT_TEMPLATE_TYPE *); 00053 static void ON_shortsort(ON_SORT_TEMPLATE_TYPE *lo, ON_SORT_TEMPLATE_TYPE *hi) 00054 { 00055 ON_SORT_TEMPLATE_TYPE *p; 00056 ON_SORT_TEMPLATE_TYPE *max; 00057 ON_SORT_TEMPLATE_TYPE tmp; 00058 00059 /* Note: in assertions below, i and j are alway inside original bound of 00060 array to sort. */ 00061 00062 while (hi > lo) 00063 { 00064 /* A[i] <= A[j] for i <= j, j > hi */ 00065 max = lo; 00066 for (p = lo+1; p <= hi; p++) 00067 { 00068 /* A[i] <= A[max] for lo <= i < p */ 00069 if ( ON_QSORT_GT(p,max) ) 00070 { 00071 max = p; 00072 } 00073 /* A[i] <= A[max] for lo <= i <= p */ 00074 } 00075 00076 /* A[i] <= A[max] for lo <= i <= hi */ 00077 00078 ON_QSORT_SWAP(max,hi); 00079 00080 /* A[i] <= A[hi] for i <= hi, so A[i] <= A[j] for i <= j, j >= hi */ 00081 00082 hi--; 00083 00084 /* A[i] <= A[j] for i <= j, j > hi, loop top condition established */ 00085 } 00086 /* A[i] <= A[j] for i <= j, j > lo, which implies A[i] <= A[j] for i < j, 00087 so array is sorted */ 00088 } 00089 00090 /* this parameter defines the cutoff between using quick sort and 00091 insertion sort for arrays; arrays with lengths shorter or equal to the 00092 below value use insertion sort */ 00093 00094 #if defined(ON_SORT_TEMPLATE_STATIC_FUNCTION) 00095 static 00096 #endif 00097 void 00098 ON_QSORT_FNAME ( 00099 ON_SORT_TEMPLATE_TYPE *base, 00100 size_t num 00101 ) 00102 { 00103 ON_SORT_TEMPLATE_TYPE *lo; /* start of sub-array currently sorting */ 00104 ON_SORT_TEMPLATE_TYPE *hi; /* end of sub-array currently sorting */ 00105 ON_SORT_TEMPLATE_TYPE *mid; /* points to middle of subarray */ 00106 ON_SORT_TEMPLATE_TYPE *loguy; /* traveling pointers for partition step */ 00107 ON_SORT_TEMPLATE_TYPE *higuy; /* traveling pointers for partition step */ 00108 ON_SORT_TEMPLATE_TYPE *lostk[ON_QSORT_STKSIZ]; 00109 ON_SORT_TEMPLATE_TYPE *histk[ON_QSORT_STKSIZ]; 00110 size_t size; /* size of the sub-array */ 00111 int stkptr; /* stack for saving sub-array to be processed */ 00112 ON_SORT_TEMPLATE_TYPE tmp; 00113 00114 if ( 0 == base || num < 2 ) 00115 return; 00116 00117 stkptr = 0; /* initialize stack */ 00118 00119 lo = base; 00120 hi = base + (num-1); /* initialize limits */ 00121 00122 /* this entry point is for pseudo-recursion calling: setting 00123 lo and hi and jumping to here is like recursion, but stkptr is 00124 preserved, locals aren't, so we preserve stuff on the stack */ 00125 recurse: 00126 00127 size = (hi - lo) + 1; /* number of el's to sort */ 00128 00129 /* below a certain size, it is faster to use a O(n^2) sorting method */ 00130 if (size <= ON_QSORT_CUTOFF) 00131 { 00132 ON_shortsort(lo, hi); 00133 } 00134 else { 00135 /* First we pick a partitioning element. The efficiency of the 00136 algorithm demands that we find one that is approximately the median 00137 of the values, but also that we select one fast. We choose the 00138 median of the first, middle, and last elements, to avoid bad 00139 performance in the face of already sorted data, or data that is made 00140 up of multiple sorted runs appended together. Testing shows that a 00141 median-of-three algorithm provides better performance than simply 00142 picking the middle element for the latter case. */ 00143 00144 mid = lo + (size / 2); /* find middle element */ 00145 00146 /* Sort the first, middle, last elements into order */ 00147 if ( ON_QSORT_GT(lo,mid) ) {ON_QSORT_SWAP(lo,mid);} 00148 if ( ON_QSORT_GT(lo,hi) ) {ON_QSORT_SWAP(lo,hi);} 00149 if ( ON_QSORT_GT(mid,hi) ) {ON_QSORT_SWAP(mid,hi);} 00150 00151 /* We now wish to partition the array into three pieces, one consisting 00152 of elements <= partition element, one of elements equal to the 00153 partition element, and one of elements > than it. This is done 00154 below; comments indicate conditions established at every step. */ 00155 00156 loguy = lo; 00157 higuy = hi; 00158 00159 /* Note that higuy decreases and loguy increases on every iteration, 00160 so loop must terminate. */ 00161 for (;;) 00162 { 00163 /* lo <= loguy < hi, lo < higuy <= hi, 00164 A[i] <= A[mid] for lo <= i <= loguy, 00165 A[i] > A[mid] for higuy <= i < hi, 00166 A[hi] >= A[mid] */ 00167 00168 /* The doubled loop is to avoid calling comp(mid,mid), since some 00169 existing comparison funcs don't work when passed the same 00170 value for both pointers. */ 00171 00172 if (mid > loguy) 00173 { 00174 do { 00175 loguy++; 00176 } while (loguy < mid && ON_QSORT_LE(loguy,mid)); 00177 } 00178 if (mid <= loguy) 00179 { 00180 do { 00181 loguy++; 00182 } while (loguy <= hi && ON_QSORT_LE(loguy,mid)); 00183 } 00184 00185 /* lo < loguy <= hi+1, A[i] <= A[mid] for lo <= i < loguy, 00186 either loguy > hi or A[loguy] > A[mid] */ 00187 00188 do { 00189 higuy--; 00190 } while (higuy > mid && ON_QSORT_GT(higuy,mid)); 00191 00192 /* lo <= higuy < hi, A[i] > A[mid] for higuy < i < hi, 00193 either higuy == lo or A[higuy] <= A[mid] */ 00194 00195 if (higuy < loguy) 00196 break; 00197 00198 /* if loguy > hi or higuy == lo, then we would have exited, so 00199 A[loguy] > A[mid], A[higuy] <= A[mid], 00200 loguy <= hi, higuy > lo */ 00201 00202 ON_QSORT_SWAP(loguy,higuy); 00203 00204 /* If the partition element was moved, follow it. Only need 00205 to check for mid == higuy, since before the swap, 00206 A[loguy] > A[mid] implies loguy != mid. */ 00207 00208 if (mid == higuy) 00209 mid = loguy; 00210 00211 /* A[loguy] <= A[mid], A[higuy] > A[mid]; so condition at top 00212 of loop is re-established */ 00213 } 00214 00215 /* A[i] <= A[mid] for lo <= i < loguy, 00216 A[i] > A[mid] for higuy < i < hi, 00217 A[hi] >= A[mid] 00218 higuy < loguy 00219 implying: 00220 higuy == loguy-1 00221 or higuy == hi - 1, loguy == hi + 1, A[hi] == A[mid] */ 00222 00223 /* Find adjacent elements equal to the partition element. The 00224 doubled loop is to avoid calling comp(mid,mid), since some 00225 existing comparison funcs don't work when passed the same value 00226 for both pointers. */ 00227 00228 higuy++; 00229 if (mid < higuy) { 00230 do { 00231 higuy--; 00232 } while (higuy > mid && ON_QSORT_EQ(higuy,mid)); 00233 } 00234 if (mid >= higuy) { 00235 do { 00236 higuy--; 00237 } while (higuy > lo && ON_QSORT_EQ(higuy,mid)); 00238 } 00239 00240 /* OK, now we have the following: 00241 higuy < loguy 00242 lo <= higuy <= hi 00243 A[i] <= A[mid] for lo <= i <= higuy 00244 A[i] == A[mid] for higuy < i < loguy 00245 A[i] > A[mid] for loguy <= i < hi 00246 A[hi] >= A[mid] */ 00247 00248 /* We've finished the partition, now we want to sort the subarrays 00249 [lo, higuy] and [loguy, hi]. 00250 We do the smaller one first to minimize stack usage. 00251 We only sort arrays of length 2 or more.*/ 00252 00253 if ( higuy - lo >= hi - loguy ) { 00254 if (lo < higuy) { 00255 lostk[stkptr] = lo; 00256 histk[stkptr] = higuy; 00257 ++stkptr; 00258 } /* save big recursion for later */ 00259 00260 if (loguy < hi) { 00261 lo = loguy; 00262 goto recurse; /* do small recursion */ 00263 } 00264 } 00265 else { 00266 if (loguy < hi) { 00267 lostk[stkptr] = loguy; 00268 histk[stkptr] = hi; 00269 ++stkptr; /* save big recursion for later */ 00270 } 00271 00272 if (lo < higuy) { 00273 hi = higuy; 00274 goto recurse; /* do small recursion */ 00275 } 00276 } 00277 } 00278 00279 /* We have sorted the array, except for any pending sorts on the stack. 00280 Check if there are any, and do them. */ 00281 00282 --stkptr; 00283 if (stkptr >= 0) { 00284 lo = lostk[stkptr]; 00285 hi = histk[stkptr]; 00286 goto recurse; /* pop subarray from stack */ 00287 } 00288 else 00289 return; /* all subarrays done */ 00290 } 00291 00292 #undef ON_QSORT_GT 00293 #undef ON_QSORT_LE 00294 #undef ON_QSORT_EQ 00295 #undef ON_QSORT_SWAP 00296 #undef ON_QSORT_CUTOFF 00297 #undef ON_QSORT_STKSIZ