Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/SuperOrxoBros_HS18/SuperOrxoBros_HS18/src/external/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @ 12175

Last change on this file since 12175 was 12175, checked in by siramesh, 5 years ago

Super Orxo Bros (Sidharth Ramesh, Nisa Balta, Jeff Ren)

File size: 10.5 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#if 0   
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(1,1,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#endif
105       
106        if (m_convexBody->getCollisionShape()->isConvex())
107        {
108                btTriangleShape tm(triangle[0],triangle[1],triangle[2]);       
109                tm.setMargin(m_collisionMarginTriangle);
110               
111                btCollisionShape* tmpShape = ob->getCollisionShape();
112                ob->internalSetTemporaryCollisionShape( &tm );
113
114                btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr);
115
116                if (m_resultOut->getBody0Internal() == m_triBody)
117                {
118                        m_resultOut->setShapeIdentifiersA(partId,triangleIndex);
119                }
120                else
121                {
122                        m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
123                }
124       
125                colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
126                colAlgo->~btCollisionAlgorithm();
127                ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
128                ob->internalSetTemporaryCollisionShape( tmpShape);
129        }
130
131
132}
133
134
135
136void    btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
137{
138        m_dispatchInfoPtr = &dispatchInfo;
139        m_collisionMarginTriangle = collisionMarginTriangle;
140        m_resultOut = resultOut;
141
142        //recalc aabbs
143        btTransform convexInTriangleSpace;
144        convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * m_convexBody->getWorldTransform();
145        btCollisionShape* convexShape = static_cast<btCollisionShape*>(m_convexBody->getCollisionShape());
146        //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
147        convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
148        btScalar extraMargin = collisionMarginTriangle;
149        btVector3 extra(extraMargin,extraMargin,extraMargin);
150
151        m_aabbMax += extra;
152        m_aabbMin -= extra;
153       
154}
155
156void btConvexConcaveCollisionAlgorithm::clearCache()
157{
158        m_btConvexTriangleCallback.clearCache();
159
160}
161
162void btConvexConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
163{
164       
165       
166        btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
167        btCollisionObject* triBody = m_isSwapped ? body0 : body1;
168
169        if (triBody->getCollisionShape()->isConcave())
170        {
171
172
173                btCollisionObject*      triOb = triBody;
174                btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
175               
176                if (convexBody->getCollisionShape()->isConvex())
177                {
178                        btScalar collisionMarginTriangle = concaveShape->getMargin();
179                                       
180                        resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
181                        m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
182
183                        //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
184                        //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
185
186                        m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
187
188                        concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
189                       
190                        resultOut->refreshContactPoints();
191       
192                }
193       
194        }
195
196}
197
198
199btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
200{
201        (void)resultOut;
202        (void)dispatchInfo;
203        btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
204        btCollisionObject* triBody = m_isSwapped ? body0 : body1;
205
206
207        //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
208
209        //only perform CCD above a certain threshold, this prevents blocking on the long run
210        //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
211        btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
212        if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
213        {
214                return btScalar(1.);
215        }
216
217        //const btVector3& from = convexbody->m_worldTransform.getOrigin();
218        //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
219        //todo: only do if the motion exceeds the 'radius'
220
221        btTransform triInv = triBody->getWorldTransform().inverse();
222        btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
223        btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
224
225        struct LocalTriangleSphereCastCallback  : public btTriangleCallback
226        {
227                btTransform m_ccdSphereFromTrans;
228                btTransform m_ccdSphereToTrans;
229                btTransform     m_meshTransform;
230
231                btScalar        m_ccdSphereRadius;
232                btScalar        m_hitFraction;
233       
234
235                LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
236                        :m_ccdSphereFromTrans(from),
237                        m_ccdSphereToTrans(to),
238                        m_ccdSphereRadius(ccdSphereRadius),
239                        m_hitFraction(hitFraction)
240                {                       
241                }
242               
243               
244                virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
245                {
246                        (void)partId;
247                        (void)triangleIndex;
248                        //do a swept sphere for now
249                        btTransform ident;
250                        ident.setIdentity();
251                        btConvexCast::CastResult castResult;
252                        castResult.m_fraction = m_hitFraction;
253                        btSphereShape   pointShape(m_ccdSphereRadius);
254                        btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
255                        btVoronoiSimplexSolver  simplexSolver;
256                        btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
257                        //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
258                        //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
259                        //local space?
260
261                        if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
262                                ident,ident,castResult))
263                        {
264                                if (m_hitFraction > castResult.m_fraction)
265                                        m_hitFraction = castResult.m_fraction;
266                        }
267
268                }
269
270        };
271
272
273       
274
275       
276        if (triBody->getCollisionShape()->isConcave())
277        {
278                btVector3 rayAabbMin = convexFromLocal.getOrigin();
279                rayAabbMin.setMin(convexToLocal.getOrigin());
280                btVector3 rayAabbMax = convexFromLocal.getOrigin();
281                rayAabbMax.setMax(convexToLocal.getOrigin());
282                btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
283                rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
284                rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
285
286                btScalar curHitFraction = btScalar(1.); //is this available?
287                LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
288                        convexbody->getCcdSweptSphereRadius(),curHitFraction);
289
290                raycastCallback.m_hitFraction = convexbody->getHitFraction();
291
292                btCollisionObject* concavebody = triBody;
293
294                btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
295               
296                if (triangleMesh)
297                {
298                        triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
299                }
300       
301
302
303                if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
304                {
305                        convexbody->setHitFraction( raycastCallback.m_hitFraction);
306                        return raycastCallback.m_hitFraction;
307                }
308        }
309
310        return btScalar(1.);
311
312}
Note: See TracBrowser for help on using the repository browser.