Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp')
-rw-r--r--extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp266
1 files changed, 153 insertions, 113 deletions
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
index 912a5285568..e50f85e2bbe 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp
@@ -13,8 +13,8 @@ subject to the following restrictions:
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"
@@ -26,11 +26,12 @@ subject to the following restrictions:
#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_isSwapped(isSwapped),
-m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped)
+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)
{
}
@@ -38,7 +39,7 @@ btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
{
}
-void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
+void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
{
if (m_btConvexTriangleCallback.m_manifoldPtr)
{
@@ -46,55 +47,46 @@ void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray&
}
}
-
-btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped):
- m_dispatcher(dispatcher),
- m_dispatchInfoPtr(0)
+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());
+ 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();
+ clearCache();
}
btConvexTriangleCallback::~btConvexTriangleCallback()
{
clearCache();
- m_dispatcher->releaseManifold( m_manifoldPtr );
-
+ m_dispatcher->releaseManifold(m_manifoldPtr);
}
-
-void btConvexTriangleCallback::clearCache()
+void btConvexTriangleCallback::clearCache()
{
m_dispatcher->clearManifold(m_manifoldPtr);
}
-
-void btConvexTriangleCallback::processTriangle(btVector3* triangle,int
-partId, int triangleIndex)
+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++);
-
-
+ //just for debugging purposes
+ //printf("triangle %d",m_triangleCount++);
btCollisionAlgorithmConstructionInfo ci;
ci.m_dispatcher1 = m_dispatcher;
-
-
#if 0
///debug drawing of the overlapping triangles
@@ -108,52 +100,55 @@ partId, int triangleIndex)
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]);
+ 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 = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr);
+ 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);
+ m_resultOut->setShapeIdentifiersA(partId, triangleIndex);
}
else
{
tmpWrap = m_resultOut->getBody1Wrap();
m_resultOut->setBody1Wrap(&triObWrap);
- m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
+ m_resultOut->setShapeIdentifiersB(partId, triangleIndex);
}
-
- colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut);
+
+ colAlgo->processCollision(m_convexBodyWrap, &triObWrap, *m_dispatchInfoPtr, m_resultOut);
if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
{
m_resultOut->setBody0Wrap(tmpWrap);
- } else
+ }
+ 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)
+void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
{
m_convexBodyWrap = convexBodyWrap;
m_triBodyWrap = triBodyWrap;
@@ -167,65 +162,120 @@ void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr
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;
- btVector3 extra(extraMargin,extraMargin,extraMargin);
+ 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)
+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();
-
- 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);
+ resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+ m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, convexBodyWrap, triBodyWrap, resultOut);
- m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
+ m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(), triBodyWrap->getCollisionObject());
- concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
-
- resultOut->refreshContactPoints();
+ concaveShape->processAllTriangles(&m_btConvexTriangleCallback, m_btConvexTriangleCallback.getAabbMin(), m_btConvexTriangleCallback.getAabbMax());
- m_btConvexTriangleCallback.clearWrapperData();
-
+ resultOut->refreshContactPoints();
+
+ m_btConvexTriangleCallback.clearWrapperData();
+ }
}
-
}
-
}
-
-btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+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
@@ -244,27 +294,26 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
- struct LocalTriangleSphereCastCallback : public btTriangleCallback
+ struct LocalTriangleSphereCastCallback : public btTriangleCallback
{
btTransform m_ccdSphereFromTrans;
btTransform m_ccdSphereToTrans;
- btTransform m_meshTransform;
+ btTransform m_meshTransform;
- btScalar m_ccdSphereRadius;
- btScalar m_hitFraction;
-
+ 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)
- {
+ 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
@@ -272,29 +321,23 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
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);
+ 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 (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();
@@ -302,33 +345,30 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
btVector3 rayAabbMax = convexFromLocal.getOrigin();
rayAabbMax.setMax(convexToLocal.getOrigin());
btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
- rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
- rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
+ rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
+ rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
- btScalar curHitFraction = btScalar(1.); //is this available?
- LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
- convexbody->getCcdSweptSphereRadius(),curHitFraction);
+ 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();
-
+ btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape();
+
if (triangleMesh)
{
- triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
+ triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax);
}
-
-
if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
{
- convexbody->setHitFraction( raycastCallback.m_hitFraction);
+ convexbody->setHitFraction(raycastCallback.m_hitFraction);
return raycastCallback.m_hitFraction;
}
}
return btScalar(1.);
-
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-07-11 14:37:58 +0300