ode/src/util.h

   1 /*************************************************************************
   2  *                                                                       *
   3  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
   4  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
   5  *                                                                       *
   6  * This library is free software; you can redistribute it and/or         *
   7  * modify it under the terms of EITHER:                                  *
   8  *   (1) The GNU Lesser General Public License as published by the Free  *
   9  *       Software Foundation; either version 2.1 of the License, or (at  *
  10  *       your option) any later version. The text of the GNU Lesser      *
  11  *       General Public License is included with this library in the     *
  12  *       file LICENSE.TXT.                                               *
  13  *   (2) The BSD-style license that is included with this library in     *
  14  *       the file LICENSE-BSD.TXT.                                       *
  15  *                                                                       *
  16  * This library is distributed in the hope that it will be useful,       *
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
  19  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
  20  *                                                                       *
  21  *************************************************************************/
  22
  23 #ifndef _ODE_UTIL_H_
  24 #define _ODE_UTIL_H_
  25
  26 #include "objects.h"
  27 #include "common.h"
  28
  29
  30 /* utility */
  31
  32 void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize);
  33 void dxStepBody (dxBody *b, dReal h);
  34
  35
  36 struct dxWorldProcessMemoryManager:
  37     public dBase
  38 {
  39     typedef void *(*alloc_block_fn_t)(sizeint block_size);
  40     typedef void *(*shrink_block_fn_t)(void *block_pointer, sizeint block_current_size, sizeint block_smaller_size);
  41     typedef void (*free_block_fn_t)(void *block_pointer, sizeint block_current_size);
  42
  43     dxWorldProcessMemoryManager(alloc_block_fn_t fnAlloc, shrink_block_fn_t fnShrink, free_block_fn_t fnFree)
  44     {
  45         Assign(fnAlloc, fnShrink, fnFree);
  46     }
  47
  48     void Assign(alloc_block_fn_t fnAlloc, shrink_block_fn_t fnShrink, free_block_fn_t fnFree)
  49     {
  50         m_fnAlloc = fnAlloc;
  51         m_fnShrink = fnShrink;
  52         m_fnFree = fnFree;
  53     }
  54
  55     alloc_block_fn_t m_fnAlloc;
  56     shrink_block_fn_t m_fnShrink;
  57     free_block_fn_t m_fnFree;
  58 };
  59
  60 extern dxWorldProcessMemoryManager g_WorldProcessMallocMemoryManager;
  61
  62 struct dxWorldProcessMemoryReserveInfo:
  63     public dBase
  64 {
  65     dxWorldProcessMemoryReserveInfo(float fReserveFactor, unsigned uiReserveMinimum)
  66     {
  67         Assign(fReserveFactor, uiReserveMinimum);
  68     }
  69
  70     void Assign(float fReserveFactor, unsigned uiReserveMinimum)
  71     {
  72         m_fReserveFactor = fReserveFactor;
  73         m_uiReserveMinimum = uiReserveMinimum;
  74     }
  75
  76     float m_fReserveFactor; // Use float as precision does not matter here
  77     unsigned m_uiReserveMinimum;
  78 };
  79
  80 extern dxWorldProcessMemoryReserveInfo g_WorldProcessDefaultReserveInfo;
  81
  82
  83 class dxWorldProcessMemArena:
  84     private dBase // new/delete must not be called for this class
  85 {
  86 public:
  87 #define BUFFER_TO_ARENA_EXTRA (EFFICIENT_ALIGNMENT + dEFFICIENT_SIZE(sizeof(dxWorldProcessMemArena)))
  88     static bool IsArenaPossible(sizeint nBufferSize)
  89     {
  90         return SIZE_MAX - BUFFER_TO_ARENA_EXTRA >= nBufferSize; // This ensures there will be no overflow
  91     }
  92
  93     static sizeint MakeBufferSize(sizeint nArenaSize)
  94     {
  95         return nArenaSize - BUFFER_TO_ARENA_EXTRA;
  96     }
  97
  98     static sizeint MakeArenaSize(sizeint nBufferSize)
  99     {
 100         return BUFFER_TO_ARENA_EXTRA + nBufferSize;
 101     }
 102 #undef BUFFER_TO_ARENA_EXTRA
 103
 104     bool IsStructureValid() const
 105     {
 106         return m_pAllocBegin != NULL && m_pAllocEnd != NULL && m_pAllocBegin <= m_pAllocEnd
 107             && (m_pAllocCurrentOrNextArena == NULL || m_pAllocCurrentOrNextArena == m_pAllocBegin)
 108             && m_pArenaBegin != NULL && m_pArenaBegin <= m_pAllocBegin;
 109     }
 110
 111     sizeint GetMemorySize() const
 112     {
 113         return (sizeint)m_pAllocEnd - (sizeint)m_pAllocBegin;
 114     }
 115
 116     void *SaveState() const
 117     {
 118         return m_pAllocCurrentOrNextArena;
 119     }
 120
 121     void RestoreState(void *state)
 122     {
 123         m_pAllocCurrentOrNextArena = state;
 124     }
 125
 126     void ResetState()
 127     {
 128         m_pAllocCurrentOrNextArena = m_pAllocBegin;
 129     }
 130
 131     void *PeekBufferRemainder() const
 132     {
 133         return m_pAllocCurrentOrNextArena;
 134     }
 135
 136     void *AllocateBlock(sizeint size)
 137     {
 138         void *arena = m_pAllocCurrentOrNextArena;
 139         m_pAllocCurrentOrNextArena = dOFFSET_EFFICIENTLY(arena, size);
 140         dIASSERT(m_pAllocCurrentOrNextArena <= m_pAllocEnd);
 141         dIASSERT(dEFFICIENT_PTR(arena) == arena);
 142
 143         return arena;
 144     }
 145
 146     void *AllocateOveralignedBlock(sizeint size, unsigned alignment)
 147     {
 148         void *arena = m_pAllocCurrentOrNextArena;
 149         m_pAllocCurrentOrNextArena = dOFFSET_OVERALIGNEDLY(arena, size, alignment);
 150         dIASSERT(m_pAllocCurrentOrNextArena <= m_pAllocEnd);
 151
 152         void *block = dOVERALIGNED_PTR(arena, alignment);
 153         return block;
 154     }
 155
 156     template<typename ElementType>
 157     ElementType *AllocateArray(sizeint count)
 158     {
 159         return (ElementType *)AllocateBlock(count * sizeof(ElementType));
 160     }
 161
 162     template<typename ElementType>
 163     ElementType *AllocateOveralignedArray(sizeint count, unsigned alignment)
 164     {
 165         return (ElementType *)AllocateOveralignedBlock(count * sizeof(ElementType), alignment);
 166     }
 167
 168     template<typename ElementType>
 169     void ShrinkArray(ElementType *arr, sizeint oldcount, sizeint newcount)
 170     {
 171         dIASSERT(newcount <= oldcount);
 172         dIASSERT(dOFFSET_EFFICIENTLY(arr, oldcount * sizeof(ElementType)) == m_pAllocCurrentOrNextArena);
 173         m_pAllocCurrentOrNextArena = dOFFSET_EFFICIENTLY(arr, newcount * sizeof(ElementType));
 174     }
 175
 176 public:
 177     static dxWorldProcessMemArena *ReallocateMemArena (
 178         dxWorldProcessMemArena *oldarena, sizeint memreq,
 179         const dxWorldProcessMemoryManager *memmgr, float rsrvfactor, unsigned rsrvminimum);
 180     static void FreeMemArena (dxWorldProcessMemArena *arena);
 181
 182     dxWorldProcessMemArena *GetNextMemArena() const { return (dxWorldProcessMemArena *)m_pAllocCurrentOrNextArena; }
 183     void SetNextMemArena(dxWorldProcessMemArena *pArenaInstance) { m_pAllocCurrentOrNextArena = pArenaInstance; }
 184
 185 private:
 186     static sizeint AdjustArenaSizeForReserveRequirements(sizeint arenareq, float rsrvfactor, unsigned rsrvminimum);
 187
 188 private:
 189     void *m_pAllocCurrentOrNextArena;
 190     void *m_pAllocBegin;
 191     void *m_pAllocEnd;
 192     void *m_pArenaBegin;
 193
 194     const dxWorldProcessMemoryManager *m_pArenaMemMgr;
 195 };
 196
 197 class dxWorldProcessContext:
 198     public dBase
 199 {
 200 public:
 201     dxWorldProcessContext();
 202     ~dxWorldProcessContext();
 203
 204     void CleanupWorldReferences(dxWorld *pswWorldInstance);
 205
 206 public:
 207     bool EnsureStepperSyncObjectsAreAllocated(dxWorld *pswWorldInstance);
 208     dCallWaitID GetIslandsSteppingWait() const { return m_pcwIslandsSteppingWait; }
 209
 210 public:
 211     dxWorldProcessMemArena *ObtainStepperMemArena();
 212     void ReturnStepperMemArena(dxWorldProcessMemArena *pmaArenaInstance);
 213
 214     dxWorldProcessMemArena *ReallocateIslandsMemArena(sizeint nMemoryRequirement,
 215         const dxWorldProcessMemoryManager *pmmMemortManager, float fReserveFactor, unsigned uiReserveMinimum);
 216     bool ReallocateStepperMemArenas(dxWorld *world, unsigned nIslandThreadsCount, sizeint nMemoryRequirement,
 217         const dxWorldProcessMemoryManager *pmmMemortManager, float fReserveFactor, unsigned uiReserveMinimum);
 218
 219 private:
 220     static void FreeArenasList(dxWorldProcessMemArena *pmaExistingArenas);
 221
 222 private:
 223     void SetIslandsMemArena(dxWorldProcessMemArena *pmaInstance) { m_pmaIslandsArena = pmaInstance; }
 224     dxWorldProcessMemArena *GetIslandsMemArena() const { return m_pmaIslandsArena; }
 225
 226     void SetStepperArenasList(dxWorldProcessMemArena *pmaInstance) { m_pmaStepperArenas = pmaInstance; }
 227     dxWorldProcessMemArena *GetStepperArenasList() const { return m_pmaStepperArenas; }
 228
 229     inline dxWorldProcessMemArena *GetStepperArenasHead() const;
 230     inline bool TryExtractingStepperArenasHead(dxWorldProcessMemArena *pmaHeadInstance);
 231     inline bool TryInsertingStepperArenasHead(dxWorldProcessMemArena *pmaArenaInstance, dxWorldProcessMemArena *pmaExistingHead);
 232
 233 public:
 234     void LockForAddLimotSerialization();
 235     void UnlockForAddLimotSerialization();
 236     void LockForStepbodySerialization();
 237     void UnlockForStepbodySerialization();
 238
 239 private:
 240     enum dxProcessContextMutex
 241     {
 242         dxPCM_STEPPER_ARENA_OBTAIN,
 243         dxPCM_STEPPER_ADDLIMOT_SERIALIZE,
 244         dxPCM_STEPPER_STEPBODY_SERIALIZE,
 245
 246         dxPCM__MAX
 247     };
 248
 249     static const char *const m_aszContextMutexNames[dxPCM__MAX];
 250
 251 private:
 252     dxWorldProcessMemArena  *m_pmaIslandsArena;
 253     dxWorldProcessMemArena  *volatile m_pmaStepperArenas;
 254     dxWorld                 *m_pswObjectsAllocWorld;
 255     dMutexGroupID           m_pmgStepperMutexGroup;
 256     dCallWaitID             m_pcwIslandsSteppingWait;
 257 };
 258
 259 struct dxWorldProcessIslandsInfo
 260 {
 261     void AssignInfo(sizeint islandcount, unsigned int const *islandsizes, dxBody *const *bodies, dxJoint *const *joints)
 262     {
 263         m_IslandCount = islandcount;
 264         m_pIslandSizes = islandsizes;
 265         m_pBodies = bodies;
 266         m_pJoints = joints;
 267     }
 268
 269     sizeint GetIslandsCount() const { return m_IslandCount; }
 270     unsigned int const *GetIslandSizes() const { return m_pIslandSizes; }
 271     dxBody *const *GetBodiesArray() const { return m_pBodies; }
 272     dxJoint *const *GetJointsArray() const { return m_pJoints; }
 273
 274 private:
 275     sizeint                  m_IslandCount;
 276     unsigned int const      *m_pIslandSizes;
 277     dxBody *const           *m_pBodies;
 278     dxJoint *const          *m_pJoints;
 279 };
 280
 281 struct dxStepperProcessingCallContext
 282 {
 283     dxStepperProcessingCallContext(dxWorld *world, dReal stepSize, unsigned stepperAllowedThreads,
 284         dxWorldProcessMemArena *stepperArena, dxBody *const *islandBodiesStart, dxJoint *const *islandJointsStart):
 285         m_world(world), m_stepSize(stepSize), m_stepperArena(stepperArena), m_finalReleasee(NULL),
 286         m_islandBodiesStart(islandBodiesStart), m_islandJointsStart(islandJointsStart), m_islandBodiesCount(0), m_islandJointsCount(0),
 287         m_stepperAllowedThreads(stepperAllowedThreads)
 288     {
 289     }
 290
 291     void AssignIslandSelection(dxBody *const *islandBodiesStart, dxJoint *const *islandJointsStart,
 292         unsigned islandBodiesCount, unsigned islandJointsCount)
 293     {
 294         m_islandBodiesStart = islandBodiesStart;
 295         m_islandJointsStart = islandJointsStart;
 296         m_islandBodiesCount = islandBodiesCount;
 297         m_islandJointsCount = islandJointsCount;
 298     }
 299
 300     dxBody *const *GetSelectedIslandBodiesEnd() const { return m_islandBodiesStart + m_islandBodiesCount; }
 301     dxJoint *const *GetSelectedIslandJointsEnd() const { return m_islandJointsStart + m_islandJointsCount; }
 302
 303     void AssignStepperCallFinalReleasee(dCallReleaseeID finalReleasee)
 304     {
 305         m_finalReleasee = finalReleasee;
 306     }
 307
 308     dxWorld                 *const m_world;
 309     dReal                   const m_stepSize;
 310     dxWorldProcessMemArena  *m_stepperArena;
 311     dCallReleaseeID         m_finalReleasee;
 312     dxBody *const           *m_islandBodiesStart;
 313     dxJoint *const          *m_islandJointsStart;
 314     unsigned                m_islandBodiesCount;
 315     unsigned                m_islandJointsCount;
 316     unsigned                m_stepperAllowedThreads;
 317 };
 318
 319 #define BEGIN_STATE_SAVE(memarena, state) void *state = memarena->SaveState();
 320 #define END_STATE_SAVE(memarena, state) memarena->RestoreState(state)
 321
 322 typedef void (*dstepper_fn_t) (const dxStepperProcessingCallContext *callContext);
 323 typedef unsigned (*dmaxcallcountestimate_fn_t) (unsigned activeThreadCount, unsigned allowedThreadCount);
 324
 325 bool dxProcessIslands (dxWorld *world, const dxWorldProcessIslandsInfo &islandsInfo,
 326                        dReal stepSize, dstepper_fn_t stepper, dmaxcallcountestimate_fn_t maxCallCountEstimator);
 327
 328
 329 typedef sizeint (*dmemestimate_fn_t) (dxBody * const *body, unsigned int nb,
 330                                      dxJoint * const *_joint, unsigned int _nj);
 331
 332 bool dxReallocateWorldProcessContext (dxWorld *world, dxWorldProcessIslandsInfo &islandsinfo,
 333                                       dReal stepsize, dmemestimate_fn_t stepperestimate);
 334
 335 dxWorldProcessMemArena *dxAllocateTemporaryWorldProcessMemArena(
 336     sizeint memreq, const dxWorldProcessMemoryManager *memmgr/*=NULL*/, const dxWorldProcessMemoryReserveInfo *reserveinfo/*=NULL*/);
 337 void dxFreeTemporaryWorldProcessMemArena(dxWorldProcessMemArena *arena);
 338
 339
 340 template<class ClassType>
 341 inline ClassType *AllocateOnDemand(ClassType *&pctStorage)
 342 {
 343     ClassType *pctCurrentInstance = pctStorage;
 344
 345     if (!pctCurrentInstance)
 346     {
 347         pctCurrentInstance = new ClassType();
 348         pctStorage = pctCurrentInstance;
 349     }
 350
 351     return pctCurrentInstance;
 352 }
 353
 354
 355 // World stepping working memory object
 356 class dxStepWorkingMemory:
 357     public dBase
 358 {
 359 public:
 360     dxStepWorkingMemory(): m_uiRefCount(1), m_ppcProcessingContext(NULL), m_priReserveInfo(NULL), m_pmmMemoryManager(NULL) {}
 361
 362 private:
 363     friend struct dBase; // To avoid GCC warning regarding private destructor
 364     ~dxStepWorkingMemory() // Use Release() instead
 365     {
 366         delete m_ppcProcessingContext;
 367         delete m_priReserveInfo;
 368         delete m_pmmMemoryManager;
 369     }
 370
 371 public:
 372     void Addref()
 373     {
 374         dIASSERT(~m_uiRefCount != 0);
 375         ++m_uiRefCount;
 376     }
 377
 378     void Release()
 379     {
 380         dIASSERT(m_uiRefCount != 0);
 381         if (--m_uiRefCount == 0)
 382         {
 383             delete this;
 384         }
 385     }
 386
 387 public:
 388     void CleanupMemory()
 389     {
 390         delete m_ppcProcessingContext;
 391         m_ppcProcessingContext = NULL;
 392     }
 393
 394     void CleanupWorldReferences(dxWorld *world)
 395     {
 396         if (m_ppcProcessingContext != NULL)
 397         {
 398             m_ppcProcessingContext->CleanupWorldReferences(world);
 399         }
 400     }
 401
 402 public:
 403     dxWorldProcessContext *SureGetWorldProcessingContext() { return AllocateOnDemand(m_ppcProcessingContext); }
 404     dxWorldProcessContext *GetWorldProcessingContext() const { return m_ppcProcessingContext; }
 405
 406     const dxWorldProcessMemoryReserveInfo *GetMemoryReserveInfo() const { return m_priReserveInfo; }
 407     const dxWorldProcessMemoryReserveInfo *SureGetMemoryReserveInfo() const { return m_priReserveInfo ? m_priReserveInfo : &g_WorldProcessDefaultReserveInfo; }
 408     void SetMemoryReserveInfo(float fReserveFactor, unsigned uiReserveMinimum)
 409     {
 410         if (m_priReserveInfo) { m_priReserveInfo->Assign(fReserveFactor, uiReserveMinimum); }
 411         else { m_priReserveInfo = new dxWorldProcessMemoryReserveInfo(fReserveFactor, uiReserveMinimum); }
 412     }
 413     void ResetMemoryReserveInfoToDefault()
 414     {
 415         if (m_priReserveInfo) { delete m_priReserveInfo; m_priReserveInfo = NULL; }
 416     }
 417
 418     const dxWorldProcessMemoryManager *GetMemoryManager() const { return m_pmmMemoryManager; }
 419     const dxWorldProcessMemoryManager *SureGetMemoryManager() const { return m_pmmMemoryManager ? m_pmmMemoryManager : &g_WorldProcessMallocMemoryManager; }
 420     void SetMemoryManager(dxWorldProcessMemoryManager::alloc_block_fn_t fnAlloc,
 421         dxWorldProcessMemoryManager::shrink_block_fn_t fnShrink,
 422         dxWorldProcessMemoryManager::free_block_fn_t fnFree)
 423     {
 424         if (m_pmmMemoryManager) { m_pmmMemoryManager->Assign(fnAlloc, fnShrink, fnFree); }
 425         else { m_pmmMemoryManager = new dxWorldProcessMemoryManager(fnAlloc, fnShrink, fnFree); }
 426     }
 427     void ResetMemoryManagerToDefault()
 428     {
 429         if (m_pmmMemoryManager) { delete m_pmmMemoryManager; m_pmmMemoryManager = NULL; }
 430     }
 431
 432 private:
 433     unsigned m_uiRefCount;
 434     dxWorldProcessContext *m_ppcProcessingContext;
 435     dxWorldProcessMemoryReserveInfo *m_priReserveInfo;
 436     dxWorldProcessMemoryManager *m_pmmMemoryManager;
 437 };
 438
 439
 440 #endif