btSoftBodyConcaveCollisionAlgorithm.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 
00017 #include "btSoftBodyConcaveCollisionAlgorithm.h"
00018 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00019 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
00020 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
00021 #include "BulletCollision/CollisionShapes/btConcaveShape.h"
00022 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
00023 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
00024 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
00025 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00026 #include "BulletCollision/CollisionShapes/btTetrahedronShape.h"
00027 #include "BulletCollision/CollisionShapes/btConvexHullShape.h"
00028 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
00029 
00030 
00031 #include "LinearMath/btIDebugDraw.h"
00032 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
00033 #include "BulletSoftBody/btSoftBody.h"
00034 
00035 #define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)//make this configurable
00036 
00037 btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
00038 : btCollisionAlgorithm(ci),
00039 m_isSwapped(isSwapped),
00040 m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped)
00041 {
00042 }
00043 
00044 
00045 
00046 btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm()
00047 {
00048 }
00049 
00050 
00051 
00052 btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher*  dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped):
00053 m_dispatcher(dispatcher),
00054 m_dispatchInfoPtr(0)
00055 {
00056         m_softBody = (isSwapped? (btSoftBody*)body1Wrap->getCollisionObject():(btSoftBody*)body0Wrap->getCollisionObject());
00057         m_triBody = isSwapped? body0Wrap->getCollisionObject():body1Wrap->getCollisionObject();
00058 
00059         //
00060         // create the manifold from the dispatcher 'manifold pool'
00061         //
00062         //        m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
00063 
00064         clearCache();
00065 }
00066 
00067 btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
00068 {
00069         clearCache();
00070         //      m_dispatcher->releaseManifold( m_manifoldPtr );
00071 
00072 }
00073 
00074 
00075 void    btSoftBodyTriangleCallback::clearCache()
00076 {
00077         for (int i=0;i<m_shapeCache.size();i++)
00078         {
00079                 btTriIndex* tmp = m_shapeCache.getAtIndex(i);
00080                 btAssert(tmp);
00081                 btAssert(tmp->m_childShape);
00082                 m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape);//necessary?
00083                 delete tmp->m_childShape;
00084         }
00085         m_shapeCache.clear();
00086 }
00087 
00088 
00089 void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
00090 {
00091         //just for debugging purposes
00092         //printf("triangle %d",m_triangleCount++);
00093         
00094         btCollisionAlgorithmConstructionInfo ci;
00095         ci.m_dispatcher1 = m_dispatcher;
00096 
00098         if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe))
00099         {
00100                 btVector3 color(1,1,0);
00101                 const btTransform& tr = m_triBody->getWorldTransform();
00102                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
00103                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
00104                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
00105         }
00106 
00107         btTriIndex      triIndex(partId,triangleIndex,0);
00108         btHashKey<btTriIndex> triKey(triIndex.getUid());
00109 
00110 
00111         btTriIndex* shapeIndex = m_shapeCache[triKey];
00112         if (shapeIndex)
00113         {
00114                 btCollisionShape* tm = shapeIndex->m_childShape;
00115                 btAssert(tm);
00116 
00117                 //copy over user pointers to temporary shape
00118                 tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer());
00119 
00120                 btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1);
00121                 //btCollisionObjectWrapper triBody(0,tm, ob, btTransform::getIdentity());//ob->getWorldTransform());//??
00122                 btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);
00123 
00124                 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr);
00125 
00126                 colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut);
00127                 colAlgo->~btCollisionAlgorithm();
00128                 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
00129                 
00130                 return;
00131         }
00132 
00133         //aabb filter is already applied!       
00134 
00135         //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
00136 
00137         //      if (m_softBody->getCollisionShape()->getShapeType()==
00138         {
00139                 //              btVector3 other;
00140                 btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
00141                 normal.normalize();
00142                 normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION;
00143                 //              other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
00144                 //              other+=normal*22.f;
00145                 btVector3       pts[6] = {triangle[0]+normal,
00146                         triangle[1]+normal,
00147                         triangle[2]+normal,
00148                         triangle[0]-normal,
00149                         triangle[1]-normal,
00150                         triangle[2]-normal};
00151 
00152                 btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6);
00153 
00154 
00155                 //              btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other);
00156 
00157                 //btTriangleShape tm(triangle[0],triangle[1],triangle[2]);      
00158                 //      tm.setMargin(m_collisionMarginTriangle);
00159 
00160                 //copy over user pointers to temporary shape
00161                 tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer());
00162 
00163                 
00164                 btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1);
00165                 btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);//btTransform::getIdentity());//??
00166 
00167                 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr);
00168 
00169                 colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut);
00170                 colAlgo->~btCollisionAlgorithm();
00171                 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
00172 
00173                 triIndex.m_childShape = tm;
00174                 m_shapeCache.insert(triKey,triIndex);
00175 
00176         }
00177 
00178 
00179 
00180 }
00181 
00182 
00183 
00184 void    btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btCollisionObjectWrapper* triBodyWrap, const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00185 {
00186         m_dispatchInfoPtr = &dispatchInfo;
00187         m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION);
00188         m_resultOut = resultOut;
00189 
00190 
00191         btVector3       aabbWorldSpaceMin,aabbWorldSpaceMax;
00192         m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax);
00193         btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5);
00194         btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5);
00195 
00196         btTransform softTransform;
00197         softTransform.setIdentity();
00198         softTransform.setOrigin(softBodyCenter);
00199 
00200         btTransform convexInTriangleSpace;
00201         convexInTriangleSpace = triBodyWrap->getWorldTransform().inverse() * softTransform;
00202         btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax);
00203 }
00204 
00205 void btSoftBodyConcaveCollisionAlgorithm::clearCache()
00206 {
00207         m_btSoftBodyTriangleCallback.clearCache();
00208 
00209 }
00210 
00211 void btSoftBodyConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00212 {
00213 
00214 
00215         //btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
00216         const btCollisionObjectWrapper* triBody = m_isSwapped ? body0Wrap : body1Wrap;
00217 
00218         if (triBody->getCollisionShape()->isConcave())
00219         {
00220 
00221 
00222                 const btCollisionObject*        triOb = triBody->getCollisionObject();
00223                 const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triOb->getCollisionShape());
00224 
00225                 //      if (convexBody->getCollisionShape()->isConvex())
00226                 {
00227                         btScalar collisionMarginTriangle = concaveShape->getMargin();
00228 
00229                         //                      resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
00230                         m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,triBody,dispatchInfo,resultOut);
00231 
00232                 
00233                         concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
00234 
00235                         //      resultOut->refreshContactPoints();
00236 
00237                 }
00238 
00239         }
00240 
00241 }
00242 
00243 
00244 btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00245 {
00246         (void)resultOut;
00247         (void)dispatchInfo;
00248         btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
00249         btCollisionObject* triBody = m_isSwapped ? body0 : body1;
00250 
00251 
00252         //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
00253 
00254         //only perform CCD above a certain threshold, this prevents blocking on the long run
00255         //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
00256         btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
00257         if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
00258         {
00259                 return btScalar(1.);
00260         }
00261 
00262         //const btVector3& from = convexbody->m_worldTransform.getOrigin();
00263         //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
00264         //todo: only do if the motion exceeds the 'radius'
00265 
00266         btTransform triInv = triBody->getWorldTransform().inverse();
00267         btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
00268         btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
00269 
00270         struct LocalTriangleSphereCastCallback  : public btTriangleCallback
00271         {
00272                 btTransform m_ccdSphereFromTrans;
00273                 btTransform m_ccdSphereToTrans;
00274                 btTransform     m_meshTransform;
00275 
00276                 btScalar        m_ccdSphereRadius;
00277                 btScalar        m_hitFraction;
00278 
00279 
00280                 LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
00281                         :m_ccdSphereFromTrans(from),
00282                         m_ccdSphereToTrans(to),
00283                         m_ccdSphereRadius(ccdSphereRadius),
00284                         m_hitFraction(hitFraction)
00285                 {                       
00286                 }
00287 
00288 
00289                 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
00290                 {
00291                         (void)partId;
00292                         (void)triangleIndex;
00293                         //do a swept sphere for now
00294                         btTransform ident;
00295                         ident.setIdentity();
00296                         btConvexCast::CastResult castResult;
00297                         castResult.m_fraction = m_hitFraction;
00298                         btSphereShape   pointShape(m_ccdSphereRadius);
00299                         btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
00300                         btVoronoiSimplexSolver  simplexSolver;
00301                         btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
00302                         //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
00303                         //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
00304                         //local space?
00305 
00306                         if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
00307                                 ident,ident,castResult))
00308                         {
00309                                 if (m_hitFraction > castResult.m_fraction)
00310                                         m_hitFraction = castResult.m_fraction;
00311                         }
00312 
00313                 }
00314 
00315         };
00316 
00317 
00318 
00319 
00320 
00321         if (triBody->getCollisionShape()->isConcave())
00322         {
00323                 btVector3 rayAabbMin = convexFromLocal.getOrigin();
00324                 rayAabbMin.setMin(convexToLocal.getOrigin());
00325                 btVector3 rayAabbMax = convexFromLocal.getOrigin();
00326                 rayAabbMax.setMax(convexToLocal.getOrigin());
00327                 btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
00328                 rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
00329                 rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
00330 
00331                 btScalar curHitFraction = btScalar(1.); //is this available?
00332                 LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
00333                         convexbody->getCcdSweptSphereRadius(),curHitFraction);
00334 
00335                 raycastCallback.m_hitFraction = convexbody->getHitFraction();
00336 
00337                 btCollisionObject* concavebody = triBody;
00338 
00339                 btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
00340 
00341                 if (triangleMesh)
00342                 {
00343                         triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
00344                 }
00345 
00346 
00347 
00348                 if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
00349                 {
00350                         convexbody->setHitFraction( raycastCallback.m_hitFraction);
00351                         return raycastCallback.m_hitFraction;
00352                 }
00353         }
00354 
00355         return btScalar(1.);
00356 
00357 }