bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionDispatch / btConvexConcaveCollisionAlgorithm.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
17 #include "btConvexConcaveCollisionAlgorithm.h"
18 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
19 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
20 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
21 #include "BulletCollision/CollisionShapes/btConcaveShape.h"
22 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
23 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
24 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
25 #include "BulletCollision/CollisionShapes/btSphereShape.h"
26 #include "LinearMath/btIDebugDraw.h"
27 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
28 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
29
30 btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
31 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
32 m_isSwapped(isSwapped),
33 m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped)
34 {
35 }
36
37 btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
38 {
39 }
40
41 void    btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray&      manifoldArray)
42 {
43         if (m_btConvexTriangleCallback.m_manifoldPtr)
44         {
45                 manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr);
46         }
47 }
48
49
50 btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher*  dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped):
51           m_dispatcher(dispatcher),
52         m_dispatchInfoPtr(0)
53 {
54         m_convexBodyWrap = isSwapped? body1Wrap:body0Wrap;
55         m_triBodyWrap = isSwapped? body0Wrap:body1Wrap;
56         
57           //
58           // create the manifold from the dispatcher 'manifold pool'
59           //
60           m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(),m_triBodyWrap->getCollisionObject());
61
62           clearCache();
63 }
64
65 btConvexTriangleCallback::~btConvexTriangleCallback()
66 {
67         clearCache();
68         m_dispatcher->releaseManifold( m_manifoldPtr );
69   
70 }
71   
72
73 void    btConvexTriangleCallback::clearCache()
74 {
75         m_dispatcher->clearManifold(m_manifoldPtr);
76 }
77
78
79
80 void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
81 {
82  
83         //just for debugging purposes
84         //printf("triangle %d",m_triangleCount++);
85
86
87         //aabb filter is already applied!       
88
89         btCollisionAlgorithmConstructionInfo ci;
90         ci.m_dispatcher1 = m_dispatcher;
91
92         //const btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
93
94
95 #if 0   
96         ///debug drawing of the overlapping triangles
97         if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
98         {
99                 btVector3 color(1,1,0);
100                 btTransform& tr = ob->getWorldTransform();
101                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
102                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
103                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
104         }
105 #endif
106         
107         if (m_convexBodyWrap->getCollisionShape()->isConvex())
108         {
109                 btTriangleShape tm(triangle[0],triangle[1],triangle[2]);        
110                 tm.setMargin(m_collisionMarginTriangle);
111                 
112                 
113                 btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform?
114                 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr);
115
116                 const btCollisionObjectWrapper* tmpWrap = 0;
117
118                 if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
119                 {
120                         tmpWrap = m_resultOut->getBody0Wrap();
121                         m_resultOut->setBody0Wrap(&triObWrap);
122                         m_resultOut->setShapeIdentifiersA(partId,triangleIndex);
123                 }
124                 else
125                 {
126                         tmpWrap = m_resultOut->getBody1Wrap();
127                         m_resultOut->setBody1Wrap(&triObWrap);
128                         m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
129                 }
130         
131                 colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut);
132
133                 if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
134                 {
135                         m_resultOut->setBody0Wrap(tmpWrap);
136                 } else
137                 {
138                         m_resultOut->setBody1Wrap(tmpWrap);
139                 }
140                 
141
142
143                 colAlgo->~btCollisionAlgorithm();
144                 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
145         }
146
147 }
148
149
150
151 void    btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
152 {
153         m_convexBodyWrap = convexBodyWrap;
154         m_triBodyWrap = triBodyWrap;
155
156         m_dispatchInfoPtr = &dispatchInfo;
157         m_collisionMarginTriangle = collisionMarginTriangle;
158         m_resultOut = resultOut;
159
160         //recalc aabbs
161         btTransform convexInTriangleSpace;
162         convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform();
163         const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
164         //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
165         convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
166         btScalar extraMargin = collisionMarginTriangle;
167         btVector3 extra(extraMargin,extraMargin,extraMargin);
168
169         m_aabbMax += extra;
170         m_aabbMin -= extra;
171         
172 }
173
174 void btConvexConcaveCollisionAlgorithm::clearCache()
175 {
176         m_btConvexTriangleCallback.clearCache();
177
178 }
179
180 void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
181 {
182         
183         
184         const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
185         const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
186
187         if (triBodyWrap->getCollisionShape()->isConcave())
188         {
189
190
191                 
192                 const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
193                 
194                 if (convexBodyWrap->getCollisionShape()->isConvex())
195                 {
196                         btScalar collisionMarginTriangle = concaveShape->getMargin();
197                                         
198                         resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
199                         m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
200
201                         m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
202
203                         concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
204                         
205                         resultOut->refreshContactPoints();
206
207                         m_btConvexTriangleCallback.clearWrapperData();
208         
209                 }
210         
211         }
212
213 }
214
215
216 btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
217 {
218         (void)resultOut;
219         (void)dispatchInfo;
220         btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
221         btCollisionObject* triBody = m_isSwapped ? body0 : body1;
222
223
224         //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
225
226         //only perform CCD above a certain threshold, this prevents blocking on the long run
227         //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
228         btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
229         if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
230         {
231                 return btScalar(1.);
232         }
233
234         //const btVector3& from = convexbody->m_worldTransform.getOrigin();
235         //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
236         //todo: only do if the motion exceeds the 'radius'
237
238         btTransform triInv = triBody->getWorldTransform().inverse();
239         btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
240         btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
241
242         struct LocalTriangleSphereCastCallback  : public btTriangleCallback
243         {
244                 btTransform m_ccdSphereFromTrans;
245                 btTransform m_ccdSphereToTrans;
246                 btTransform     m_meshTransform;
247
248                 btScalar        m_ccdSphereRadius;
249                 btScalar        m_hitFraction;
250         
251
252                 LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
253                         :m_ccdSphereFromTrans(from),
254                         m_ccdSphereToTrans(to),
255                         m_ccdSphereRadius(ccdSphereRadius),
256                         m_hitFraction(hitFraction)
257                 {                       
258                 }
259                 
260                 
261                 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
262                 {
263                         (void)partId;
264                         (void)triangleIndex;
265                         //do a swept sphere for now
266                         btTransform ident;
267                         ident.setIdentity();
268                         btConvexCast::CastResult castResult;
269                         castResult.m_fraction = m_hitFraction;
270                         btSphereShape   pointShape(m_ccdSphereRadius);
271                         btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
272                         btVoronoiSimplexSolver  simplexSolver;
273                         btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
274                         //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
275                         //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
276                         //local space?
277
278                         if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
279                                 ident,ident,castResult))
280                         {
281                                 if (m_hitFraction > castResult.m_fraction)
282                                         m_hitFraction = castResult.m_fraction;
283                         }
284
285                 }
286
287         };
288
289
290         
291
292         
293         if (triBody->getCollisionShape()->isConcave())
294         {
295                 btVector3 rayAabbMin = convexFromLocal.getOrigin();
296                 rayAabbMin.setMin(convexToLocal.getOrigin());
297                 btVector3 rayAabbMax = convexFromLocal.getOrigin();
298                 rayAabbMax.setMax(convexToLocal.getOrigin());
299                 btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
300                 rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
301                 rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
302
303                 btScalar curHitFraction = btScalar(1.); //is this available?
304                 LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
305                         convexbody->getCcdSweptSphereRadius(),curHitFraction);
306
307                 raycastCallback.m_hitFraction = convexbody->getHitFraction();
308
309                 btCollisionObject* concavebody = triBody;
310
311                 btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
312                 
313                 if (triangleMesh)
314                 {
315                         triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
316                 }
317         
318
319
320                 if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
321                 {
322                         convexbody->setHitFraction( raycastCallback.m_hitFraction);
323                         return raycastCallback.m_hitFraction;
324                 }
325         }
326
327         return btScalar(1.);
328
329 }