src/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuContactResult.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   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 #include "SpuContactResult.h"
  17
  18 //#define DEBUG_SPU_COLLISION_DETECTION 1
  19
  20 #ifdef DEBUG_SPU_COLLISION_DETECTION
  21 #ifndef __SPU__
  22 #include <stdio.h>
  23 #define spu_printf printf
  24 #endif
  25 #endif //DEBUG_SPU_COLLISION_DETECTION
  26
  27 SpuContactResult::SpuContactResult()
  28 {
  29         m_manifoldAddress = 0;
  30         m_spuManifold = NULL;
  31         m_RequiresWriteBack = false;
  32 }
  33
  34  SpuContactResult::~SpuContactResult()
  35 {
  36         g_manifoldDmaExport.swapBuffers();
  37 }
  38
  39         ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
  40 inline btScalar calculateCombinedFriction(btScalar friction0,btScalar friction1)
  41 {
  42         btScalar friction = friction0*friction1;
  43
  44         const btScalar MAX_FRICTION  = btScalar(10.);
  45
  46         if (friction < -MAX_FRICTION)
  47                 friction = -MAX_FRICTION;
  48         if (friction > MAX_FRICTION)
  49                 friction = MAX_FRICTION;
  50         return friction;
  51
  52 }
  53
  54 inline btScalar calculateCombinedRestitution(btScalar restitution0,btScalar restitution1)
  55 {
  56         return restitution0*restitution1;
  57 }
  58
  59
  60
  61  void   SpuContactResult::setContactInfo(btPersistentManifold* spuManifold, ppu_address_t       manifoldAddress,const btTransform& worldTrans0,const btTransform& worldTrans1, btScalar restitution0,btScalar restitution1, btScalar friction0,btScalar friction1, bool isSwapped)
  62  {
  63         //spu_printf("SpuContactResult::setContactInfo ManifoldAddress: %lu\n", manifoldAddress);
  64         m_rootWorldTransform0 = worldTrans0;
  65         m_rootWorldTransform1 = worldTrans1;
  66         m_manifoldAddress = manifoldAddress;
  67         m_spuManifold = spuManifold;
  68
  69         m_combinedFriction = calculateCombinedFriction(friction0,friction1);
  70         m_combinedRestitution = calculateCombinedRestitution(restitution0,restitution1);
  71         m_isSwapped = isSwapped;
  72  }
  73
  74  void SpuContactResult::setShapeIdentifiersA(int partId0,int index0)
  75  {
  76
  77  }
  78
  79  void SpuContactResult::setShapeIdentifiersB(int partId1,int index1)
  80  {
  81
  82  }
  83
  84
  85
  86  ///return true if it requires a dma transfer back
  87 bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld,
  88                                                                    const btVector3& pointInWorld,
  89                                                                    float depth,
  90                                                                    btPersistentManifold* manifoldPtr,
  91                                                                    btTransform& transA,
  92                                                                    btTransform& transB,
  93                                                                         btScalar        combinedFriction,
  94                                                                         btScalar        combinedRestitution,
  95                                                                    bool isSwapped)
  96 {
  97
  98 //      float contactTreshold = manifoldPtr->getContactBreakingThreshold();
  99
 100         //spu_printf("SPU: add contactpoint, depth:%f, contactTreshold %f, manifoldPtr %llx\n",depth,contactTreshold,manifoldPtr);
 101
 102 #ifdef DEBUG_SPU_COLLISION_DETECTION
 103         spu_printf("SPU: contactTreshold %f\n",contactTreshold);
 104 #endif //DEBUG_SPU_COLLISION_DETECTION
 105         if (depth > manifoldPtr->getContactBreakingThreshold())
 106                 return false;
 107
 108         //if (depth > manifoldPtr->getContactProcessingThreshold())
 109         //      return false;
 110
 111
 112
 113         btVector3 pointA;
 114         btVector3 localA;
 115         btVector3 localB;
 116         btVector3 normal;
 117
 118
 119         if (isSwapped)
 120         {
 121                 normal = normalOnBInWorld * -1;
 122                 pointA = pointInWorld + normal * depth;
 123                 localA = transA.invXform(pointA );
 124                 localB = transB.invXform(pointInWorld);
 125         }
 126         else
 127         {
 128                 normal = normalOnBInWorld;
 129                 pointA = pointInWorld + normal * depth;
 130                 localA = transA.invXform(pointA );
 131                 localB = transB.invXform(pointInWorld);
 132         }
 133
 134         btManifoldPoint newPt(localA,localB,normal,depth);
 135         newPt.m_positionWorldOnA = pointA;
 136         newPt.m_positionWorldOnB = pointInWorld;
 137
 138         newPt.m_combinedFriction = combinedFriction;
 139         newPt.m_combinedRestitution = combinedRestitution;
 140
 141
 142         int insertIndex = manifoldPtr->getCacheEntry(newPt);
 143         if (insertIndex >= 0)
 144         {
 145                 // we need to replace the current contact point, otherwise small errors will accumulate (spheres start rolling etc)
 146                 manifoldPtr->replaceContactPoint(newPt,insertIndex);
 147                 return true;
 148
 149         } else
 150         {
 151
 152                 /*
 153                 ///@todo: SPU callbacks, either immediate (local on the SPU), or deferred
 154                 //User can override friction and/or restitution
 155                 if (gContactAddedCallback &&
 156                         //and if either of the two bodies requires custom material
 157                          ((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) ||
 158                            (m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback)))
 159                 {
 160                         //experimental feature info, for per-triangle material etc.
 161                         (*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1);
 162                 }
 163                 */
 164
 165                 manifoldPtr->addManifoldPoint(newPt);
 166                 return true;
 167
 168         }
 169         return false;
 170
 171 }
 172
 173
 174 void SpuContactResult::writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold)
 175 {
 176         ///only write back the contact information on SPU. Other platforms avoid copying, and use the data in-place
 177         ///see SpuFakeDma.cpp 'cellDmaLargeGetReadOnly'
 178 #if defined (__SPU__) || defined (USE_LIBSPE2)
 179     memcpy(g_manifoldDmaExport.getFront(),lsManifold,sizeof(btPersistentManifold));
 180
 181     g_manifoldDmaExport.swapBuffers();
 182     ppu_address_t mmAddr = (ppu_address_t)mmManifold;
 183     g_manifoldDmaExport.backBufferDmaPut(mmAddr, sizeof(btPersistentManifold), DMA_TAG(9));
 184         // Should there be any kind of wait here?  What if somebody tries to use this tag again?  What if we call this function again really soon?
 185         //no, the swapBuffers does the wait
 186 #endif
 187 }
 188
 189 void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
 190 {
 191 #ifdef DEBUG_SPU_COLLISION_DETECTION
 192         spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth);
 193         spu_printf("*** normal = %f,%f,%f\n",normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ());
 194         spu_printf("*** position = %f,%f,%f\n",pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ());
 195 #endif //DEBUG_SPU_COLLISION_DETECTION
 196
 197
 198 #ifdef DEBUG_SPU_COLLISION_DETECTION
 199  //   int sman = sizeof(rage::phManifold);
 200 //      spu_printf("sizeof_manifold = %i\n",sman);
 201 #endif //DEBUG_SPU_COLLISION_DETECTION
 202
 203         btPersistentManifold* localManifold = m_spuManifold;
 204
 205         btVector3       normalB(normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ());
 206         btVector3       pointWrld(pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ());
 207
 208         //process the contact point
 209         const bool retVal = ManifoldResultAddContactPoint(normalB,
 210                 pointWrld,
 211                 depth,
 212                 localManifold,
 213                 m_rootWorldTransform0,
 214                 m_rootWorldTransform1,
 215                 m_combinedFriction,
 216                 m_combinedRestitution,
 217                 m_isSwapped);
 218         m_RequiresWriteBack = m_RequiresWriteBack || retVal;
 219 }
 220
 221 void SpuContactResult::flush()
 222 {
 223
 224         if (m_spuManifold && m_spuManifold->getNumContacts())
 225         {
 226                 m_spuManifold->refreshContactPoints(m_rootWorldTransform0,m_rootWorldTransform1);
 227                 m_RequiresWriteBack = true;
 228         }
 229
 230
 231         if (m_RequiresWriteBack)
 232         {
 233 #ifdef DEBUG_SPU_COLLISION_DETECTION
 234                 spu_printf("SPU: Start SpuContactResult::flush (Put) DMA\n");
 235                 spu_printf("Num contacts:%d\n", m_spuManifold->getNumContacts());
 236                 spu_printf("Manifold address: %llu\n", m_manifoldAddress);
 237 #endif //DEBUG_SPU_COLLISION_DETECTION
 238         //      spu_printf("writeDoubleBufferedManifold\n");
 239                 writeDoubleBufferedManifold(m_spuManifold, (btPersistentManifold*)m_manifoldAddress);
 240 #ifdef DEBUG_SPU_COLLISION_DETECTION
 241                 spu_printf("SPU: Finished (Put) DMA\n");
 242 #endif //DEBUG_SPU_COLLISION_DETECTION
 243         }
 244         m_spuManifold = NULL;
 245         m_RequiresWriteBack = false;
 246 }
 247
 248