dali-physics/third-party/bullet3/src/LinearMath/btAlignedObjectArray.h

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #ifndef BT_OBJECT_ARRAY__
  17 #define BT_OBJECT_ARRAY__
  18
  19 #include "btScalar.h"  // has definitions like SIMD_FORCE_INLINE
  20 #include "btAlignedAllocator.h"
  21
  22 ///If the platform doesn't support placement new, you can disable BT_USE_PLACEMENT_NEW
  23 ///then the btAlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors
  24 ///You can enable BT_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator=
  25 ///see discussion here: https://bulletphysics.orgphpBB2/viewtopic.php?t=1231 and
  26 ///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240
  27
  28 #define BT_USE_PLACEMENT_NEW 1
  29 //#define BT_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
  30 #define BT_ALLOW_ARRAY_COPY_OPERATOR  // enabling this can accidently perform deep copies of data if you are not careful
  31
  32 #ifdef BT_USE_MEMCPY
  33 #include <memory.h>
  34 #include <string.h>
  35 #endif  //BT_USE_MEMCPY
  36
  37 #ifdef BT_USE_PLACEMENT_NEW
  38 #include <new>  //for placement new
  39 #endif          //BT_USE_PLACEMENT_NEW
  40
  41 ///The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods
  42 ///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
  43 template <typename T>
  44 //template <class T>
  45 class btAlignedObjectArray
  46 {
  47         btAlignedAllocator<T, 16> m_allocator;
  48
  49         int m_size;
  50         int m_capacity;
  51         T* m_data;
  52         //PCK: added this line
  53         bool m_ownsMemory;
  54
  55 #ifdef BT_ALLOW_ARRAY_COPY_OPERATOR
  56 public:
  57         SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T>& other)
  58         {
  59                 copyFromArray(other);
  60                 return *this;
  61         }
  62 #else   //BT_ALLOW_ARRAY_COPY_OPERATOR
  63 private:
  64         SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T>& other);
  65 #endif  //BT_ALLOW_ARRAY_COPY_OPERATOR
  66
  67 protected:
  68         SIMD_FORCE_INLINE int allocSize(int size)
  69         {
  70                 return (size ? size * 2 : 1);
  71         }
  72         SIMD_FORCE_INLINE void copy(int start, int end, T* dest) const
  73         {
  74                 int i;
  75                 for (i = start; i < end; ++i)
  76 #ifdef BT_USE_PLACEMENT_NEW
  77                         new (&dest[i]) T(m_data[i]);
  78 #else
  79                         dest[i] = m_data[i];
  80 #endif  //BT_USE_PLACEMENT_NEW
  81         }
  82
  83         SIMD_FORCE_INLINE void init()
  84         {
  85                 //PCK: added this line
  86                 m_ownsMemory = true;
  87                 m_data = 0;
  88                 m_size = 0;
  89                 m_capacity = 0;
  90         }
  91         SIMD_FORCE_INLINE void destroy(int first, int last)
  92         {
  93                 int i;
  94                 for (i = first; i < last; i++)
  95                 {
  96                         m_data[i].~T();
  97                 }
  98         }
  99
 100         SIMD_FORCE_INLINE void* allocate(int size)
 101         {
 102                 if (size)
 103                         return m_allocator.allocate(size);
 104                 return 0;
 105         }
 106
 107         SIMD_FORCE_INLINE void deallocate()
 108         {
 109                 if (m_data)
 110                 {
 111                         //PCK: enclosed the deallocation in this block
 112                         if (m_ownsMemory)
 113                         {
 114                                 m_allocator.deallocate(m_data);
 115                         }
 116                         m_data = 0;
 117                 }
 118         }
 119
 120 public:
 121         btAlignedObjectArray()
 122         {
 123                 init();
 124         }
 125
 126         ~btAlignedObjectArray()
 127         {
 128                 clear();
 129         }
 130
 131         ///Generally it is best to avoid using the copy constructor of an btAlignedObjectArray, and use a (const) reference to the array instead.
 132         btAlignedObjectArray(const btAlignedObjectArray& otherArray)
 133         {
 134                 init();
 135
 136                 int otherSize = otherArray.size();
 137                 resize(otherSize);
 138                 otherArray.copy(0, otherSize, m_data);
 139         }
 140
 141         /// return the number of elements in the array
 142         SIMD_FORCE_INLINE int size() const
 143         {
 144                 return m_size;
 145         }
 146
 147         SIMD_FORCE_INLINE const T& at(int n) const
 148         {
 149                 btAssert(n >= 0);
 150                 btAssert(n < size());
 151                 return m_data[n];
 152         }
 153
 154         SIMD_FORCE_INLINE T& at(int n)
 155         {
 156                 btAssert(n >= 0);
 157                 btAssert(n < size());
 158                 return m_data[n];
 159         }
 160
 161         SIMD_FORCE_INLINE const T& operator[](int n) const
 162         {
 163                 btAssert(n >= 0);
 164                 btAssert(n < size());
 165                 return m_data[n];
 166         }
 167
 168         SIMD_FORCE_INLINE T& operator[](int n)
 169         {
 170                 btAssert(n >= 0);
 171                 btAssert(n < size());
 172                 return m_data[n];
 173         }
 174
 175         ///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
 176         SIMD_FORCE_INLINE void clear()
 177         {
 178                 destroy(0, size());
 179
 180                 deallocate();
 181
 182                 init();
 183         }
 184
 185         SIMD_FORCE_INLINE void pop_back()
 186         {
 187                 btAssert(m_size > 0);
 188                 m_size--;
 189                 m_data[m_size].~T();
 190         }
 191
 192         ///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
 193         ///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
 194         SIMD_FORCE_INLINE void resizeNoInitialize(int newsize)
 195         {
 196                 if (newsize > size())
 197                 {
 198                         reserve(newsize);
 199                 }
 200                 m_size = newsize;
 201         }
 202
 203         SIMD_FORCE_INLINE void resize(int newsize, const T& fillData = T())
 204         {
 205                 const int curSize = size();
 206
 207                 if (newsize < curSize)
 208                 {
 209                         for (int i = newsize; i < curSize; i++)
 210                         {
 211                                 m_data[i].~T();
 212                         }
 213                 }
 214                 else
 215                 {
 216                         if (newsize > curSize)
 217                         {
 218                                 reserve(newsize);
 219                         }
 220 #ifdef BT_USE_PLACEMENT_NEW
 221                         for (int i = curSize; i < newsize; i++)
 222                         {
 223                                 new (&m_data[i]) T(fillData);
 224                         }
 225 #endif  //BT_USE_PLACEMENT_NEW
 226                 }
 227
 228                 m_size = newsize;
 229         }
 230         SIMD_FORCE_INLINE T& expandNonInitializing()
 231         {
 232                 const int sz = size();
 233                 if (sz == capacity())
 234                 {
 235                         reserve(allocSize(size()));
 236                 }
 237                 m_size++;
 238
 239                 return m_data[sz];
 240         }
 241
 242         SIMD_FORCE_INLINE T& expand(const T& fillValue = T())
 243         {
 244                 const int sz = size();
 245                 if (sz == capacity())
 246                 {
 247                         reserve(allocSize(size()));
 248                 }
 249                 m_size++;
 250 #ifdef BT_USE_PLACEMENT_NEW
 251                 new (&m_data[sz]) T(fillValue);  //use the in-place new (not really allocating heap memory)
 252 #endif
 253
 254                 return m_data[sz];
 255         }
 256
 257         SIMD_FORCE_INLINE void push_back(const T& _Val)
 258         {
 259                 const int sz = size();
 260                 if (sz == capacity())
 261                 {
 262                         reserve(allocSize(size()));
 263                 }
 264
 265 #ifdef BT_USE_PLACEMENT_NEW
 266                 new (&m_data[m_size]) T(_Val);
 267 #else
 268                 m_data[size()] = _Val;
 269 #endif  //BT_USE_PLACEMENT_NEW
 270
 271                 m_size++;
 272         }
 273
 274         /// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
 275         SIMD_FORCE_INLINE int capacity() const
 276         {
 277                 return m_capacity;
 278         }
 279
 280         SIMD_FORCE_INLINE void reserve(int _Count)
 281         {  // determine new minimum length of allocated storage
 282                 if (capacity() < _Count)
 283                 {  // not enough room, reallocate
 284                         T* s = (T*)allocate(_Count);
 285
 286                         copy(0, size(), s);
 287
 288                         destroy(0, size());
 289
 290                         deallocate();
 291
 292                         //PCK: added this line
 293                         m_ownsMemory = true;
 294
 295                         m_data = s;
 296
 297                         m_capacity = _Count;
 298                 }
 299         }
 300
 301         class less
 302         {
 303         public:
 304                 bool operator()(const T& a, const T& b) const
 305                 {
 306                         return (a < b);
 307                 }
 308         };
 309
 310         template <typename L>
 311         void quickSortInternal(const L& CompareFunc, int lo, int hi)
 312         {
 313                 //  lo is the lower index, hi is the upper index
 314                 //  of the region of array a that is to be sorted
 315                 int i = lo, j = hi;
 316                 T x = m_data[(lo + hi) / 2];
 317
 318                 //  partition
 319                 do
 320                 {
 321                         while (CompareFunc(m_data[i], x))
 322                                 i++;
 323                         while (CompareFunc(x, m_data[j]))
 324                                 j--;
 325                         if (i <= j)
 326                         {
 327                                 swap(i, j);
 328                                 i++;
 329                                 j--;
 330                         }
 331                 } while (i <= j);
 332
 333                 //  recursion
 334                 if (lo < j)
 335                         quickSortInternal(CompareFunc, lo, j);
 336                 if (i < hi)
 337                         quickSortInternal(CompareFunc, i, hi);
 338         }
 339
 340         template <typename L>
 341         void quickSort(const L& CompareFunc)
 342         {
 343                 //don't sort 0 or 1 elements
 344                 if (size() > 1)
 345                 {
 346                         quickSortInternal(CompareFunc, 0, size() - 1);
 347                 }
 348         }
 349
 350         ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
 351         template <typename L>
 352         void downHeap(T* pArr, int k, int n, const L& CompareFunc)
 353         {
 354                 /*  PRE: a[k+1..N] is a heap */
 355                 /* POST:  a[k..N]  is a heap */
 356
 357                 T temp = pArr[k - 1];
 358                 /* k has child(s) */
 359                 while (k <= n / 2)
 360                 {
 361                         int child = 2 * k;
 362
 363                         if ((child < n) && CompareFunc(pArr[child - 1], pArr[child]))
 364                         {
 365                                 child++;
 366                         }
 367                         /* pick larger child */
 368                         if (CompareFunc(temp, pArr[child - 1]))
 369                         {
 370                                 /* move child up */
 371                                 pArr[k - 1] = pArr[child - 1];
 372                                 k = child;
 373                         }
 374                         else
 375                         {
 376                                 break;
 377                         }
 378                 }
 379                 pArr[k - 1] = temp;
 380         } /*downHeap*/
 381
 382         void swap(int index0, int index1)
 383         {
 384 #ifdef BT_USE_MEMCPY
 385                 char temp[sizeof(T)];
 386                 memcpy(temp, &m_data[index0], sizeof(T));
 387                 memcpy(&m_data[index0], &m_data[index1], sizeof(T));
 388                 memcpy(&m_data[index1], temp, sizeof(T));
 389 #else
 390                 T temp = m_data[index0];
 391                 m_data[index0] = m_data[index1];
 392                 m_data[index1] = temp;
 393 #endif  //BT_USE_PLACEMENT_NEW
 394         }
 395
 396         template <typename L>
 397         void heapSort(const L& CompareFunc)
 398         {
 399                 /* sort a[0..N-1],  N.B. 0 to N-1 */
 400                 int k;
 401                 int n = m_size;
 402                 for (k = n / 2; k > 0; k--)
 403                 {
 404                         downHeap(m_data, k, n, CompareFunc);
 405                 }
 406
 407                 /* a[1..N] is now a heap */
 408                 while (n >= 1)
 409                 {
 410                         swap(0, n - 1); /* largest of a[0..n-1] */
 411
 412                         n = n - 1;
 413                         /* restore a[1..i-1] heap */
 414                         downHeap(m_data, 1, n, CompareFunc);
 415                 }
 416         }
 417
 418         ///non-recursive binary search, assumes sorted array
 419         int findBinarySearch(const T& key) const
 420         {
 421                 int first = 0;
 422                 int last = size() - 1;
 423
 424                 //assume sorted array
 425                 while (first <= last)
 426                 {
 427                         int mid = (first + last) / 2;  // compute mid point.
 428                         if (key > m_data[mid])
 429                                 first = mid + 1;  // repeat search in top half.
 430                         else if (key < m_data[mid])
 431                                 last = mid - 1;  // repeat search in bottom half.
 432                         else
 433                                 return mid;  // found it. return position /////
 434                 }
 435                 return size();  // failed to find key
 436         }
 437
 438         int findLinearSearch(const T& key) const
 439         {
 440                 int index = size();
 441                 int i;
 442
 443                 for (i = 0; i < size(); i++)
 444                 {
 445                         if (m_data[i] == key)
 446                         {
 447                                 index = i;
 448                                 break;
 449                         }
 450                 }
 451                 return index;
 452         }
 453
 454         // If the key is not in the array, return -1 instead of 0,
 455         // since 0 also means the first element in the array.
 456         int findLinearSearch2(const T& key) const
 457         {
 458                 int index = -1;
 459                 int i;
 460
 461                 for (i = 0; i < size(); i++)
 462                 {
 463                         if (m_data[i] == key)
 464                         {
 465                                 index = i;
 466                                 break;
 467                         }
 468                 }
 469                 return index;
 470         }
 471
 472         void removeAtIndex(int index)
 473         {
 474                 if (index < size())
 475                 {
 476                         swap(index, size() - 1);
 477                         pop_back();
 478                 }
 479         }
 480         void remove(const T& key)
 481         {
 482                 int findIndex = findLinearSearch(key);
 483                 removeAtIndex(findIndex);
 484         }
 485
 486         //PCK: whole function
 487         void initializeFromBuffer(void* buffer, int size, int capacity)
 488         {
 489                 clear();
 490                 m_ownsMemory = false;
 491                 m_data = (T*)buffer;
 492                 m_size = size;
 493                 m_capacity = capacity;
 494         }
 495
 496         void copyFromArray(const btAlignedObjectArray& otherArray)
 497         {
 498                 int otherSize = otherArray.size();
 499                 resize(otherSize);
 500                 otherArray.copy(0, otherSize, m_data);
 501         }
 502 };
 503
 504 #endif  //BT_OBJECT_ARRAY__