SpuContactResult.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #include "SpuContactResult.h"
00017 
00018 //#define DEBUG_SPU_COLLISION_DETECTION 1
00019 
00020 #ifdef DEBUG_SPU_COLLISION_DETECTION
00021 #ifndef __SPU__
00022 #include <stdio.h>
00023 #define spu_printf printf
00024 #endif
00025 #endif //DEBUG_SPU_COLLISION_DETECTION
00026 
00027 SpuContactResult::SpuContactResult()
00028 {
00029         m_manifoldAddress = 0;
00030         m_spuManifold = NULL;
00031         m_RequiresWriteBack = false;
00032 }
00033 
00034  SpuContactResult::~SpuContactResult()
00035 {
00036         g_manifoldDmaExport.swapBuffers();
00037 }
00038 
00040 inline btScalar calculateCombinedFriction(btScalar friction0,btScalar friction1)
00041 {
00042         btScalar friction = friction0*friction1;
00043 
00044         const btScalar MAX_FRICTION  = btScalar(10.);
00045 
00046         if (friction < -MAX_FRICTION)
00047                 friction = -MAX_FRICTION;
00048         if (friction > MAX_FRICTION)
00049                 friction = MAX_FRICTION;
00050         return friction;
00051 
00052 }
00053 
00054 inline btScalar calculateCombinedRestitution(btScalar restitution0,btScalar restitution1)
00055 {
00056         return restitution0*restitution1;
00057 }
00058 
00059 
00060 
00061  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)
00062  {
00063         //spu_printf("SpuContactResult::setContactInfo ManifoldAddress: %lu\n", manifoldAddress);
00064         m_rootWorldTransform0 = worldTrans0;
00065         m_rootWorldTransform1 = worldTrans1;
00066         m_manifoldAddress = manifoldAddress;    
00067         m_spuManifold = spuManifold;
00068 
00069         m_combinedFriction = calculateCombinedFriction(friction0,friction1);
00070         m_combinedRestitution = calculateCombinedRestitution(restitution0,restitution1);
00071         m_isSwapped = isSwapped;
00072  }
00073 
00074  void SpuContactResult::setShapeIdentifiersA(int partId0,int index0)
00075  {
00076         
00077  }
00078 
00079  void SpuContactResult::setShapeIdentifiersB(int partId1,int index1)
00080  {
00081         
00082  }
00083 
00084 
00085 
00087 bool ManifoldResultAddContactPoint(const btVector3& normalOnBInWorld,
00088                                                                    const btVector3& pointInWorld,
00089                                                                    float depth,
00090                                                                    btPersistentManifold* manifoldPtr,
00091                                                                    btTransform& transA,
00092                                                                    btTransform& transB,
00093                                                                         btScalar        combinedFriction,
00094                                                                         btScalar        combinedRestitution,
00095                                                                    bool isSwapped)
00096 {
00097         
00098 //      float contactTreshold = manifoldPtr->getContactBreakingThreshold();
00099 
00100         //spu_printf("SPU: add contactpoint, depth:%f, contactTreshold %f, manifoldPtr %llx\n",depth,contactTreshold,manifoldPtr);
00101 
00102 #ifdef DEBUG_SPU_COLLISION_DETECTION
00103         spu_printf("SPU: contactTreshold %f\n",contactTreshold);
00104 #endif //DEBUG_SPU_COLLISION_DETECTION
00105         if (depth > manifoldPtr->getContactBreakingThreshold())
00106                 return false;
00107 
00108         //if (depth > manifoldPtr->getContactProcessingThreshold())
00109         //      return false;
00110 
00111 
00112 
00113         btVector3 pointA;
00114         btVector3 localA;
00115         btVector3 localB;
00116         btVector3 normal;
00117 
00118 
00119         if (isSwapped)
00120         {
00121                 normal = normalOnBInWorld * -1;
00122                 pointA = pointInWorld + normal * depth;
00123                 localA = transA.invXform(pointA );
00124                 localB = transB.invXform(pointInWorld);
00125         }
00126         else
00127         {
00128                 normal = normalOnBInWorld;
00129                 pointA = pointInWorld + normal * depth;
00130                 localA = transA.invXform(pointA );
00131                 localB = transB.invXform(pointInWorld);
00132         }
00133 
00134         btManifoldPoint newPt(localA,localB,normal,depth);
00135         newPt.m_positionWorldOnA = pointA;
00136         newPt.m_positionWorldOnB = pointInWorld;
00137 
00138         newPt.m_combinedFriction = combinedFriction;
00139         newPt.m_combinedRestitution = combinedRestitution;
00140 
00141 
00142         int insertIndex = manifoldPtr->getCacheEntry(newPt);
00143         if (insertIndex >= 0)
00144         {
00145                 // we need to replace the current contact point, otherwise small errors will accumulate (spheres start rolling etc)
00146                 manifoldPtr->replaceContactPoint(newPt,insertIndex);
00147                 return true;
00148                 
00149         } else
00150         {
00151 
00152                 /*
00154                 //User can override friction and/or restitution
00155                 if (gContactAddedCallback &&
00156                         //and if either of the two bodies requires custom material
00157                          ((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) ||
00158                            (m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback)))
00159                 {
00160                         //experimental feature info, for per-triangle material etc.
00161                         (*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1);
00162                 }
00163                 */
00164 
00165                 manifoldPtr->addManifoldPoint(newPt);
00166                 return true;
00167 
00168         }
00169         return false;
00170         
00171 }
00172 
00173 
00174 void SpuContactResult::writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold)
00175 {
00178 #if defined (__SPU__) || defined (USE_LIBSPE2)
00179     memcpy(g_manifoldDmaExport.getFront(),lsManifold,sizeof(btPersistentManifold));
00180 
00181     g_manifoldDmaExport.swapBuffers();
00182     ppu_address_t mmAddr = (ppu_address_t)mmManifold;
00183     g_manifoldDmaExport.backBufferDmaPut(mmAddr, sizeof(btPersistentManifold), DMA_TAG(9));
00184         // 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?
00185         //no, the swapBuffers does the wait
00186 #endif
00187 }
00188 
00189 void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
00190 {
00191 #ifdef DEBUG_SPU_COLLISION_DETECTION
00192         spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth);
00193         spu_printf("*** normal = %f,%f,%f\n",normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ());
00194         spu_printf("*** position = %f,%f,%f\n",pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ());
00195 #endif //DEBUG_SPU_COLLISION_DETECTION
00196         
00197 
00198 #ifdef DEBUG_SPU_COLLISION_DETECTION
00199  //   int sman = sizeof(rage::phManifold);
00200 //      spu_printf("sizeof_manifold = %i\n",sman);
00201 #endif //DEBUG_SPU_COLLISION_DETECTION
00202 
00203         btPersistentManifold* localManifold = m_spuManifold;
00204 
00205         btVector3       normalB(normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ());
00206         btVector3       pointWrld(pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ());
00207 
00208         //process the contact point
00209         const bool retVal = ManifoldResultAddContactPoint(normalB,
00210                 pointWrld,
00211                 depth,
00212                 localManifold,
00213                 m_rootWorldTransform0,
00214                 m_rootWorldTransform1,
00215                 m_combinedFriction,
00216                 m_combinedRestitution,
00217                 m_isSwapped);
00218         m_RequiresWriteBack = m_RequiresWriteBack || retVal;
00219 }
00220 
00221 void SpuContactResult::flush()
00222 {
00223 
00224         if (m_spuManifold && m_spuManifold->getNumContacts())
00225         {
00226                 m_spuManifold->refreshContactPoints(m_rootWorldTransform0,m_rootWorldTransform1);
00227                 m_RequiresWriteBack = true;
00228         }
00229 
00230 
00231         if (m_RequiresWriteBack)
00232         {
00233 #ifdef DEBUG_SPU_COLLISION_DETECTION
00234                 spu_printf("SPU: Start SpuContactResult::flush (Put) DMA\n");
00235                 spu_printf("Num contacts:%d\n", m_spuManifold->getNumContacts());
00236                 spu_printf("Manifold address: %llu\n", m_manifoldAddress);
00237 #endif //DEBUG_SPU_COLLISION_DETECTION
00238         //      spu_printf("writeDoubleBufferedManifold\n");
00239                 writeDoubleBufferedManifold(m_spuManifold, (btPersistentManifold*)m_manifoldAddress);
00240 #ifdef DEBUG_SPU_COLLISION_DETECTION
00241                 spu_printf("SPU: Finished (Put) DMA\n");
00242 #endif //DEBUG_SPU_COLLISION_DETECTION
00243         }
00244         m_spuManifold = NULL;
00245         m_RequiresWriteBack = false;
00246 }
00247 
00248