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