00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "btConvexConcaveCollisionAlgorithm.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 "LinearMath/btIDebugDraw.h"
00027 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
00028 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
00029
00030 btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
00031 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
00032 m_isSwapped(isSwapped),
00033 m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped)
00034 {
00035 }
00036
00037 btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
00038 {
00039 }
00040
00041 void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
00042 {
00043 if (m_btConvexTriangleCallback.m_manifoldPtr)
00044 {
00045 manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr);
00046 }
00047 }
00048
00049
00050 btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped):
00051 m_dispatcher(dispatcher),
00052 m_dispatchInfoPtr(0)
00053 {
00054 m_convexBodyWrap = isSwapped? body1Wrap:body0Wrap;
00055 m_triBodyWrap = isSwapped? body0Wrap:body1Wrap;
00056
00057
00058
00059
00060 m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(),m_triBodyWrap->getCollisionObject());
00061
00062 clearCache();
00063 }
00064
00065 btConvexTriangleCallback::~btConvexTriangleCallback()
00066 {
00067 clearCache();
00068 m_dispatcher->releaseManifold( m_manifoldPtr );
00069
00070 }
00071
00072
00073 void btConvexTriangleCallback::clearCache()
00074 {
00075 m_dispatcher->clearManifold(m_manifoldPtr);
00076 }
00077
00078
00079
00080 void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
00081 {
00082
00083
00084
00085
00086
00087
00088
00089 btCollisionAlgorithmConstructionInfo ci;
00090 ci.m_dispatcher1 = m_dispatcher;
00091
00092
00093
00094
00095 #if 0
00096
00097 if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
00098 {
00099 btVector3 color(1,1,0);
00100 btTransform& tr = ob->getWorldTransform();
00101 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
00102 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
00103 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
00104 }
00105 #endif
00106
00107 if (m_convexBodyWrap->getCollisionShape()->isConvex())
00108 {
00109 btTriangleShape tm(triangle[0],triangle[1],triangle[2]);
00110 tm.setMargin(m_collisionMarginTriangle);
00111
00112
00113 btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);
00114 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr);
00115
00116 const btCollisionObjectWrapper* tmpWrap = 0;
00117
00118 if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
00119 {
00120 tmpWrap = m_resultOut->getBody0Wrap();
00121 m_resultOut->setBody0Wrap(&triObWrap);
00122 m_resultOut->setShapeIdentifiersA(partId,triangleIndex);
00123 }
00124 else
00125 {
00126 tmpWrap = m_resultOut->getBody1Wrap();
00127 m_resultOut->setBody1Wrap(&triObWrap);
00128 m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
00129 }
00130
00131 colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut);
00132
00133 if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
00134 {
00135 m_resultOut->setBody0Wrap(tmpWrap);
00136 } else
00137 {
00138 m_resultOut->setBody1Wrap(tmpWrap);
00139 }
00140
00141
00142
00143 colAlgo->~btCollisionAlgorithm();
00144 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
00145 }
00146
00147 }
00148
00149
00150
00151 void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
00152 {
00153 m_convexBodyWrap = convexBodyWrap;
00154 m_triBodyWrap = triBodyWrap;
00155
00156 m_dispatchInfoPtr = &dispatchInfo;
00157 m_collisionMarginTriangle = collisionMarginTriangle;
00158 m_resultOut = resultOut;
00159
00160
00161 btTransform convexInTriangleSpace;
00162 convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform();
00163 const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
00164
00165 convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
00166 btScalar extraMargin = collisionMarginTriangle;
00167 btVector3 extra(extraMargin,extraMargin,extraMargin);
00168
00169 m_aabbMax += extra;
00170 m_aabbMin -= extra;
00171
00172 }
00173
00174 void btConvexConcaveCollisionAlgorithm::clearCache()
00175 {
00176 m_btConvexTriangleCallback.clearCache();
00177
00178 }
00179
00180 void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00181 {
00182
00183
00184 const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
00185 const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
00186
00187 if (triBodyWrap->getCollisionShape()->isConcave())
00188 {
00189
00190
00191
00192 const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
00193
00194 if (convexBodyWrap->getCollisionShape()->isConvex())
00195 {
00196 btScalar collisionMarginTriangle = concaveShape->getMargin();
00197
00198 resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
00199 m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
00200
00201 m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
00202
00203 concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
00204
00205 resultOut->refreshContactPoints();
00206
00207 m_btConvexTriangleCallback.clearWrapperData();
00208
00209 }
00210
00211 }
00212
00213 }
00214
00215
00216 btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00217 {
00218 (void)resultOut;
00219 (void)dispatchInfo;
00220 btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
00221 btCollisionObject* triBody = m_isSwapped ? body0 : body1;
00222
00223
00224
00225
00226
00227
00228 btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
00229 if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
00230 {
00231 return btScalar(1.);
00232 }
00233
00234
00235
00236
00237
00238 btTransform triInv = triBody->getWorldTransform().inverse();
00239 btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
00240 btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
00241
00242 struct LocalTriangleSphereCastCallback : public btTriangleCallback
00243 {
00244 btTransform m_ccdSphereFromTrans;
00245 btTransform m_ccdSphereToTrans;
00246 btTransform m_meshTransform;
00247
00248 btScalar m_ccdSphereRadius;
00249 btScalar m_hitFraction;
00250
00251
00252 LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
00253 :m_ccdSphereFromTrans(from),
00254 m_ccdSphereToTrans(to),
00255 m_ccdSphereRadius(ccdSphereRadius),
00256 m_hitFraction(hitFraction)
00257 {
00258 }
00259
00260
00261 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
00262 {
00263 (void)partId;
00264 (void)triangleIndex;
00265
00266 btTransform ident;
00267 ident.setIdentity();
00268 btConvexCast::CastResult castResult;
00269 castResult.m_fraction = m_hitFraction;
00270 btSphereShape pointShape(m_ccdSphereRadius);
00271 btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
00272 btVoronoiSimplexSolver simplexSolver;
00273 btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
00274
00275
00276
00277
00278 if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
00279 ident,ident,castResult))
00280 {
00281 if (m_hitFraction > castResult.m_fraction)
00282 m_hitFraction = castResult.m_fraction;
00283 }
00284
00285 }
00286
00287 };
00288
00289
00290
00291
00292
00293 if (triBody->getCollisionShape()->isConcave())
00294 {
00295 btVector3 rayAabbMin = convexFromLocal.getOrigin();
00296 rayAabbMin.setMin(convexToLocal.getOrigin());
00297 btVector3 rayAabbMax = convexFromLocal.getOrigin();
00298 rayAabbMax.setMax(convexToLocal.getOrigin());
00299 btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
00300 rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
00301 rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
00302
00303 btScalar curHitFraction = btScalar(1.);
00304 LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
00305 convexbody->getCcdSweptSphereRadius(),curHitFraction);
00306
00307 raycastCallback.m_hitFraction = convexbody->getHitFraction();
00308
00309 btCollisionObject* concavebody = triBody;
00310
00311 btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
00312
00313 if (triangleMesh)
00314 {
00315 triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
00316 }
00317
00318
00319
00320 if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
00321 {
00322 convexbody->setHitFraction( raycastCallback.m_hitFraction);
00323 return raycastCallback.m_hitFraction;
00324 }
00325 }
00326
00327 return btScalar(1.);
00328
00329 }