/ExtLibs/bullet/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
C++ | 846 lines | 595 code | 164 blank | 87 comment | 50 complexity | f088159983aa73f3f4ee7c315bc14b00 MD5 | raw file
- /*
- 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 "btCollisionWorld.h"
- #include "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
- }