00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "SpuContactResult.h"
00017
00018
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
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
00099
00100
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
00109
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
00146 manifoldPtr->replaceContactPoint(newPt,insertIndex);
00147 return true;
00148
00149 } else
00150 {
00151
00152
00154
00155
00156
00157
00158
00159
00160
00161
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
00185
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
00200
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
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
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