void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btTransform& convexFromWorld, const btTransform& convexToWorld, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration) const { BT_PROFILE("convexSweepTest"); /// use the broadphase to accelerate the search for objects, based on their aabb /// and for each object with ray-aabb overlap, perform an exact ray test /// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical btTransform convexFromTrans,convexToTrans; convexFromTrans = convexFromWorld; convexToTrans = convexToWorld; btVector3 castShapeAabbMin, castShapeAabbMax; /* Compute AABB that encompasses angular movement */ { btVector3 linVel, angVel; btTransformUtil::calculateVelocity (convexFromTrans, convexToTrans, 1.0, linVel, angVel); btVector3 zeroLinVel; zeroLinVel.setValue(0,0,0); btTransform R; R.setIdentity (); R.setRotation (convexFromTrans.getRotation()); castShape->calculateTemporalAabb (R, zeroLinVel, angVel, 1.0, castShapeAabbMin, castShapeAabbMax); } #ifndef USE_BRUTEFORCE_RAYBROADPHASE btSingleSweepCallback convexCB(castShape,convexFromWorld,convexToWorld,this,resultCallback,allowedCcdPenetration); m_broadphasePairCache->rayTest(convexFromTrans.getOrigin(),convexToTrans.getOrigin(),convexCB,castShapeAabbMin,castShapeAabbMax); #else /// go over all objects, and if the ray intersects their aabb + cast shape aabb, // do a ray-shape query using convexCaster (CCD) int i; for (i=0;i<m_collisionObjects.size();i++) { btCollisionObject* collisionObject= m_collisionObjects[i]; //only perform raycast if filterMask matches if(resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); btVector3 collisionObjectAabbMin,collisionObjectAabbMax; collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); AabbExpand (collisionObjectAabbMin, collisionObjectAabbMax, castShapeAabbMin, castShapeAabbMax); btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing btVector3 hitNormal; if (btRayAabb(convexFromWorld.getOrigin(),convexToWorld.getOrigin(),collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal)) { objectQuerySingle(castShape, convexFromTrans,convexToTrans, collisionObject, collisionObject->getCollisionShape(), collisionObject->getWorldTransform(), resultCallback, allowedCcdPenetration); } } } #endif //USE_BRUTEFORCE_RAYBROADPHASE }
virtual btScalar addSingleResult (btCollisionWorld::LocalConvexResult& r, bool b) { btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = -1; shapeInfo.m_triangleIndex = m_i; if (r.m_localShapeInfo == NULL) r.m_localShapeInfo = &shapeInfo; const btScalar result = m_userCallback->addSingleResult(r, b); m_closestHitFraction = m_userCallback->m_closestHitFraction; return result; }
void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans, btCollisionObject* collisionObject, const btCollisionShape* collisionShape, const btTransform& colObjWorldTransform, ConvexResultCallback& resultCallback, btScalar allowedPenetration) { if (collisionShape->isConvex()) { //BT_PROFILE("convexSweepConvex"); btConvexCast::CastResult castResult; castResult.m_allowedPenetration = allowedPenetration; castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//?? btConvexShape* convexShape = (btConvexShape*) collisionShape; btVoronoiSimplexSolver simplexSolver; btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver; btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver); //btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver); //btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver); btConvexCast* castPtr = &convexCaster1; if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) { //add hit if (castResult.m_normal.length2() > btScalar(0.0001)) { if (castResult.m_fraction < resultCallback.m_closestHitFraction) { castResult.m_normal.normalize(); btCollisionWorld::LocalConvexResult localConvexResult ( collisionObject, 0, castResult.m_normal, castResult.m_hitPoint, castResult.m_fraction ); bool normalInWorldSpace = true; resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); } } } } else { if (collisionShape->isConcave()) { if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) { //BT_PROFILE("convexSweepbtBvhTriangleMesh"); btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; btTransform worldTocollisionObject = colObjWorldTransform.inverse(); btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis()); //ConvexCast::CastResult struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback { btCollisionWorld::ConvexResultCallback* m_resultCallback; btCollisionObject* m_collisionObject; btTriangleMeshShape* m_triangleMesh; BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld): btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), m_resultCallback(resultCallback), m_collisionObject(collisionObject), m_triangleMesh(triangleMesh) { } virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) { btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; if (hitFraction <= m_resultCallback->m_closestHitFraction) { btCollisionWorld::LocalConvexResult convexResult (m_collisionObject, &shapeInfo, hitNormalLocal, hitPointLocal, hitFraction); bool normalInWorldSpace = true; return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); } return hitFraction; } }; BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform); tccb.m_hitFraction = resultCallback.m_closestHitFraction; btVector3 boxMinLocal, boxMaxLocal; castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal); } else { //BT_PROFILE("convexSweepConcave"); btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; btTransform worldTocollisionObject = colObjWorldTransform.inverse(); btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis()); //ConvexCast::CastResult struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback { btCollisionWorld::ConvexResultCallback* m_resultCallback; btCollisionObject* m_collisionObject; btConcaveShape* m_triangleMesh; BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld): btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), m_resultCallback(resultCallback), m_collisionObject(collisionObject), m_triangleMesh(triangleMesh) { } virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) { btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; if (hitFraction <= m_resultCallback->m_closestHitFraction) { btCollisionWorld::LocalConvexResult convexResult (m_collisionObject, &shapeInfo, hitNormalLocal, hitPointLocal, hitFraction); bool normalInWorldSpace = false; return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); } return hitFraction; } }; BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform); tccb.m_hitFraction = resultCallback.m_closestHitFraction; btVector3 boxMinLocal, boxMaxLocal; castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); btVector3 rayAabbMinLocal = convexFromLocal; rayAabbMinLocal.setMin(convexToLocal); btVector3 rayAabbMaxLocal = convexFromLocal; rayAabbMaxLocal.setMax(convexToLocal); rayAabbMinLocal += boxMinLocal; rayAabbMaxLocal += boxMaxLocal; concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); } } else { ///@todo : use AABB tree or other BVH acceleration structure! if (collisionShape->isCompound()) { BT_PROFILE("convexSweepCompound"); const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape); int i=0; for (i=0;i<compoundShape->getNumChildShapes();i++) { btTransform childTrans = compoundShape->getChildTransform(i); const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i); btTransform childWorldTrans = colObjWorldTransform * childTrans; // replace collision shape so that callback can determine the triangle btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape(); collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); objectQuerySingle(castShape, convexFromTrans,convexToTrans, collisionObject, childCollisionShape, childWorldTrans, resultCallback, allowedPenetration); // restore collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); } } } } }
virtual bool needsCollision(btBroadphaseProxy* p) const { return m_userCallback->needsCollision(p); }