dali-physics/third-party/bullet3/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         template <class T, class Z>
  45         inline int operator()(const T& a, const Z& b)
  46         {
  47                 return (a < b ? -1 : (a > b ? 1 : 0));
  48         }
  49 };
  50
  51 //! Prototype for comparators
  52 class integer_comparator
  53 {
  54 public:
  55         template <class T>
  56         inline int operator()(const T& a, const T& b)
  57         {
  58                 return (int)(a - b);
  59         }
  60 };
  61
  62 //!Prototype for getting the integer representation of an object
  63 class uint_key_func
  64 {
  65 public:
  66         template <class T>
  67         inline GUINT operator()(const T& a)
  68         {
  69                 return (GUINT)a;
  70         }
  71 };
  72
  73 //!Prototype for copying elements
  74 class copy_elements_func
  75 {
  76 public:
  77         template <class T>
  78         inline void operator()(T& a, T& b)
  79         {
  80                 a = b;
  81         }
  82 };
  83
  84 //!Prototype for copying elements
  85 class memcopy_elements_func
  86 {
  87 public:
  88         template <class T>
  89         inline void operator()(T& a, T& b)
  90         {
  91                 gim_simd_memcpy(&a, &b, sizeof(T));
  92         }
  93 };
  94
  95 //! @{
  96 struct GIM_RSORT_TOKEN
  97 {
  98         GUINT m_key;
  99         GUINT m_value;
 100         GIM_RSORT_TOKEN()
 101         {
 102         }
 103         GIM_RSORT_TOKEN(const GIM_RSORT_TOKEN& rtoken)
 104         {
 105                 m_key = rtoken.m_key;
 106                 m_value = rtoken.m_value;
 107         }
 108
 109         inline bool operator<(const GIM_RSORT_TOKEN& other) const
 110         {
 111                 return (m_key < other.m_key);
 112         }
 113
 114         inline bool operator>(const GIM_RSORT_TOKEN& other) const
 115         {
 116                 return (m_key > other.m_key);
 117         }
 118 };
 119
 120 //! Prototype for comparators
 121 class GIM_RSORT_TOKEN_COMPARATOR
 122 {
 123 public:
 124         inline int operator()(const GIM_RSORT_TOKEN& a, const GIM_RSORT_TOKEN& b)
 125         {
 126                 return (int)((a.m_key) - (b.m_key));
 127         }
 128 };
 129
 130 #define kHist 2048
 131 // ---- utils for accessing 11-bit quantities
 132 #define D11_0(x) (x & 0x7FF)
 133 #define D11_1(x) (x >> 11 & 0x7FF)
 134 #define D11_2(x) (x >> 22)
 135
 136 ///Radix sort for unsigned integer keys
 137 inline void gim_radix_sort_rtokens(
 138         GIM_RSORT_TOKEN* array,
 139         GIM_RSORT_TOKEN* sorted, GUINT element_count)
 140 {
 141         GUINT i;
 142         GUINT b0[kHist * 3];
 143         GUINT* b1 = b0 + kHist;
 144         GUINT* b2 = b1 + kHist;
 145         for (i = 0; i < kHist * 3; ++i)
 146         {
 147                 b0[i] = 0;
 148         }
 149         GUINT fi;
 150         GUINT pos;
 151         for (i = 0; i < element_count; ++i)
 152         {
 153                 fi = array[i].m_key;
 154                 b0[D11_0(fi)]++;
 155                 b1[D11_1(fi)]++;
 156                 b2[D11_2(fi)]++;
 157         }
 158         {
 159                 GUINT sum0 = 0, sum1 = 0, sum2 = 0;
 160                 GUINT tsum;
 161                 for (i = 0; i < kHist; ++i)
 162                 {
 163                         tsum = b0[i] + sum0;
 164                         b0[i] = sum0 - 1;
 165                         sum0 = tsum;
 166                         tsum = b1[i] + sum1;
 167                         b1[i] = sum1 - 1;
 168                         sum1 = tsum;
 169                         tsum = b2[i] + sum2;
 170                         b2[i] = sum2 - 1;
 171                         sum2 = tsum;
 172                 }
 173         }
 174         for (i = 0; i < element_count; ++i)
 175         {
 176                 fi = array[i].m_key;
 177                 pos = D11_0(fi);
 178                 pos = ++b0[pos];
 179                 sorted[pos].m_key = array[i].m_key;
 180                 sorted[pos].m_value = array[i].m_value;
 181         }
 182         for (i = 0; i < element_count; ++i)
 183         {
 184                 fi = sorted[i].m_key;
 185                 pos = D11_1(fi);
 186                 pos = ++b1[pos];
 187                 array[pos].m_key = sorted[i].m_key;
 188                 array[pos].m_value = sorted[i].m_value;
 189         }
 190         for (i = 0; i < element_count; ++i)
 191         {
 192                 fi = array[i].m_key;
 193                 pos = D11_2(fi);
 194                 pos = ++b2[pos];
 195                 sorted[pos].m_key = array[i].m_key;
 196                 sorted[pos].m_value = array[i].m_value;
 197         }
 198 }
 199
 200 /// Get the sorted tokens from an array. For generic use. Tokens are IRR_RSORT_TOKEN
 201 /*!
 202 *\param array Array of elements to sort
 203 *\param sorted_tokens Tokens of sorted elements
 204 *\param element_count element count
 205 *\param uintkey_macro Functor which retrieves the integer representation of an array element
 206 */
 207 template <typename T, class GETKEY_CLASS>
 208 void gim_radix_sort_array_tokens(
 209         T* array,
 210         GIM_RSORT_TOKEN* sorted_tokens,
 211         GUINT element_count, GETKEY_CLASS uintkey_macro)
 212 {
 213         GIM_RSORT_TOKEN* _unsorted = (GIM_RSORT_TOKEN*)gim_alloc(sizeof(GIM_RSORT_TOKEN) * element_count);
 214         for (GUINT _i = 0; _i < element_count; ++_i)
 215         {
 216                 _unsorted[_i].m_key = uintkey_macro(array[_i]);
 217                 _unsorted[_i].m_value = _i;
 218         }
 219         gim_radix_sort_rtokens(_unsorted, sorted_tokens, element_count);
 220         gim_free(_unsorted);
 221         gim_free(_unsorted);
 222 }
 223
 224 /// Sorts array in place. For generic use
 225 /*!
 226 \param type Type of the array
 227 \param array
 228 \param element_count
 229 \param get_uintkey_macro Macro for extract the Integer value of the element. Similar to SIMPLE_GET_UINTKEY
 230 \param copy_elements_macro Macro for copy elements, similar to SIMPLE_COPY_ELEMENTS
 231 */
 232 template <typename T, class GETKEY_CLASS, class COPY_CLASS>
 233 void gim_radix_sort(
 234         T* array, GUINT element_count,
 235         GETKEY_CLASS get_uintkey_macro, COPY_CLASS copy_elements_macro)
 236 {
 237         GIM_RSORT_TOKEN* _sorted = (GIM_RSORT_TOKEN*)gim_alloc(sizeof(GIM_RSORT_TOKEN) * element_count);
 238         gim_radix_sort_array_tokens(array, _sorted, element_count, get_uintkey_macro);
 239         T* _original_array = (T*)gim_alloc(sizeof(T) * element_count);
 240         gim_simd_memcpy(_original_array, array, sizeof(T) * element_count);
 241         for (GUINT _i = 0; _i < element_count; ++_i)
 242         {
 243                 copy_elements_macro(array[_i], _original_array[_sorted[_i].m_value]);
 244         }
 245         gim_free(_original_array);
 246         gim_free(_sorted);
 247 }
 248
 249 //! Failsafe Iterative binary search,
 250 /*!
 251 If the element is not found, it returns the nearest upper element position, may be the further position after the last element.
 252 \param _array
 253 \param _start_i the beginning of the array
 254 \param _end_i the ending  index of the array
 255 \param _search_key Value to find
 256 \param _comp_macro macro for comparing elements
 257 \param _found If true the value has found. Boolean
 258 \param _result_index the index of the found element, or if not found then it will get the index of the  closest bigger value
 259 */
 260 template <class T, typename KEYCLASS, typename COMP_CLASS>
 261 bool gim_binary_search_ex(
 262         const T* _array, GUINT _start_i,
 263         GUINT _end_i, GUINT& _result_index,
 264         const KEYCLASS& _search_key,
 265         COMP_CLASS _comp_macro)
 266 {
 267         GUINT _k;
 268         int _comp_result;
 269         GUINT _i = _start_i;
 270         GUINT _j = _end_i + 1;
 271         while (_i < _j)
 272         {
 273                 _k = (_j + _i - 1) / 2;
 274                 _comp_result = _comp_macro(_array[_k], _search_key);
 275                 if (_comp_result == 0)
 276                 {
 277                         _result_index = _k;
 278                         return true;
 279                 }
 280                 else if (_comp_result < 0)
 281                 {
 282                         _i = _k + 1;
 283                 }
 284                 else
 285                 {
 286                         _j = _k;
 287                 }
 288         }
 289         _result_index = _i;
 290         return false;
 291 }
 292
 293 //! Failsafe Iterative binary search,Template version
 294 /*!
 295 If the element is not found, it returns the nearest upper element position, may be the further position after the last element.
 296 \param _array
 297 \param _start_i the beginning of the array
 298 \param _end_i the ending  index of the array
 299 \param _search_key Value to find
 300 \param _result_index the index of the found element, or if not found then it will get the index of the  closest bigger value
 301 \return true if found, else false
 302 */
 303 template <class T>
 304 bool gim_binary_search(
 305         const T* _array, GUINT _start_i,
 306         GUINT _end_i, const T& _search_key,
 307         GUINT& _result_index)
 308 {
 309         GUINT _i = _start_i;
 310         GUINT _j = _end_i + 1;
 311         GUINT _k;
 312         while (_i < _j)
 313         {
 314                 _k = (_j + _i - 1) / 2;
 315                 if (_array[_k] == _search_key)
 316                 {
 317                         _result_index = _k;
 318                         return true;
 319                 }
 320                 else if (_array[_k] < _search_key)
 321                 {
 322                         _i = _k + 1;
 323                 }
 324                 else
 325                 {
 326                         _j = _k;
 327                 }
 328         }
 329         _result_index = _i;
 330         return false;
 331 }
 332
 333 ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
 334 template <typename T, typename COMP_CLASS>
 335 void gim_down_heap(T* pArr, GUINT k, GUINT n, COMP_CLASS CompareFunc)
 336 {
 337         /*  PRE: a[k+1..N] is a heap */
 338         /* POST:  a[k..N]  is a heap */
 339
 340         T temp = pArr[k - 1];
 341         /* k has child(s) */
 342         while (k <= n / 2)
 343         {
 344                 int child = 2 * k;
 345
 346                 if ((child < (int)n) && CompareFunc(pArr[child - 1], pArr[child]) < 0)
 347                 {
 348                         child++;
 349                 }
 350                 /* pick larger child */
 351                 if (CompareFunc(temp, pArr[child - 1]) < 0)
 352                 {
 353                         /* move child up */
 354                         pArr[k - 1] = pArr[child - 1];
 355                         k = child;
 356                 }
 357                 else
 358                 {
 359                         break;
 360                 }
 361         }
 362         pArr[k - 1] = temp;
 363 } /*downHeap*/
 364
 365 template <typename T, typename COMP_CLASS>
 366 void gim_heap_sort(T* pArr, GUINT element_count, COMP_CLASS CompareFunc)
 367 {
 368         /* sort a[0..N-1],  N.B. 0 to N-1 */
 369         GUINT k;
 370         GUINT n = element_count;
 371         for (k = n / 2; k > 0; k--)
 372         {
 373                 gim_down_heap(pArr, k, n, CompareFunc);
 374         }
 375
 376         /* a[1..N] is now a heap */
 377         while (n >= 2)
 378         {
 379                 gim_swap_elements(pArr, 0, n - 1); /* largest of a[0..n-1] */
 380                 --n;
 381                 /* restore a[1..i-1] heap */
 382                 gim_down_heap(pArr, 1, n, CompareFunc);
 383         }
 384 }
 385
 386 #endif  // GIM_RADIXSORT_H_INCLUDED