| /* |
| Bullet Continuous Collision Detection and Physics Library |
| Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 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. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| |
| #include "BulletCollision/CollisionDispatch/btCollisionWorld.h" |
| #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" |
| #include "BulletCollision/CollisionDispatch/btCollisionObject.h" |
| #include "BulletCollision/CollisionShapes/btCollisionShape.h" |
| #include "BulletCollision/CollisionShapes/btConvexShape.h" |
| #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" |
| #include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting |
| #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting |
| #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" |
| #include "BulletCollision/CollisionShapes/btCompoundShape.h" |
| #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" |
| #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" |
| #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" |
| |
| #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" |
| #include "LinearMath/btAabbUtil2.h" |
| #include "LinearMath/btQuickprof.h" |
| #include "LinearMath/btStackAlloc.h" |
| |
| //#define USE_BRUTEFORCE_RAYBROADPHASE 1 |
| //RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation' or 'updateAabbs' before using a rayTest |
| //#define RECALCULATE_AABB_RAYCAST 1 |
| |
| //When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor) |
| #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" |
| #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" |
| #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" |
| |
| |
| btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache, btCollisionConfiguration* collisionConfiguration) |
| :m_dispatcher1(dispatcher), |
| m_broadphasePairCache(pairCache), |
| m_debugDrawer(0), |
| m_forceUpdateAllAabbs(true) |
| { |
| m_stackAlloc = collisionConfiguration->getStackAllocator(); |
| m_dispatchInfo.m_stackAllocator = m_stackAlloc; |
| } |
| |
| |
| btCollisionWorld::~btCollisionWorld() |
| { |
| |
| //clean up remaining objects |
| int i; |
| for (i=0;i<m_collisionObjects.size();i++) |
| { |
| btCollisionObject* collisionObject= m_collisionObjects[i]; |
| |
| btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); |
| if (bp) |
| { |
| // |
| // only clear the cached algorithms |
| // |
| getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); |
| getBroadphase()->destroyProxy(bp,m_dispatcher1); |
| collisionObject->setBroadphaseHandle(0); |
| } |
| } |
| |
| |
| } |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| |
| void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask) |
| { |
| |
| btAssert(collisionObject); |
| |
| //check that the object isn't already added |
| btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size()); |
| |
| m_collisionObjects.push_back(collisionObject); |
| |
| //calculate new AABB |
| btTransform trans = collisionObject->getWorldTransform(); |
| |
| btVector3 minAabb; |
| btVector3 maxAabb; |
| collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb); |
| |
| int type = collisionObject->getCollisionShape()->getShapeType(); |
| collisionObject->setBroadphaseHandle( getBroadphase()->createProxy( |
| minAabb, |
| maxAabb, |
| type, |
| collisionObject, |
| collisionFilterGroup, |
| collisionFilterMask, |
| m_dispatcher1,0 |
| )) ; |
| |
| |
| |
| |
| |
| } |
| |
| |
| |
| void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) |
| { |
| btVector3 minAabb,maxAabb; |
| colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); |
| //need to increase the aabb for contact thresholds |
| btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold); |
| minAabb -= contactThreshold; |
| maxAabb += contactThreshold; |
| |
| btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; |
| |
| //moving objects should be moderately sized, probably something wrong if not |
| if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) |
| { |
| bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); |
| } else |
| { |
| //something went wrong, investigate |
| //this assert is unwanted in 3D modelers (danger of loosing work) |
| colObj->setActivationState(DISABLE_SIMULATION); |
| |
| static bool reportMe = true; |
| if (reportMe && m_debugDrawer) |
| { |
| reportMe = false; |
| m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation"); |
| m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n"); |
| m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n"); |
| m_debugDrawer->reportErrorWarning("Thanks.\n"); |
| } |
| } |
| } |
| |
| void btCollisionWorld::updateAabbs() |
| { |
| BT_PROFILE("updateAabbs"); |
| |
| btTransform predictedTrans; |
| for ( int i=0;i<m_collisionObjects.size();i++) |
| { |
| btCollisionObject* colObj = m_collisionObjects[i]; |
| |
| //only update aabb of active objects |
| if (m_forceUpdateAllAabbs || colObj->isActive()) |
| { |
| updateSingleAabb(colObj); |
| } |
| } |
| } |
| |
| |
| |
| void btCollisionWorld::performDiscreteCollisionDetection() |
| { |
| BT_PROFILE("performDiscreteCollisionDetection"); |
| |
| btDispatcherInfo& dispatchInfo = getDispatchInfo(); |
| |
| updateAabbs(); |
| |
| { |
| BT_PROFILE("calculateOverlappingPairs"); |
| m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); |
| } |
| |
| |
| btDispatcher* dispatcher = getDispatcher(); |
| { |
| BT_PROFILE("dispatchAllCollisionPairs"); |
| if (dispatcher) |
| dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); |
| } |
| |
| } |
| |
| |
| |
| void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) |
| { |
| |
| |
| //bool removeFromBroadphase = false; |
| |
| { |
| |
| btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle(); |
| if (bp) |
| { |
| // |
| // only clear the cached algorithms |
| // |
| getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); |
| getBroadphase()->destroyProxy(bp,m_dispatcher1); |
| collisionObject->setBroadphaseHandle(0); |
| } |
| } |
| |
| |
| //swapremove |
| m_collisionObjects.remove(collisionObject); |
| |
| } |
| |
| |
| |
| void btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans, |
| btCollisionObject* collisionObject, |
| const btCollisionShape* collisionShape, |
| const btTransform& colObjWorldTransform, |
| RayResultCallback& resultCallback) |
| { |
| btSphereShape pointShape(btScalar(0.0)); |
| pointShape.setMargin(0.f); |
| const btConvexShape* castShape = &pointShape; |
| |
| if (collisionShape->isConvex()) |
| { |
| // BT_PROFILE("rayTestConvex"); |
| btConvexCast::CastResult castResult; |
| castResult.m_fraction = resultCallback.m_closestHitFraction; |
| |
| 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()) |
| { |
| // BT_PROFILE("rayTestConcave"); |
| if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) |
| { |
| ///optimized version for btBvhTriangleMeshShape |
| btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)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; |
| |
| btTransform m_colObjWorldTransform; |
| |
| BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, |
| btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh,const btTransform& colObjWorldTransform): |
| //@BP Mod |
| btTriangleRaycastCallback(from,to, resultCallback->m_flags), |
| m_resultCallback(resultCallback), |
| m_collisionObject(collisionObject), |
| m_triangleMesh(triangleMesh), |
| m_colObjWorldTransform(colObjWorldTransform) |
| { |
| } |
| |
| |
| virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) |
| { |
| btCollisionWorld::LocalShapeInfo shapeInfo; |
| shapeInfo.m_shapePart = partId; |
| shapeInfo.m_triangleIndex = triangleIndex; |
| |
| btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; |
| |
| btCollisionWorld::LocalRayResult rayResult |
| (m_collisionObject, |
| &shapeInfo, |
| hitNormalWorld, |
| hitFraction); |
| |
| bool normalInWorldSpace = true; |
| return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); |
| } |
| |
| }; |
| |
| BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh,colObjWorldTransform); |
| rcb.m_hitFraction = resultCallback.m_closestHitFraction; |
| triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); |
| } else |
| { |
| //generic (slower) case |
| btConcaveShape* concaveShape = (btConcaveShape*)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; |
| btConcaveShape* m_triangleMesh; |
| |
| btTransform m_colObjWorldTransform; |
| |
| BridgeTriangleRaycastCallback( const btVector3& from,const btVector3& to, |
| btCollisionWorld::RayResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& colObjWorldTransform): |
| //@BP Mod |
| btTriangleRaycastCallback(from,to, resultCallback->m_flags), |
| m_resultCallback(resultCallback), |
| m_collisionObject(collisionObject), |
| m_triangleMesh(triangleMesh), |
| m_colObjWorldTransform(colObjWorldTransform) |
| { |
| } |
| |
| |
| virtual btScalar reportHit(const btVector3& hitNormalLocal, btScalar hitFraction, int partId, int triangleIndex ) |
| { |
| btCollisionWorld::LocalShapeInfo shapeInfo; |
| shapeInfo.m_shapePart = partId; |
| shapeInfo.m_triangleIndex = triangleIndex; |
| |
| btVector3 hitNormalWorld = m_colObjWorldTransform.getBasis() * hitNormalLocal; |
| |
| btCollisionWorld::LocalRayResult rayResult |
| (m_collisionObject, |
| &shapeInfo, |
| hitNormalWorld, |
| hitFraction); |
| |
| bool normalInWorldSpace = true; |
| return m_resultCallback->addSingleResult(rayResult,normalInWorldSpace); |
| } |
| |
| }; |
| |
| |
| BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,concaveShape, colObjWorldTransform); |
| rcb.m_hitFraction = resultCallback.m_closestHitFraction; |
| |
| btVector3 rayAabbMinLocal = rayFromLocal; |
| rayAabbMinLocal.setMin(rayToLocal); |
| btVector3 rayAabbMaxLocal = rayFromLocal; |
| rayAabbMaxLocal.setMax(rayToLocal); |
| |
| concaveShape->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); |
| } |
| } else { |
| // BT_PROFILE("rayTestCompound"); |
| ///@todo: use AABB tree or other BVH acceleration structure, see btDbvt |
| 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; |
| // replace collision shape so that callback can determine the triangle |
| btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape(); |
| collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape); |
| rayTestSingle(rayFromTrans,rayToTrans, |
| collisionObject, |
| childCollisionShape, |
| childWorldTrans, |
| resultCallback); |
| // restore |
| collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape); |
| } |
| } |
| } |
| } |
| } |
| |
| 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); |
| } |
| } |
| } |
| } |
| } |
| |
| |
| struct btSingleRayCallback : public btBroadphaseRayCallback |
| { |
| |
| btVector3 m_rayFromWorld; |
| btVector3 m_rayToWorld; |
| btTransform m_rayFromTrans; |
| btTransform m_rayToTrans; |
| btVector3 m_hitNormal; |
| |
| const btCollisionWorld* m_world; |
| btCollisionWorld::RayResultCallback& m_resultCallback; |
| |
| btSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btCollisionWorld* world,btCollisionWorld::RayResultCallback& resultCallback) |
| :m_rayFromWorld(rayFromWorld), |
| m_rayToWorld(rayToWorld), |
| m_world(world), |
| m_resultCallback(resultCallback) |
| { |
| m_rayFromTrans.setIdentity(); |
| m_rayFromTrans.setOrigin(m_rayFromWorld); |
| m_rayToTrans.setIdentity(); |
| m_rayToTrans.setOrigin(m_rayToWorld); |
| |
| btVector3 rayDir = (rayToWorld-rayFromWorld); |
| |
| rayDir.normalize (); |
| ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT |
| m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; |
| m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; |
| m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; |
| m_signs[0] = m_rayDirectionInverse[0] < 0.0; |
| m_signs[1] = m_rayDirectionInverse[1] < 0.0; |
| m_signs[2] = m_rayDirectionInverse[2] < 0.0; |
| |
| m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld); |
| |
| } |
| |
| |
| |
| virtual bool process(const btBroadphaseProxy* proxy) |
| { |
| ///terminate further ray tests, once the closestHitFraction reached zero |
| if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) |
| return false; |
| |
| btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; |
| |
| //only perform raycast if filterMask matches |
| if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) |
| { |
| //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); |
| //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; |
| #if 0 |
| #ifdef RECALCULATE_AABB |
| btVector3 collisionObjectAabbMin,collisionObjectAabbMax; |
| collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); |
| #else |
| //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax); |
| const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin; |
| const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; |
| #endif |
| #endif |
| //btScalar hitLambda = m_resultCallback.m_closestHitFraction; |
| //culling already done by broadphase |
| //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) |
| { |
| m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans, |
| collisionObject, |
| collisionObject->getCollisionShape(), |
| collisionObject->getWorldTransform(), |
| m_resultCallback); |
| } |
| } |
| return true; |
| } |
| }; |
| |
| void btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const |
| { |
| //BT_PROFILE("rayTest"); |
| /// 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 |
| btSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback); |
| |
| #ifndef USE_BRUTEFORCE_RAYBROADPHASE |
| m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB); |
| #else |
| for (int i=0;i<this->getNumCollisionObjects();i++) |
| { |
| rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); |
| } |
| #endif //USE_BRUTEFORCE_RAYBROADPHASE |
| |
| } |
| |
| |
| struct btSingleSweepCallback : public btBroadphaseRayCallback |
| { |
| |
| btTransform m_convexFromTrans; |
| btTransform m_convexToTrans; |
| btVector3 m_hitNormal; |
| const btCollisionWorld* m_world; |
| btCollisionWorld::ConvexResultCallback& m_resultCallback; |
| btScalar m_allowedCcdPenetration; |
| const btConvexShape* m_castShape; |
| |
| |
| btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration) |
| :m_convexFromTrans(convexFromTrans), |
| m_convexToTrans(convexToTrans), |
| m_world(world), |
| m_resultCallback(resultCallback), |
| m_allowedCcdPenetration(allowedPenetration), |
| m_castShape(castShape) |
| { |
| btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin()); |
| btVector3 rayDir = unnormalizedRayDir.normalized(); |
| ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT |
| m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0]; |
| m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1]; |
| m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2]; |
| m_signs[0] = m_rayDirectionInverse[0] < 0.0; |
| m_signs[1] = m_rayDirectionInverse[1] < 0.0; |
| m_signs[2] = m_rayDirectionInverse[2] < 0.0; |
| |
| m_lambda_max = rayDir.dot(unnormalizedRayDir); |
| |
| } |
| |
| virtual bool process(const btBroadphaseProxy* proxy) |
| { |
| ///terminate further convex sweep tests, once the closestHitFraction reached zero |
| if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) |
| return false; |
| |
| btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; |
| |
| //only perform raycast if filterMask matches |
| if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) { |
| //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); |
| m_world->objectQuerySingle(m_castShape, m_convexFromTrans,m_convexToTrans, |
| collisionObject, |
| collisionObject->getCollisionShape(), |
| collisionObject->getWorldTransform(), |
| m_resultCallback, |
| m_allowedCcdPenetration); |
| } |
| |
| return true; |
| } |
| }; |
| |
| |
| |
| 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 |
| } |