src/BulletCollision/Gimpact/gim_radixsort.h

   1 #ifndef GIM_RADIXSORT_H_INCLUDED
   2 #define GIM_RADIXSORT_H_INCLUDED
   3 /*! \file gim_radixsort.h
   4 \author Francisco Leon Najera.
   5 Based on the work of Michael Herf : "fast floating-point radix sort"
   6 Avaliable on http://www.stereopsis.com/radix.html
   7 */
   8 /*
   9 -----------------------------------------------------------------------------
  10 This source file is part of GIMPACT Library.
  11
  12 For the latest info, see http://gimpact.sourceforge.net/
  13
  14 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
  15 email: projectileman@yahoo.com
  16
  17  This library is free software; you can redistribute it and/or
  18  modify it under the terms of EITHER:
  19    (1) The GNU Lesser General Public License as published by the Free
  20        Software Foundation; either version 2.1 of the License, or (at
  21        your option) any later version. The text of the GNU Lesser
  22        General Public License is included with this library in the
  23        file GIMPACT-LICENSE-LGPL.TXT.
  24    (2) The BSD-style license that is included with this library in
  25        the file GIMPACT-LICENSE-BSD.TXT.
  26    (3) The zlib/libpng license that is included with this library in
  27        the file GIMPACT-LICENSE-ZLIB.TXT.
  28
  29  This library is distributed in the hope that it will be useful,
  30  but WITHOUT ANY WARRANTY; without even the implied warranty of
  31  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
  32  GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
  33
  34 -----------------------------------------------------------------------------
  35 */
  36
  37 #include "gim_memory.h"
  38
  39 ///Macros for sorting.
  40 //! Prototype for comparators
  41 class less_comparator
  42 {
  43         public:
  44
  45         template<class T,class Z>
  46         inline int operator() ( const T& a, const Z& b )
  47         {
  48                 return ( a<b?-1:(a>b?1:0));
  49         }
  50 };
  51
  52 //! Prototype for comparators
  53 class integer_comparator
  54 {
  55         public:
  56
  57         template<class T>
  58         inline int operator() ( const T& a, const T& b )
  59         {
  60                 return (int)(a-b);
  61         }
  62 };
  63
  64 //!Prototype for getting the integer representation of an object
  65 class uint_key_func
  66 {
  67 public:
  68         template<class T>
  69         inline GUINT operator()( const T& a)
  70         {
  71                 return (GUINT)a;
  72         }
  73 };
  74
  75
  76 //!Prototype for copying elements
  77 class copy_elements_func
  78 {
  79 public:
  80         template<class T>
  81         inline void operator()(T& a,T& b)
  82         {
  83                 a = b;
  84         }
  85 };
  86
  87 //!Prototype for copying elements
  88 class memcopy_elements_func
  89 {
  90 public:
  91         template<class T>
  92         inline void operator()(T& a,T& b)
  93         {
  94                 gim_simd_memcpy(&a,&b,sizeof(T));
  95         }
  96 };
  97
  98
  99 //! @{
 100 struct GIM_RSORT_TOKEN
 101 {
 102     GUINT m_key;
 103     GUINT m_value;
 104     GIM_RSORT_TOKEN()
 105     {
 106     }
 107     GIM_RSORT_TOKEN(const GIM_RSORT_TOKEN& rtoken)
 108     {
 109         m_key = rtoken.m_key;
 110         m_value = rtoken.m_value;
 111     }
 112
 113     inline bool operator <(const GIM_RSORT_TOKEN& other) const
 114         {
 115                 return (m_key < other.m_key);
 116         }
 117
 118         inline bool operator >(const GIM_RSORT_TOKEN& other) const
 119         {
 120                 return (m_key > other.m_key);
 121         }
 122 };
 123
 124 //! Prototype for comparators
 125 class GIM_RSORT_TOKEN_COMPARATOR
 126 {
 127         public:
 128
 129         inline int operator()( const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b )
 130         {
 131                 return (int)((a.m_key) - (b.m_key));
 132         }
 133 };
 134
 135
 136
 137 #define kHist 2048
 138 // ---- utils for accessing 11-bit quantities
 139 #define D11_0(x)        (x & 0x7FF)
 140 #define D11_1(x)        (x >> 11 & 0x7FF)
 141 #define D11_2(x)        (x >> 22 )
 142
 143
 144
 145 ///Radix sort for unsigned integer keys
 146 inline void gim_radix_sort_rtokens(
 147                                 GIM_RSORT_TOKEN * array,
 148                                 GIM_RSORT_TOKEN * sorted, GUINT element_count)
 149 {
 150         GUINT i;
 151         GUINT b0[kHist * 3];
 152         GUINT *b1 = b0 + kHist;
 153         GUINT *b2 = b1 + kHist;
 154         for (i = 0; i < kHist * 3; ++i)
 155         {
 156                 b0[i] = 0;
 157         }
 158         GUINT fi;
 159         GUINT pos;
 160         for (i = 0; i < element_count; ++i)
 161         {
 162             fi = array[i].m_key;
 163                 b0[D11_0(fi)] ++;
 164                 b1[D11_1(fi)] ++;
 165                 b2[D11_2(fi)] ++;
 166         }
 167         {
 168                 GUINT sum0 = 0, sum1 = 0, sum2 = 0;
 169                 GUINT tsum;
 170                 for (i = 0; i < kHist; ++i)
 171                 {
 172                         tsum = b0[i] + sum0;
 173                         b0[i] = sum0 - 1;
 174                         sum0 = tsum;
 175                         tsum = b1[i] + sum1;
 176                         b1[i] = sum1 - 1;
 177                         sum1 = tsum;
 178                         tsum = b2[i] + sum2;
 179                         b2[i] = sum2 - 1;
 180                         sum2 = tsum;
 181                 }
 182         }
 183         for (i = 0; i < element_count; ++i)
 184         {
 185         fi = array[i].m_key;
 186                 pos = D11_0(fi);
 187                 pos = ++b0[pos];
 188                 sorted[pos].m_key = array[i].m_key;
 189                 sorted[pos].m_value = array[i].m_value;
 190         }
 191         for (i = 0; i < element_count; ++i)
 192         {
 193         fi = sorted[i].m_key;
 194                 pos = D11_1(fi);
 195                 pos = ++b1[pos];
 196                 array[pos].m_key = sorted[i].m_key;
 197                 array[pos].m_value = sorted[i].m_value;
 198         }
 199         for (i = 0; i < element_count; ++i)
 200         {
 201         fi = array[i].m_key;
 202                 pos = D11_2(fi);
 203                 pos = ++b2[pos];
 204                 sorted[pos].m_key = array[i].m_key;
 205                 sorted[pos].m_value = array[i].m_value;
 206         }
 207 }
 208
 209
 210
 211
 212 /// Get the sorted tokens from an array. For generic use. Tokens are IRR_RSORT_TOKEN
 213 /*!
 214 *\param array Array of elements to sort
 215 *\param sorted_tokens Tokens of sorted elements
 216 *\param element_count element count
 217 *\param uintkey_macro Functor which retrieves the integer representation of an array element
 218 */
 219 template<typename T, class GETKEY_CLASS>
 220 void gim_radix_sort_array_tokens(
 221                         T* array ,
 222                         GIM_RSORT_TOKEN * sorted_tokens,
 223                         GUINT element_count,GETKEY_CLASS uintkey_macro)
 224 {
 225         GIM_RSORT_TOKEN * _unsorted = (GIM_RSORT_TOKEN *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count);
 226     for (GUINT _i=0;_i<element_count;++_i)
 227     {
 228         _unsorted[_i].m_key = uintkey_macro(array[_i]);
 229         _unsorted[_i].m_value = _i;
 230     }
 231     gim_radix_sort_rtokens(_unsorted,sorted_tokens,element_count);
 232     gim_free(_unsorted);
 233     gim_free(_unsorted);
 234 }
 235
 236 /// Sorts array in place. For generic use
 237 /*!
 238 \param type Type of the array
 239 \param array
 240 \param element_count
 241 \param get_uintkey_macro Macro for extract the Integer value of the element. Similar to SIMPLE_GET_UINTKEY
 242 \param copy_elements_macro Macro for copy elements, similar to SIMPLE_COPY_ELEMENTS
 243 */
 244 template<typename T, class GETKEY_CLASS, class COPY_CLASS>
 245 void gim_radix_sort(
 246         T * array, GUINT element_count,
 247         GETKEY_CLASS get_uintkey_macro, COPY_CLASS copy_elements_macro)
 248 {
 249         GIM_RSORT_TOKEN * _sorted = (GIM_RSORT_TOKEN  *) gim_alloc(sizeof(GIM_RSORT_TOKEN)*element_count);
 250     gim_radix_sort_array_tokens(array,_sorted,element_count,get_uintkey_macro);
 251     T * _original_array = (T *) gim_alloc(sizeof(T)*element_count);
 252     gim_simd_memcpy(_original_array,array,sizeof(T)*element_count);
 253     for (GUINT _i=0;_i<element_count;++_i)
 254     {
 255         copy_elements_macro(array[_i],_original_array[_sorted[_i].m_value]);
 256     }
 257     gim_free(_original_array);
 258     gim_free(_sorted);
 259 }
 260
 261 //! Failsafe Iterative binary search,
 262 /*!
 263 If the element is not found, it returns the nearest upper element position, may be the further position after the last element.
 264 \param _array
 265 \param _start_i the beginning of the array
 266 \param _end_i the ending  index of the array
 267 \param _search_key Value to find
 268 \param _comp_macro macro for comparing elements
 269 \param _found If true the value has found. Boolean
 270 \param _result_index the index of the found element, or if not found then it will get the index of the  closest bigger value
 271 */
 272 template<class T, typename KEYCLASS, typename COMP_CLASS>
 273 bool  gim_binary_search_ex(
 274                 const T* _array, GUINT _start_i,
 275                 GUINT _end_i,GUINT & _result_index,
 276                 const KEYCLASS & _search_key,
 277                 COMP_CLASS _comp_macro)
 278 {
 279         GUINT _k;
 280         int _comp_result;
 281         GUINT _i = _start_i;
 282         GUINT _j = _end_i+1;
 283         while (_i < _j)
 284         {
 285                 _k = (_j+_i-1)/2;
 286                 _comp_result = _comp_macro(_array[_k], _search_key);
 287                 if (_comp_result == 0)
 288                 {
 289                         _result_index = _k;
 290                         return true;
 291                 }
 292                 else if (_comp_result < 0)
 293                 {
 294                         _i = _k+1;
 295                 }
 296                 else
 297                 {
 298                         _j = _k;
 299                 }
 300         }
 301         _result_index = _i;
 302         return false;
 303 }
 304
 305
 306
 307 //! Failsafe Iterative binary search,Template version
 308 /*!
 309 If the element is not found, it returns the nearest upper element position, may be the further position after the last element.
 310 \param _array
 311 \param _start_i the beginning of the array
 312 \param _end_i the ending  index of the array
 313 \param _search_key Value to find
 314 \param _result_index the index of the found element, or if not found then it will get the index of the  closest bigger value
 315 \return true if found, else false
 316 */
 317 template<class T>
 318 bool gim_binary_search(
 319         const T*_array,GUINT _start_i,
 320         GUINT _end_i,const T & _search_key,
 321         GUINT & _result_index)
 322 {
 323         GUINT _i = _start_i;
 324         GUINT _j = _end_i+1;
 325         GUINT _k;
 326         while(_i < _j)
 327         {
 328                 _k = (_j+_i-1)/2;
 329                 if(_array[_k]==_search_key)
 330                 {
 331                         _result_index = _k;
 332                         return true;
 333                 }
 334                 else if (_array[_k]<_search_key)
 335                 {
 336                         _i = _k+1;
 337                 }
 338                 else
 339                 {
 340                         _j = _k;
 341                 }
 342         }
 343         _result_index = _i;
 344         return false;
 345 }
 346
 347
 348
 349 ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
 350 template <typename T, typename COMP_CLASS>
 351 void gim_down_heap(T *pArr, GUINT k, GUINT n,COMP_CLASS CompareFunc)
 352 {
 353         /*  PRE: a[k+1..N] is a heap */
 354         /* POST:  a[k..N]  is a heap */
 355
 356         T temp = pArr[k - 1];
 357         /* k has child(s) */
 358         while (k <= n/2)
 359         {
 360                 int child = 2*k;
 361
 362                 if ((child < (int)n) && CompareFunc(pArr[child - 1] , pArr[child])<0)
 363                 {
 364                         child++;
 365                 }
 366                 /* pick larger child */
 367                 if (CompareFunc(temp , pArr[child - 1])<0)
 368                 {
 369                         /* move child up */
 370                         pArr[k - 1] = pArr[child - 1];
 371                         k = child;
 372                 }
 373                 else
 374                 {
 375                         break;
 376                 }
 377         }
 378         pArr[k - 1] = temp;
 379 } /*downHeap*/
 380
 381
 382 template <typename T, typename COMP_CLASS>
 383 void gim_heap_sort(T *pArr, GUINT element_count, COMP_CLASS CompareFunc)
 384 {
 385         /* sort a[0..N-1],  N.B. 0 to N-1 */
 386         GUINT k;
 387         GUINT n = element_count;
 388         for (k = n/2; k > 0; k--)
 389         {
 390                 gim_down_heap(pArr, k, n, CompareFunc);
 391         }
 392
 393         /* a[1..N] is now a heap */
 394         while ( n>=2 )
 395         {
 396                 gim_swap_elements(pArr,0,n-1); /* largest of a[0..n-1] */
 397                 --n;
 398                 /* restore a[1..i-1] heap */
 399                 gim_down_heap(pArr, 1, n, CompareFunc);
 400         }
 401 }
 402
 403
 404
 405
 406 #endif // GIM_RADIXSORT_H_INCLUDED