00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
00061
00062
00063
00064 clearCache();
00065 }
00066
00067 btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
00068 {
00069 clearCache();
00070
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);
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
00092
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
00118 tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer());
00119
00120 btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1);
00121
00122 btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);
00123
00124 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);
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
00134
00135
00136
00137
00138 {
00139
00140 btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
00141 normal.normalize();
00142 normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION;
00143
00144
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
00156
00157
00158
00159
00160
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);
00166
00167 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);
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
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
00226 {
00227 btScalar collisionMarginTriangle = concaveShape->getMargin();
00228
00229
00230 m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,triBody,dispatchInfo,resultOut);
00231
00232
00233 concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
00234
00235
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
00253
00254
00255
00256 btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
00257 if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
00258 {
00259 return btScalar(1.);
00260 }
00261
00262
00263
00264
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
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
00303
00304
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.);
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 }