diff options
Diffstat (limited to 'thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp | 374 |
1 files changed, 0 insertions, 374 deletions
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp deleted file mode 100644 index e50f85e2bb..0000000000 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ /dev/null @@ -1,374 +0,0 @@ -/* -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 "btConvexConcaveCollisionAlgorithm.h" -#include "LinearMath/btQuickprof.h" -#include "BulletCollision/CollisionDispatch/btCollisionObject.h" -#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" -#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "BulletCollision/CollisionShapes/btConcaveShape.h" -#include "BulletCollision/CollisionDispatch/btManifoldResult.h" -#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "LinearMath/btIDebugDraw.h" -#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" -#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" -#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h" - -btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) - : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap), - m_btConvexTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped), - m_isSwapped(isSwapped) -{ -} - -btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm() -{ -} - -void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray) -{ - if (m_btConvexTriangleCallback.m_manifoldPtr) - { - manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr); - } -} - -btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher), - m_dispatchInfoPtr(0) -{ - m_convexBodyWrap = isSwapped ? body1Wrap : body0Wrap; - m_triBodyWrap = isSwapped ? body0Wrap : body1Wrap; - - // - // create the manifold from the dispatcher 'manifold pool' - // - m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(), m_triBodyWrap->getCollisionObject()); - - clearCache(); -} - -btConvexTriangleCallback::~btConvexTriangleCallback() -{ - clearCache(); - m_dispatcher->releaseManifold(m_manifoldPtr); -} - -void btConvexTriangleCallback::clearCache() -{ - m_dispatcher->clearManifold(m_manifoldPtr); -} - -void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex) -{ - BT_PROFILE("btConvexTriangleCallback::processTriangle"); - - if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax)) - { - return; - } - - //just for debugging purposes - //printf("triangle %d",m_triangleCount++); - - btCollisionAlgorithmConstructionInfo ci; - ci.m_dispatcher1 = m_dispatcher; - -#if 0 - - ///debug drawing of the overlapping triangles - if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe )) - { - const btCollisionObject* ob = const_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject()); - btVector3 color(1,1,0); - btTransform& tr = ob->getWorldTransform(); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color); - m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); - } -#endif - - if (m_convexBodyWrap->getCollisionShape()->isConvex()) - { - btTriangleShape tm(triangle[0], triangle[1], triangle[2]); - tm.setMargin(m_collisionMarginTriangle); - - btCollisionObjectWrapper triObWrap(m_triBodyWrap, &tm, m_triBodyWrap->getCollisionObject(), m_triBodyWrap->getWorldTransform(), partId, triangleIndex); //correct transform? - btCollisionAlgorithm* colAlgo = 0; - - if (m_resultOut->m_closestPointDistanceThreshold > 0) - { - colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); - } - else - { - colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, m_manifoldPtr, BT_CONTACT_POINT_ALGORITHMS); - } - const btCollisionObjectWrapper* tmpWrap = 0; - - if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) - { - tmpWrap = m_resultOut->getBody0Wrap(); - m_resultOut->setBody0Wrap(&triObWrap); - m_resultOut->setShapeIdentifiersA(partId, triangleIndex); - } - else - { - tmpWrap = m_resultOut->getBody1Wrap(); - m_resultOut->setBody1Wrap(&triObWrap); - m_resultOut->setShapeIdentifiersB(partId, triangleIndex); - } - - colAlgo->processCollision(m_convexBodyWrap, &triObWrap, *m_dispatchInfoPtr, m_resultOut); - - if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) - { - m_resultOut->setBody0Wrap(tmpWrap); - } - else - { - m_resultOut->setBody1Wrap(tmpWrap); - } - - colAlgo->~btCollisionAlgorithm(); - ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); - } -} - -void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut) -{ - m_convexBodyWrap = convexBodyWrap; - m_triBodyWrap = triBodyWrap; - - m_dispatchInfoPtr = &dispatchInfo; - m_collisionMarginTriangle = collisionMarginTriangle; - m_resultOut = resultOut; - - //recalc aabbs - btTransform convexInTriangleSpace; - convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform(); - const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape()); - //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); - convexShape->getAabb(convexInTriangleSpace, m_aabbMin, m_aabbMax); - btScalar extraMargin = collisionMarginTriangle + resultOut->m_closestPointDistanceThreshold; - - btVector3 extra(extraMargin, extraMargin, extraMargin); - - m_aabbMax += extra; - m_aabbMin -= extra; -} - -void btConvexConcaveCollisionAlgorithm::clearCache() -{ - m_btConvexTriangleCallback.clearCache(); -} - -void btConvexConcaveCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) -{ - BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision"); - - const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap; - const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap; - - if (triBodyWrap->getCollisionShape()->isConcave()) - { - if (triBodyWrap->getCollisionShape()->getShapeType() == SDF_SHAPE_PROXYTYPE) - { - btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape(); - if (convexBodyWrap->getCollisionShape()->isConvex()) - { - btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape(); - btAlignedObjectArray<btVector3> queryVertices; - - if (convex->isPolyhedral()) - { - btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex; - for (int v = 0; v < poly->getNumVertices(); v++) - { - btVector3 vtx; - poly->getVertex(v, vtx); - queryVertices.push_back(vtx); - } - } - btScalar maxDist = SIMD_EPSILON; - - if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE) - { - queryVertices.push_back(btVector3(0, 0, 0)); - btSphereShape* sphere = (btSphereShape*)convex; - maxDist = sphere->getRadius() + SIMD_EPSILON; - } - if (queryVertices.size()) - { - resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); - //m_btConvexTriangleCallback.m_manifoldPtr->clearManifold(); - - btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex; - for (int v = 0; v < queryVertices.size(); v++) - { - const btVector3& vtx = queryVertices[v]; - btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform() * vtx; - btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace); - - btVector3 normalLocal; - btScalar dist; - if (sdfShape->queryPoint(vtxInSdf, dist, normalLocal)) - { - if (dist <= maxDist) - { - normalLocal.safeNormalize(); - btVector3 normal = triBodyWrap->getWorldTransform().getBasis() * normalLocal; - - if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE) - { - btSphereShape* sphere = (btSphereShape*)convex; - dist -= sphere->getRadius(); - vtxWorldSpace -= sphere->getRadius() * normal; - } - resultOut->addContactPoint(normal, vtxWorldSpace - normal * dist, dist); - } - } - } - resultOut->refreshContactPoints(); - } - } - } - else - { - const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>(triBodyWrap->getCollisionShape()); - - if (convexBodyWrap->getCollisionShape()->isConvex()) - { - btScalar collisionMarginTriangle = concaveShape->getMargin(); - - resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); - m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, convexBodyWrap, triBodyWrap, resultOut); - - m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(), triBodyWrap->getCollisionObject()); - - concaveShape->processAllTriangles(&m_btConvexTriangleCallback, m_btConvexTriangleCallback.getAabbMin(), m_btConvexTriangleCallback.getAabbMax()); - - resultOut->refreshContactPoints(); - - m_btConvexTriangleCallback.clearWrapperData(); - } - } - } -} - -btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) -{ - (void)resultOut; - (void)dispatchInfo; - btCollisionObject* convexbody = m_isSwapped ? body1 : body0; - btCollisionObject* triBody = m_isSwapped ? body0 : body1; - - //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) - - //only perform CCD above a certain threshold, this prevents blocking on the long run - //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... - btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2(); - if (squareMot0 < convexbody->getCcdSquareMotionThreshold()) - { - return btScalar(1.); - } - - //const btVector3& from = convexbody->m_worldTransform.getOrigin(); - //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); - //todo: only do if the motion exceeds the 'radius' - - btTransform triInv = triBody->getWorldTransform().inverse(); - btTransform convexFromLocal = triInv * convexbody->getWorldTransform(); - btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform(); - - struct LocalTriangleSphereCastCallback : public btTriangleCallback - { - btTransform m_ccdSphereFromTrans; - btTransform m_ccdSphereToTrans; - btTransform m_meshTransform; - - btScalar m_ccdSphereRadius; - btScalar m_hitFraction; - - LocalTriangleSphereCastCallback(const btTransform& from, const btTransform& to, btScalar ccdSphereRadius, btScalar hitFraction) - : m_ccdSphereFromTrans(from), - m_ccdSphereToTrans(to), - m_ccdSphereRadius(ccdSphereRadius), - m_hitFraction(hitFraction) - { - } - - virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) - { - BT_PROFILE("processTriangle"); - (void)partId; - (void)triangleIndex; - //do a swept sphere for now - btTransform ident; - ident.setIdentity(); - btConvexCast::CastResult castResult; - castResult.m_fraction = m_hitFraction; - btSphereShape pointShape(m_ccdSphereRadius); - btTriangleShape triShape(triangle[0], triangle[1], triangle[2]); - btVoronoiSimplexSolver simplexSolver; - btSubsimplexConvexCast convexCaster(&pointShape, &triShape, &simplexSolver); - //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); - //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); - //local space? - - if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans, - ident, ident, castResult)) - { - if (m_hitFraction > castResult.m_fraction) - m_hitFraction = castResult.m_fraction; - } - } - }; - - if (triBody->getCollisionShape()->isConcave()) - { - btVector3 rayAabbMin = convexFromLocal.getOrigin(); - rayAabbMin.setMin(convexToLocal.getOrigin()); - btVector3 rayAabbMax = convexFromLocal.getOrigin(); - rayAabbMax.setMax(convexToLocal.getOrigin()); - btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius(); - rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0); - rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0); - - btScalar curHitFraction = btScalar(1.); //is this available? - LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal, - convexbody->getCcdSweptSphereRadius(), curHitFraction); - - raycastCallback.m_hitFraction = convexbody->getHitFraction(); - - btCollisionObject* concavebody = triBody; - - btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape(); - - if (triangleMesh) - { - triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax); - } - - if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) - { - convexbody->setHitFraction(raycastCallback.m_hitFraction); - return raycastCallback.m_hitFraction; - } - } - - return btScalar(1.); -} |