Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @ 8331

Last change on this file since 8331 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

  • Property svn:eol-style set to native
File size: 11.1 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
29btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
30: btActivatingCollisionAlgorithm(ci,body0,body1),
31m_isSwapped(isSwapped),
32m_btConvexTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped)
33{
34}
35
36btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
37{
38}
39
40void    btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray&      manifoldArray)
41{
42        if (m_btConvexTriangleCallback.m_manifoldPtr)
43        {
44                manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr);
45        }
46}
47
48
49btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher*  dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
50          m_dispatcher(dispatcher),
51        m_dispatchInfoPtr(0)
52{
53        m_convexBody = isSwapped? body1:body0;
54        m_triBody = isSwapped? body0:body1;
55       
56          //
57          // create the manifold from the dispatcher 'manifold pool'
58          //
59          m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
60
61          clearCache();
62}
63
64btConvexTriangleCallback::~btConvexTriangleCallback()
65{
66        clearCache();
67        m_dispatcher->releaseManifold( m_manifoldPtr );
68 
69}
70 
71
72void    btConvexTriangleCallback::clearCache()
73{
74        m_dispatcher->clearManifold(m_manifoldPtr);
75}
76
77
78
79void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
80{
81 
82        //just for debugging purposes
83        //printf("triangle %d",m_triangleCount++);
84
85
86        //aabb filter is already applied!       
87
88        btCollisionAlgorithmConstructionInfo ci;
89        ci.m_dispatcher1 = m_dispatcher;
90
91        btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
92
93
94       
95        ///debug drawing of the overlapping triangles
96        if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
97        {
98                btVector3 color(255,255,0);
99                btTransform& tr = ob->getWorldTransform();
100                m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
101                m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
102                m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
103
104                //btVector3 center = triangle[0] + triangle[1]+triangle[2];
105                //center *= btScalar(0.333333);
106                //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color);
107                //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color);
108                //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color);
109
110        }
111
112
113        //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
114       
115        if (m_convexBody->getCollisionShape()->isConvex())
116        {
117                btTriangleShape tm(triangle[0],triangle[1],triangle[2]);       
118                tm.setMargin(m_collisionMarginTriangle);
119               
120                btCollisionShape* tmpShape = ob->getCollisionShape();
121                ob->internalSetTemporaryCollisionShape( &tm );
122               
123                btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr);
124                ///this should use the btDispatcher, so the actual registered algorithm is used
125                //              btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
126
127                m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex);
128        //      cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
129//              cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
130                colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
131                colAlgo->~btCollisionAlgorithm();
132                ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
133                ob->internalSetTemporaryCollisionShape( tmpShape);
134        }
135
136
137}
138
139
140
141void    btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
142{
143        m_dispatchInfoPtr = &dispatchInfo;
144        m_collisionMarginTriangle = collisionMarginTriangle;
145        m_resultOut = resultOut;
146
147        //recalc aabbs
148        btTransform convexInTriangleSpace;
149        convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * m_convexBody->getWorldTransform();
150        btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->getCollisionShape());
151        //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
152        convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
153        btScalar extraMargin = collisionMarginTriangle;
154        btVector3 extra(extraMargin,extraMargin,extraMargin);
155
156        m_aabbMax += extra;
157        m_aabbMin -= extra;
158       
159}
160
161void btConvexConcaveCollisionAlgorithm::clearCache()
162{
163        m_btConvexTriangleCallback.clearCache();
164
165}
166
167void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
168{
169       
170       
171        btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
172        btCollisionObject* triBody = m_isSwapped ? body0 : body1;
173
174        if (triBody->getCollisionShape()->isConcave())
175        {
176
177
178                btCollisionObject*      triOb = triBody;
179                btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
180               
181                if (convexBody->getCollisionShape()->isConvex())
182                {
183                        btScalar collisionMarginTriangle = concaveShape->getMargin();
184                                       
185                        resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
186                        m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
187
188                        //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
189                        //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
190
191                        m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
192
193                        concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
194                       
195                        resultOut->refreshContactPoints();
196       
197                }
198       
199        }
200
201}
202
203
204btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
205{
206        (void)resultOut;
207        (void)dispatchInfo;
208        btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
209        btCollisionObject* triBody = m_isSwapped ? body0 : body1;
210
211
212        //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
213
214        //only perform CCD above a certain threshold, this prevents blocking on the long run
215        //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
216        btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
217        if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
218        {
219                return btScalar(1.);
220        }
221
222        //const btVector3& from = convexbody->m_worldTransform.getOrigin();
223        //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
224        //todo: only do if the motion exceeds the 'radius'
225
226        btTransform triInv = triBody->getWorldTransform().inverse();
227        btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
228        btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
229
230        struct LocalTriangleSphereCastCallback  : public btTriangleCallback
231        {
232                btTransform m_ccdSphereFromTrans;
233                btTransform m_ccdSphereToTrans;
234                btTransform     m_meshTransform;
235
236                btScalar        m_ccdSphereRadius;
237                btScalar        m_hitFraction;
238       
239
240                LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
241                        :m_ccdSphereFromTrans(from),
242                        m_ccdSphereToTrans(to),
243                        m_ccdSphereRadius(ccdSphereRadius),
244                        m_hitFraction(hitFraction)
245                {                       
246                }
247               
248               
249                virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
250                {
251                        (void)partId;
252                        (void)triangleIndex;
253                        //do a swept sphere for now
254                        btTransform ident;
255                        ident.setIdentity();
256                        btConvexCast::CastResult castResult;
257                        castResult.m_fraction = m_hitFraction;
258                        btSphereShape   pointShape(m_ccdSphereRadius);
259                        btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
260                        btVoronoiSimplexSolver  simplexSolver;
261                        btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
262                        //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
263                        //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
264                        //local space?
265
266                        if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
267                                ident,ident,castResult))
268                        {
269                                if (m_hitFraction > castResult.m_fraction)
270                                        m_hitFraction = castResult.m_fraction;
271                        }
272
273                }
274
275        };
276
277
278       
279
280       
281        if (triBody->getCollisionShape()->isConcave())
282        {
283                btVector3 rayAabbMin = convexFromLocal.getOrigin();
284                rayAabbMin.setMin(convexToLocal.getOrigin());
285                btVector3 rayAabbMax = convexFromLocal.getOrigin();
286                rayAabbMax.setMax(convexToLocal.getOrigin());
287                btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
288                rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
289                rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
290
291                btScalar curHitFraction = btScalar(1.); //is this available?
292                LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
293                        convexbody->getCcdSweptSphereRadius(),curHitFraction);
294
295                raycastCallback.m_hitFraction = convexbody->getHitFraction();
296
297                btCollisionObject* concavebody = triBody;
298
299                btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
300               
301                if (triangleMesh)
302                {
303                        triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
304                }
305       
306
307
308                if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
309                {
310                        convexbody->setHitFraction( raycastCallback.m_hitFraction);
311                        return raycastCallback.m_hitFraction;
312                }
313        }
314
315        return btScalar(1.);
316
317}
Note: See TracBrowser for help on using the repository browser.