Open Dynamics Engine

util.h

00001 /*************************************************************************
00002  *                                                                       *
00003  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
00004  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
00005  *                                                                       *
00006  * This library is free software; you can redistribute it and/or         *
00007  * modify it under the terms of EITHER:                                  *
00008  *   (1) The GNU Lesser General Public License as published by the Free  *
00009  *       Software Foundation; either version 2.1 of the License, or (at  *
00010  *       your option) any later version. The text of the GNU Lesser      *
00011  *       General Public License is included with this library in the     *
00012  *       file LICENSE.TXT.                                               *
00013  *   (2) The BSD-style license that is included with this library in     *
00014  *       the file LICENSE-BSD.TXT.                                       *
00015  *                                                                       *
00016  * This library is distributed in the hope that it will be useful,       *
00017  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00018  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
00019  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
00020  *                                                                       *
00021  *************************************************************************/
00022 
00023 #ifndef _ODE_UTIL_H_
00024 #define _ODE_UTIL_H_
00025 
00026 #include "objects.h"
00027 
00028 
00029 /* the efficient alignment. most platforms align data structures to some
00030  * number of bytes, but this is not always the most efficient alignment.
00031  * for example, many x86 compilers align to 4 bytes, but on a pentium it
00032  * is important to align doubles to 8 byte boundaries (for speed), and
00033  * the 4 floats in a SIMD register to 16 byte boundaries. many other
00034  * platforms have similar behavior. setting a larger alignment can waste
00035  * a (very) small amount of memory. NOTE: this number must be a power of
00036  * two. this is set to 16 by default.
00037  */
00038 #ifndef EFFICIENT_ALIGNMENT
00039 #define EFFICIENT_ALIGNMENT 16
00040 #endif
00041 
00042 
00043 /* utility */
00044 
00045 
00046 /* round something up to be a multiple of the EFFICIENT_ALIGNMENT */
00047 
00048 #define dEFFICIENT_SIZE(x) (((x)+(EFFICIENT_ALIGNMENT-1)) & ~((size_t)(EFFICIENT_ALIGNMENT-1)))
00049 #define dEFFICIENT_PTR(p) ((void *)dEFFICIENT_SIZE((size_t)(p)))
00050 #define dOFFSET_EFFICIENTLY(p, b) ((void *)((size_t)(p) + dEFFICIENT_SIZE(b)))
00051 
00052 /* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste
00053  * up to 15 bytes per allocation, depending on what alloca() returns.
00054  */
00055 
00056 #define dALLOCA16(n) \
00057   ((char*)dEFFICIENT_PTR(alloca((n)+(EFFICIENT_ALIGNMENT-1))))
00058 
00059 
00060 #ifndef SIZE_MAX
00061 #define SIZE_MAX  ((size_t)(-1))
00062 #endif
00063 
00064 void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize);
00065 void dxStepBody (dxBody *b, dReal h);
00066 
00067 
00068 struct dxWorldProcessMemoryManager:
00069   public dBase
00070 {
00071   typedef void *(*alloc_block_fn_t)(size_t block_size);
00072   typedef void *(*shrink_block_fn_t)(void *block_pointer, size_t block_current_size, size_t block_smaller_size);
00073   typedef void (*free_block_fn_t)(void *block_pointer, size_t block_current_size);
00074   
00075   dxWorldProcessMemoryManager(alloc_block_fn_t fnAlloc, shrink_block_fn_t fnShrink, free_block_fn_t fnFree)
00076   {
00077     Assign(fnAlloc, fnShrink, fnFree);
00078   }
00079 
00080   void Assign(alloc_block_fn_t fnAlloc, shrink_block_fn_t fnShrink, free_block_fn_t fnFree)
00081   {
00082     m_fnAlloc = fnAlloc;
00083     m_fnShrink = fnShrink;
00084     m_fnFree = fnFree;
00085   }
00086 
00087   alloc_block_fn_t m_fnAlloc;
00088   shrink_block_fn_t m_fnShrink;
00089   free_block_fn_t m_fnFree;
00090 };
00091 
00092 extern dxWorldProcessMemoryManager g_WorldProcessMallocMemoryManager;
00093 
00094 struct dxWorldProcessMemoryReserveInfo:
00095   public dBase
00096 {
00097   dxWorldProcessMemoryReserveInfo(float fReserveFactor, unsigned uiReserveMinimum)
00098   {
00099     Assign(fReserveFactor, uiReserveMinimum);
00100   }
00101 
00102   void Assign(float fReserveFactor, unsigned uiReserveMinimum)
00103   {
00104     m_fReserveFactor = fReserveFactor;
00105     m_uiReserveMinimum = uiReserveMinimum;
00106   }
00107 
00108   float m_fReserveFactor; // Use float as precision does not matter here
00109   unsigned m_uiReserveMinimum;
00110 };
00111 
00112 extern dxWorldProcessMemoryReserveInfo g_WorldProcessDefaultReserveInfo;
00113 
00114 
00115 class dxWorldProcessMemArena:
00116     private dBase // new/delete must not be called for this class
00117 {
00118 public:
00119 #define BUFFER_TO_ARENA_EXTRA (EFFICIENT_ALIGNMENT + dEFFICIENT_SIZE(sizeof(dxWorldProcessMemArena)))
00120   static bool IsArenaPossible(size_t nBufferSize)
00121   {
00122     return SIZE_MAX - BUFFER_TO_ARENA_EXTRA >= nBufferSize; // This ensures there will be no overflow
00123   }
00124 
00125   static size_t MakeBufferSize(size_t nArenaSize)
00126   {
00127     return nArenaSize - BUFFER_TO_ARENA_EXTRA;
00128   }
00129 
00130   static size_t MakeArenaSize(size_t nBufferSize)
00131   {
00132     return BUFFER_TO_ARENA_EXTRA + nBufferSize;
00133   }
00134 #undef BUFFER_TO_ARENA_EXTRA
00135 
00136   bool IsStructureValid() const
00137   {
00138     return m_pAllocBegin && m_pAllocEnd && m_pAllocBegin <= m_pAllocEnd && m_pAllocCurrent == m_pAllocBegin && m_pArenaBegin && m_pArenaBegin <= m_pAllocBegin; 
00139   }
00140 
00141   size_t GetMemorySize() const
00142   {
00143     return (size_t)m_pAllocEnd - (size_t)m_pAllocBegin;
00144   }
00145 
00146   void *SaveState() const
00147   {
00148     return m_pAllocCurrent;
00149   }
00150 
00151   void RestoreState(void *state)
00152   {
00153     m_pAllocCurrent = state;
00154   }
00155 
00156   void ResetState()
00157   {
00158     m_pAllocCurrent = m_pAllocBegin;
00159   }
00160 
00161   void *PeekBufferRemainder() const
00162   {
00163     return m_pAllocCurrent;
00164   }
00165 
00166   void *AllocateBlock(size_t size)
00167   {
00168     void *block = m_pAllocCurrent;
00169     m_pAllocCurrent = dOFFSET_EFFICIENTLY(block, size);
00170     dIASSERT(m_pAllocCurrent <= m_pAllocEnd);
00171     return block;
00172   }
00173 
00174   template<typename ElementType>
00175   ElementType *AllocateArray(size_t count)
00176   {
00177     return (ElementType *)AllocateBlock(count * sizeof(ElementType));
00178   }
00179 
00180   template<typename ElementType>
00181   void ShrinkArray(ElementType *arr, size_t oldcount, size_t newcount)
00182   {
00183     dIASSERT(newcount <= oldcount);
00184     dIASSERT(dOFFSET_EFFICIENTLY(arr, oldcount * sizeof(ElementType)) == m_pAllocCurrent);
00185     m_pAllocCurrent = dOFFSET_EFFICIENTLY(arr, newcount * sizeof(ElementType));
00186   }
00187 
00188 public:
00189   static dxWorldProcessMemArena *ReallocateMemArena (
00190     dxWorldProcessMemArena *oldarena, size_t memreq, 
00191     const dxWorldProcessMemoryManager *memmgr, float rsrvfactor, unsigned rsrvminimum);
00192   static void FreeMemArena (dxWorldProcessMemArena *arena);
00193 
00194 private:
00195   static size_t AdjustArenaSizeForReserveRequirements(size_t arenareq, float rsrvfactor, unsigned rsrvminimum);
00196 
00197 private:
00198   void *m_pAllocBegin;
00199   void *m_pAllocEnd;
00200   void *m_pAllocCurrent;
00201   void *m_pArenaBegin;
00202 
00203   const dxWorldProcessMemoryManager *m_pArenaMemMgr;
00204 };
00205 
00206 class dxWorldProcessContext:
00207   public dBase
00208 {
00209 public:
00210   dxWorldProcessContext();
00211   ~dxWorldProcessContext();
00212 
00213   bool IsStructureValid() const;
00214   void CleanupContext();
00215 
00216   dxWorldProcessMemArena *GetIslandsMemArena() const { return m_pmaIslandsArena; }
00217   dxWorldProcessMemArena *GetStepperMemArena() const { return m_pmaStepperArena; }
00218 
00219   dxWorldProcessMemArena *ReallocateIslandsMemArena(size_t nMemoryRequirement, 
00220     const dxWorldProcessMemoryManager *pmmMemortManager, float fReserveFactor, unsigned uiReserveMinimum);
00221   dxWorldProcessMemArena *ReallocateStepperMemArena(size_t nMemoryRequirement, 
00222     const dxWorldProcessMemoryManager *pmmMemortManager, float fReserveFactor, unsigned uiReserveMinimum);
00223 
00224 private:
00225   void SetIslandsMemArena(dxWorldProcessMemArena *pmaInstance) { m_pmaIslandsArena = pmaInstance; }
00226   void SetStepperMemArena(dxWorldProcessMemArena *pmaInstance) { m_pmaStepperArena = pmaInstance; }
00227 
00228 private:
00229   dxWorldProcessMemArena  *m_pmaIslandsArena;
00230   dxWorldProcessMemArena  *m_pmaStepperArena;
00231 };
00232 
00233 struct dxWorldProcessIslandsInfo
00234 {
00235   void AssignInfo(size_t islandcount, unsigned int const *islandsizes, dxBody *const *bodies, dxJoint *const *joints)
00236   {
00237     m_IslandCount = islandcount;
00238     m_pIslandSizes = islandsizes;
00239     m_pBodies = bodies;
00240     m_pJoints = joints;
00241   }
00242   
00243   size_t GetIslandsCount() const { return m_IslandCount; }
00244   unsigned int const *GetIslandSizes() const { return m_pIslandSizes; }
00245   dxBody *const *GetBodiesArray() const { return m_pBodies; }
00246   dxJoint *const *GetJointsArray() const { return m_pJoints; }
00247 
00248 private:
00249   size_t m_IslandCount;
00250   unsigned int const *m_pIslandSizes;
00251   dxBody *const *m_pBodies;
00252   dxJoint *const *m_pJoints;
00253 };
00254 
00255 
00256 
00257 #define BEGIN_STATE_SAVE(memarena, state) void *state = memarena->SaveState();
00258 #define END_STATE_SAVE(memarena, state) memarena->RestoreState(state)
00259 
00260 typedef void (*dstepper_fn_t) (dxWorldProcessMemArena *memarena, 
00261         dxWorld *world, dxBody * const *body, unsigned int nb,
00262         dxJoint * const *_joint, unsigned int _nj, dReal stepsize);
00263 
00264 void dxProcessIslands (dxWorld *world, const dxWorldProcessIslandsInfo &islandsinfo, dReal stepsize, dstepper_fn_t stepper);
00265 
00266 
00267 typedef size_t (*dmemestimate_fn_t) (dxBody * const *body, unsigned int nb, 
00268   dxJoint * const *_joint, unsigned int _nj);
00269 
00270 bool dxReallocateWorldProcessContext (dxWorld *world, dxWorldProcessIslandsInfo &islandsinfo, 
00271   dReal stepsize, dmemestimate_fn_t stepperestimate);
00272 void dxCleanupWorldProcessContext (dxWorld *world);
00273 
00274 dxWorldProcessMemArena *dxAllocateTemporaryWorldProcessMemArena(
00275   size_t memreq, const dxWorldProcessMemoryManager *memmgr/*=NULL*/, const dxWorldProcessMemoryReserveInfo *reserveinfo/*=NULL*/);
00276 void dxFreeTemporaryWorldProcessMemArena(dxWorldProcessMemArena *arena);
00277 
00278 
00279 
00280 template<class ClassType>
00281 inline ClassType *AllocateOnDemand(ClassType *&pctStorage)
00282 {
00283   ClassType *pctCurrentInstance = pctStorage;
00284 
00285   if (!pctCurrentInstance)
00286   {
00287     pctCurrentInstance = new ClassType();
00288     pctStorage = pctCurrentInstance;
00289   }
00290 
00291   return pctCurrentInstance;
00292 }
00293 
00294 
00295 // World stepping working memory object
00296 class dxStepWorkingMemory:
00297   public dBase
00298 {
00299 public:
00300   dxStepWorkingMemory(): m_uiRefCount(1), m_ppcProcessingContext(NULL), m_priReserveInfo(NULL), m_pmmMemoryManager(NULL) {}
00301 
00302 private:
00303   friend struct dBase; // To avoid GCC warning regarding private destructor
00304   ~dxStepWorkingMemory() // Use Release() instead
00305   {
00306     delete m_ppcProcessingContext;
00307     delete m_priReserveInfo;
00308     delete m_pmmMemoryManager;
00309   }
00310 
00311 public:
00312   void Addref()
00313   {
00314     dIASSERT(~m_uiRefCount != 0);
00315     ++m_uiRefCount;
00316   }
00317 
00318   void Release()
00319   {
00320     dIASSERT(m_uiRefCount != 0);
00321     if (--m_uiRefCount == 0)
00322     {
00323       delete this;
00324     }
00325   }
00326 
00327 public:
00328   void CleanupMemory()
00329   {
00330     delete m_ppcProcessingContext;
00331     m_ppcProcessingContext = NULL;
00332   }
00333 
00334 public: 
00335   dxWorldProcessContext *SureGetWorldProcessingContext() { return AllocateOnDemand(m_ppcProcessingContext); }
00336   dxWorldProcessContext *GetWorldProcessingContext() const { return m_ppcProcessingContext; }
00337 
00338   const dxWorldProcessMemoryReserveInfo *GetMemoryReserveInfo() const { return m_priReserveInfo; }
00339   const dxWorldProcessMemoryReserveInfo *SureGetMemoryReserveInfo() const { return m_priReserveInfo ? m_priReserveInfo : &g_WorldProcessDefaultReserveInfo; }
00340   void SetMemoryReserveInfo(float fReserveFactor, unsigned uiReserveMinimum)
00341   {
00342     if (m_priReserveInfo) { m_priReserveInfo->Assign(fReserveFactor, uiReserveMinimum); }
00343     else { m_priReserveInfo = new dxWorldProcessMemoryReserveInfo(fReserveFactor, uiReserveMinimum); }
00344   }
00345   void ResetMemoryReserveInfoToDefault()
00346   {
00347     if (m_priReserveInfo) { delete m_priReserveInfo; m_priReserveInfo = NULL; }
00348   }
00349 
00350   const dxWorldProcessMemoryManager *GetMemoryManager() const { return m_pmmMemoryManager; }
00351   const dxWorldProcessMemoryManager *SureGetMemoryManager() const { return m_pmmMemoryManager ? m_pmmMemoryManager : &g_WorldProcessMallocMemoryManager; }
00352   void SetMemoryManager(dxWorldProcessMemoryManager::alloc_block_fn_t fnAlloc, 
00353     dxWorldProcessMemoryManager::shrink_block_fn_t fnShrink, 
00354     dxWorldProcessMemoryManager::free_block_fn_t fnFree) 
00355   {
00356     if (m_pmmMemoryManager) { m_pmmMemoryManager->Assign(fnAlloc, fnShrink, fnFree); }
00357     else { m_pmmMemoryManager = new dxWorldProcessMemoryManager(fnAlloc, fnShrink, fnFree); }
00358   }
00359   void ResetMemoryManagerToDefault()
00360   {
00361     if (m_pmmMemoryManager) { delete m_pmmMemoryManager; m_pmmMemoryManager = NULL; }
00362   }
00363 
00364 private:
00365   unsigned m_uiRefCount;
00366   dxWorldProcessContext *m_ppcProcessingContext;
00367   dxWorldProcessMemoryReserveInfo *m_priReserveInfo;
00368   dxWorldProcessMemoryManager *m_pmmMemoryManager;
00369 };
00370 
00371 
00372 #endif