== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / 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         btVector3 pointA;
109         btVector3 localA;
110         btVector3 localB;
111         btVector3 normal;
112
113
114         if (isSwapped)
115         {
116                 normal = normalOnBInWorld * -1;
117                 pointA = pointInWorld + normal * depth;
118                 localA = transA.invXform(pointA );
119                 localB = transB.invXform(pointInWorld);
120         }
121         else
122         {
123                 normal = normalOnBInWorld;
124                 pointA = pointInWorld + normal * depth;
125                 localA = transA.invXform(pointA );
126                 localB = transB.invXform(pointInWorld);
127         }
128
129         btManifoldPoint newPt(localA,localB,normal,depth);
130         newPt.m_positionWorldOnA = pointA;
131         newPt.m_positionWorldOnB = pointInWorld;
132
133         newPt.m_combinedFriction = combinedFriction;
134         newPt.m_combinedRestitution = combinedRestitution;
135
136
137         int insertIndex = manifoldPtr->getCacheEntry(newPt);
138         if (insertIndex >= 0)
139         {
140                 // we need to replace the current contact point, otherwise small errors will accumulate (spheres start rolling etc)
141                 manifoldPtr->replaceContactPoint(newPt,insertIndex);
142                 return true;
143                 
144         } else
145         {
146
147                 /*
148                 ///@todo: SPU callbacks, either immediate (local on the SPU), or deferred
149                 //User can override friction and/or restitution
150                 if (gContactAddedCallback &&
151                         //and if either of the two bodies requires custom material
152                          ((m_body0->m_collisionFlags & btCollisionObject::customMaterialCallback) ||
153                            (m_body1->m_collisionFlags & btCollisionObject::customMaterialCallback)))
154                 {
155                         //experimental feature info, for per-triangle material etc.
156                         (*gContactAddedCallback)(newPt,m_body0,m_partId0,m_index0,m_body1,m_partId1,m_index1);
157                 }
158                 */
159                 manifoldPtr->addManifoldPoint(newPt);
160                 return true;
161
162         }
163         return false;
164         
165 }
166
167
168 void SpuContactResult::writeDoubleBufferedManifold(btPersistentManifold* lsManifold, btPersistentManifold* mmManifold)
169 {
170         ///only write back the contact information on SPU. Other platforms avoid copying, and use the data in-place
171         ///see SpuFakeDma.cpp 'cellDmaLargeGetReadOnly'
172 #if defined (__SPU__) || defined (USE_LIBSPE2)
173     memcpy(g_manifoldDmaExport.getFront(),lsManifold,sizeof(btPersistentManifold));
174
175     g_manifoldDmaExport.swapBuffers();
176     ppu_address_t mmAddr = (ppu_address_t)mmManifold;
177     g_manifoldDmaExport.backBufferDmaPut(mmAddr, sizeof(btPersistentManifold), DMA_TAG(9));
178         // 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?
179         //no, the swapBuffers does the wait
180 #endif
181 }
182
183 void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
184 {
185 #ifdef DEBUG_SPU_COLLISION_DETECTION
186         spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth);
187         spu_printf("*** normal = %f,%f,%f\n",normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ());
188         spu_printf("*** position = %f,%f,%f\n",pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ());
189 #endif //DEBUG_SPU_COLLISION_DETECTION
190         
191
192 #ifdef DEBUG_SPU_COLLISION_DETECTION
193  //   int sman = sizeof(rage::phManifold);
194 //      spu_printf("sizeof_manifold = %i\n",sman);
195 #endif //DEBUG_SPU_COLLISION_DETECTION
196
197         btPersistentManifold* localManifold = m_spuManifold;
198
199         btVector3       normalB(normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ());
200         btVector3       pointWrld(pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ());
201
202         //process the contact point
203         const bool retVal = ManifoldResultAddContactPoint(normalB,
204                 pointWrld,
205                 depth,
206                 localManifold,
207                 m_rootWorldTransform0,
208                 m_rootWorldTransform1,
209                 m_combinedFriction,
210                 m_combinedRestitution,
211                 m_isSwapped);
212         m_RequiresWriteBack = m_RequiresWriteBack || retVal;
213 }
214
215 void SpuContactResult::flush()
216 {
217
218         if (m_spuManifold && m_spuManifold->getNumContacts())
219         {
220                 m_spuManifold->refreshContactPoints(m_rootWorldTransform0,m_rootWorldTransform1);
221                 m_RequiresWriteBack = true;
222         }
223
224
225         if (m_RequiresWriteBack)
226         {
227 #ifdef DEBUG_SPU_COLLISION_DETECTION
228                 spu_printf("SPU: Start SpuContactResult::flush (Put) DMA\n");
229                 spu_printf("Num contacts:%d\n", m_spuManifold->getNumContacts());
230                 spu_printf("Manifold address: %llu\n", m_manifoldAddress);
231 #endif //DEBUG_SPU_COLLISION_DETECTION
232         //      spu_printf("writeDoubleBufferedManifold\n");
233                 writeDoubleBufferedManifold(m_spuManifold, (btPersistentManifold*)m_manifoldAddress);
234 #ifdef DEBUG_SPU_COLLISION_DETECTION
235                 spu_printf("SPU: Finished (Put) DMA\n");
236 #endif //DEBUG_SPU_COLLISION_DETECTION
237         }
238         m_spuManifold = NULL;
239         m_RequiresWriteBack = false;
240 }
241
242