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 }
void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, btCollisionObject* collisionObject, const btCollisionShape* collisionShape, const btTransform& colObjWorldTransform, RayResultCallback& resultCallback,short int collisionFilterMask) { btSphereShape pointShape(btScalar(0.0)); pointShape.setMargin(0.f); objectQuerySingle(&pointShape,rayFromTrans,rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback,collisionFilterMask); }
void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& rayFromTrans,const btTransform& rayToTrans, btCollisionObject* collisionObject, const btCollisionShape* collisionShape, const btTransform& colObjWorldTransform, RayResultCallback& resultCallback,short int collisionFilterMask) { if (collisionShape->isConvex()) { btConvexCast::CastResult castResult; castResult.m_fraction = btScalar(1.);//?? btConvexShape* convexShape = (btConvexShape*) collisionShape; btVoronoiSimplexSolver simplexSolver; #define USE_SUBSIMPLEX_CONVEX_CAST 1 #ifdef USE_SUBSIMPLEX_CONVEX_CAST btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver); #else //btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver); //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); #endif //#USE_SUBSIMPLEX_CONVEX_CAST if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) { //add hit if (castResult.m_normal.length2() > btScalar(0.0001)) { if (castResult.m_fraction < resultCallback.m_closestHitFraction) { #ifdef USE_SUBSIMPLEX_CONVEX_CAST //rotate normal into worldspace castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal; #endif //USE_SUBSIMPLEX_CONVEX_CAST castResult.m_normal.normalize(); btCollisionWorld::LocalRayResult localRayResult ( collisionObject, 0, castResult.m_normal, castResult.m_fraction ); bool normalInWorldSpace = true; resultCallback.AddSingleResult(localRayResult, normalInWorldSpace); } } } } else { if (collisionShape->isConcave()) { btTriangleMeshShape* triangleMesh = (btTriangleMeshShape*)collisionShape; btTransform worldTocollisionObject = colObjWorldTransform.inverse(); btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); //ConvexCast::CastResult struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback { btCollisionWorld::RayResultCallback* m_resultCallback; btCollisionObject* m_collisionObject; btTriangleMeshShape* m_triangleMesh; BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh): btTriangleRaycastCallback(from,to), m_resultCallback(resultCallback), m_collisionObject(collisionObject), m_triangleMesh(triangleMesh) { } virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) { btCollisionWorld::LocalShapeInfo shapeInfo; shapeInfo.m_shapePart = partId; shapeInfo.m_triangleIndex = triangleIndex; btCollisionWorld::LocalRayResult rayResult (m_collisionObject, &shapeInfo, hitNormalLocal, hitFraction); bool normalInWorldSpace = false; return m_resultCallback->AddSingleResult(rayResult,normalInWorldSpace); } }; BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); rcb.m_hitFraction = resultCallback.m_closestHitFraction; btVector3 rayAabbMinLocal = rayFromLocal; rayAabbMinLocal.setMin(rayToLocal); btVector3 rayAabbMaxLocal = rayFromLocal; rayAabbMaxLocal.setMax(rayToLocal); triangleMesh->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); } else { //todo: use AABB tree or other BVH acceleration structure! if (collisionShape->isCompound()) { 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; objectQuerySingle(castShape, rayFromTrans,rayToTrans, collisionObject, childCollisionShape, childWorldTrans, resultCallback, collisionFilterMask); } } } } }
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); } } } } }