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/btConvexConvexAlgorithm.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp')
-rw-r--r--extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp119
1 files changed, 82 insertions, 37 deletions
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
index dd1f3e2490f..62f98a846f4 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
@@ -52,7 +52,7 @@ subject to the following restrictions:
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
-
+#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
///////////
@@ -191,8 +191,8 @@ btConvexConvexAlgorithm::CreateFunc::~CreateFunc()
{
}
-btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
-: btActivatingCollisionAlgorithm(ci,body0,body1),
+btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
+: btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
m_simplexSolver(simplexSolver),
m_pdSolver(pdSolver),
m_ownManifold (false),
@@ -205,8 +205,8 @@ m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngu
m_numPerturbationIterations(numPerturbationIterations),
m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
{
- (void)body0;
- (void)body1;
+ (void)body0Wrap;
+ (void)body1Wrap;
}
@@ -289,13 +289,13 @@ extern btScalar gContactBreakingThreshold;
//
// Convex-Convex collision algorithm
//
-void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
+void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
if (!m_manifoldPtr)
{
//swapped?
- m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
+ m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
m_ownManifold = true;
}
resultOut->setPersistentManifold(m_manifoldPtr);
@@ -304,8 +304,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
//resultOut->getPersistentManifold()->clearManifold();
- btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
- btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
+ const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
+ const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());
btVector3 normalOnB;
btVector3 pointOnBWorld;
@@ -314,14 +314,14 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
{
btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
- btVector3 localScalingA = capsuleA->getLocalScaling();
- btVector3 localScalingB = capsuleB->getLocalScaling();
+ // btVector3 localScalingA = capsuleA->getLocalScaling();
+ // btVector3 localScalingB = capsuleB->getLocalScaling();
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
- body0->getWorldTransform(),body1->getWorldTransform(),threshold);
+ body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold);
if (dist<threshold)
{
@@ -374,8 +374,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
}
input.m_stackAlloc = dispatchInfo.m_stackAllocator;
- input.m_transformA = body0->getWorldTransform();
- input.m_transformB = body1->getWorldTransform();
+ input.m_transformA = body0Wrap->getWorldTransform();
+ input.m_transformB = body1Wrap->getWorldTransform();
@@ -407,9 +407,51 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
{
}
};
+
+
+ struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result
+ {
+ btDiscreteCollisionDetectorInterface::Result* m_originalResult;
+ btVector3 m_reportedNormalOnWorld;
+ btScalar m_marginOnA;
+ btScalar m_marginOnB;
+ btScalar m_reportedDistance;
+
+ bool m_foundResult;
+ btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB)
+ :m_originalResult(result),
+ m_marginOnA(marginOnA),
+ m_marginOnB(marginOnB),
+ m_foundResult(false)
+ {
+ }
+
+ virtual void setShapeIdentifiersA(int partId0,int index0){}
+ virtual void setShapeIdentifiersB(int partId1,int index1){}
+ virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorldOrg,btScalar depthOrg)
+ {
+ m_reportedDistance = depthOrg;
+ m_reportedNormalOnWorld = normalOnBInWorld;
+
+ btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB;
+ m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB);
+ if (m_reportedDistance<0.f)
+ {
+ m_foundResult = true;
+ }
+ m_originalResult->addContactPoint(normalOnBInWorld,adjustedPointB,m_reportedDistance);
+ }
+ };
+
btDummyResult dummy;
+///btBoxShape is an exception: its vertices are created WITH margin so don't subtract it
+
+ btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin();
+ btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin();
+
+ btWithoutMarginResult withoutMargin(resultOut, min0Margin,min1Margin);
btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
@@ -429,39 +471,42 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
{
foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
- body0->getWorldTransform(),
- body1->getWorldTransform(),
- sepNormalWorldSpace);
+ body0Wrap->getWorldTransform(),
+ body1Wrap->getWorldTransform(),
+ sepNormalWorldSpace,*resultOut);
} else
{
#ifdef ZERO_MARGIN
gjkPairDetector.setIgnoreMargin(true);
gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
#else
- //gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
- gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
+
+
+ gjkPairDetector.getClosestPoints(input,withoutMargin,dispatchInfo.m_debugDraw);
+ //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
#endif //ZERO_MARGIN
- btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
- if (l2>SIMD_EPSILON)
+ //btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
+ //if (l2>SIMD_EPSILON)
{
- sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
+ sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
//minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
- minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
+ minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin();
#ifdef ZERO_MARGIN
foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
#else
- foundSepAxis = gjkPairDetector.getCachedSeparatingDistance()<(min0->getMargin()+min1->getMargin());
+ foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin());
#endif
}
}
if (foundSepAxis)
{
+
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
- body0->getWorldTransform(),
- body1->getWorldTransform(), minDist-threshold, threshold, *resultOut);
+ body0Wrap->getWorldTransform(),
+ body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut);
}
if (m_ownManifold)
@@ -478,9 +523,9 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
btVertexArray vertices;
btTriangleShape* tri = (btTriangleShape*)polyhedronB;
- vertices.push_back( body1->getWorldTransform()*tri->m_vertices1[0]);
- vertices.push_back( body1->getWorldTransform()*tri->m_vertices1[1]);
- vertices.push_back( body1->getWorldTransform()*tri->m_vertices1[2]);
+ vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
+ vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
+ vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
//tri->initializePolyhedralFeatures();
@@ -496,9 +541,9 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
polyhedronB->initializePolyhedralFeatures();
foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
*polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
- body0->getWorldTransform(),
- body1->getWorldTransform(),
- sepNormalWorldSpace);
+ body0Wrap->getWorldTransform(),
+ body1Wrap->getWorldTransform(),
+ sepNormalWorldSpace,*resultOut);
// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
} else
@@ -525,7 +570,7 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
if (foundSepAxis)
{
btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(),
- body0->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut);
+ body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut);
}
@@ -599,15 +644,15 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
if (perturbeA)
{
- input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis());
- input.m_transformB = body1->getWorldTransform();
+ input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis());
+ input.m_transformB = body1Wrap->getWorldTransform();
#ifdef DEBUG_CONTACTS
dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
#endif //DEBUG_CONTACTS
} else
{
- input.m_transformA = body0->getWorldTransform();
- input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis());
+ input.m_transformA = body0Wrap->getWorldTransform();
+ input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis());
#ifdef DEBUG_CONTACTS
dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
#endif
generated by cgit v1.2.3 (git 2.25.1) at 2024-09-02 18:40:55 +0300