diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/CollisionDispatch')
32 files changed, 671 insertions, 321 deletions
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h index f656e5c323a..22953af43fd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SPHERE_TRIANGLE_DETECTOR_H -#define SPHERE_TRIANGLE_DETECTOR_H +#ifndef BT_SPHERE_TRIANGLE_DETECTOR_H +#define BT_SPHERE_TRIANGLE_DETECTOR_H #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" @@ -47,5 +47,5 @@ private: btScalar m_contactBreakingThreshold; }; -#endif //SPHERE_TRIANGLE_DETECTOR_H +#endif //BT_SPHERE_TRIANGLE_DETECTOR_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h index 21342175238..97c5be77003 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef BOX_2D_BOX_2D__COLLISION_ALGORITHM_H -#define BOX_2D_BOX_2D__COLLISION_ALGORITHM_H +#ifndef BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H +#define BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H #include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -62,5 +62,5 @@ public: }; -#endif //BOX_2D_BOX_2D__COLLISION_ALGORITHM_H +#endif //BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h index e7d2cc25c22..f0bbae61e3b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef BOX_BOX__COLLISION_ALGORITHM_H -#define BOX_BOX__COLLISION_ALGORITHM_H +#ifndef BT_BOX_BOX__COLLISION_ALGORITHM_H +#define BT_BOX_BOX__COLLISION_ALGORITHM_H #include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -62,5 +62,5 @@ public: }; -#endif //BOX_BOX__COLLISION_ALGORITHM_H +#endif //BT_BOX_BOX__COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h index 605294d47bd..3c941f7deb2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h @@ -16,8 +16,8 @@ subject to the following restrictions: 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. */ -#ifndef BOX_BOX_DETECTOR_H -#define BOX_BOX_DETECTOR_H +#ifndef BT_BOX_BOX_DETECTOR_H +#define BT_BOX_BOX_DETECTOR_H class btBoxShape; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h index 1db51a36d03..f63e0923b78 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h @@ -15,6 +15,7 @@ subject to the following restrictions: #ifndef BT_COLLISION_CONFIGURATION #define BT_COLLISION_CONFIGURATION + struct btCollisionAlgorithmCreateFunc; class btStackAlloc; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h index a6da5f61a3c..1d7e74401dd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef COLLISION_CREATE_FUNC -#define COLLISION_CREATE_FUNC +#ifndef BT_COLLISION_CREATE_FUNC +#define BT_COLLISION_CREATE_FUNC #include "LinearMath/btAlignedObjectArray.h" class btCollisionAlgorithm; @@ -41,5 +41,5 @@ struct btCollisionAlgorithmCreateFunc return 0; } }; -#endif //COLLISION_CREATE_FUNC +#endif //BT_COLLISION_CREATE_FUNC diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 9fed44a19f7..29674f3be46 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -92,8 +92,16 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); } else { - mem = btAlignedAlloc(sizeof(btPersistentManifold),16); - + //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert. + if ((m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION)==0) + { + mem = btAlignedAlloc(sizeof(btPersistentManifold),16); + } else + { + btAssert(0); + //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration + return 0; + } } btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold); manifold->m_index1a = m_manifoldsPtr.size(); @@ -172,8 +180,7 @@ bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionO if (!(m_dispatcherFlags & btCollisionDispatcher::CD_STATIC_STATIC_REPORTED)) { //broadphase filtering already deals with this - if ((body0->isStaticObject() || body0->isKinematicObject()) && - (body1->isStaticObject() || body1->isKinematicObject())) + if (body0->isStaticOrKinematicObject() && body1->isStaticOrKinematicObject()) { m_dispatcherFlags |= btCollisionDispatcher::CD_STATIC_STATIC_REPORTED; printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index 3c4f039504a..5accad9a993 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef COLLISION__DISPATCHER_H -#define COLLISION__DISPATCHER_H +#ifndef BT_COLLISION__DISPATCHER_H +#define BT_COLLISION__DISPATCHER_H #include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -67,7 +67,8 @@ public: enum DispatcherFlags { CD_STATIC_STATIC_REPORTED = 1, - CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD = 2 + CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD = 2, + CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION = 4 }; int getDispatcherFlags() const @@ -90,7 +91,7 @@ public: btPersistentManifold** getInternalManifoldPointer() { - return &m_manifoldsPtr[0]; + return m_manifoldsPtr.size()? &m_manifoldsPtr[0] : 0; } btPersistentManifold* getManifoldByIndexInternal(int index) @@ -155,7 +156,17 @@ public: m_collisionConfiguration = config; } + virtual btPoolAllocator* getInternalManifoldPool() + { + return m_persistentManifoldPoolAllocator; + } + + virtual const btPoolAllocator* getInternalManifoldPool() const + { + return m_persistentManifoldPoolAllocator; + } + }; -#endif //COLLISION__DISPATCHER_H +#endif //BT_COLLISION__DISPATCHER_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 5de829824ff..3a11c967ac9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef COLLISION_OBJECT_H -#define COLLISION_OBJECT_H +#ifndef BT_COLLISION_OBJECT_H +#define BT_COLLISION_OBJECT_H #include "LinearMath/btTransform.h" @@ -521,4 +521,4 @@ SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const -#endif //COLLISION_OBJECT_H +#endif //BT_COLLISION_OBJECT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index bfe8d4f52fb..66b93b88efa 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -33,6 +33,7 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "LinearMath/btStackAlloc.h" #include "LinearMath/btSerializer.h" +#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION @@ -154,7 +155,7 @@ void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) minAabb -= contactThreshold; maxAabb += contactThreshold; - if(getDispatchInfo().m_convexMaxDistanceUseCPT) + if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) { btVector3 minAabb2,maxAabb2; colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); @@ -662,68 +663,103 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt 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 + if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE) { - 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) + btConvexCast::CastResult castResult; + castResult.m_allowedPenetration = allowedPenetration; + castResult.m_fraction = resultCallback.m_closestHitFraction; + btStaticPlaneShape* planeShape = (btStaticPlaneShape*) collisionShape; + btContinuousConvexCollision convexCaster1(castShape,planeShape); + 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); + } + } } - - virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) + } 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::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = partId; - shapeInfo.m_triangleIndex = triangleIndex; - if (hitFraction <= m_resultCallback->m_closestHitFraction) + 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) { + } - btCollisionWorld::LocalConvexResult convexResult - (m_collisionObject, - &shapeInfo, - hitNormalLocal, - hitPointLocal, - hitFraction); - bool normalInWorldSpace = false; + 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) + { - return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); - } - return hitFraction; - } + btCollisionWorld::LocalConvexResult convexResult + (m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitPointLocal, + hitFraction); - }; + bool normalInWorldSpace = false; - BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform); - tccb.m_hitFraction = resultCallback.m_closestHitFraction; - tccb.m_allowedPenetration = allowedPenetration; - btVector3 boxMinLocal, boxMaxLocal; - castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); + return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); + } + return hitFraction; + } + + }; - btVector3 rayAabbMinLocal = convexFromLocal; - rayAabbMinLocal.setMin(convexToLocal); - btVector3 rayAabbMaxLocal = convexFromLocal; - rayAabbMaxLocal.setMax(convexToLocal); - rayAabbMinLocal += boxMinLocal; - rayAabbMaxLocal += boxMaxLocal; - concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform); + tccb.m_hitFraction = resultCallback.m_closestHitFraction; + tccb.m_allowedPenetration = allowedPenetration; + 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! @@ -1162,15 +1198,14 @@ public: wv1 = m_worldTrans*triangle[1]; wv2 = m_worldTrans*triangle[2]; btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.); - - btVector3 normal = (wv1-wv0).cross(wv2-wv0); - normal.normalize(); - btVector3 normalColor(1,1,0); - m_debugDrawer->drawLine(center,center+normal,normalColor); - - - - + + if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawNormals ) + { + btVector3 normal = (wv1-wv0).cross(wv2-wv0); + normal.normalize(); + btVector3 normalColor(1,1,0); + m_debugDrawer->drawLine(center,center+normal,normalColor); + } m_debugDrawer->drawLine(wv0,wv1,m_color); m_debugDrawer->drawLine(wv1,wv2,m_color); m_debugDrawer->drawLine(wv2,wv0,m_color); @@ -1195,126 +1230,162 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const } else { - switch (shape->getShapeType()) - { - case BOX_SHAPE_PROXYTYPE: - { - const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape); - btVector3 halfExtents = boxShape->getHalfExtentsWithMargin(); - getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color); - break; - } + /// for polyhedral shapes + if (shape->isPolyhedral()) + { + btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; - case SPHERE_SHAPE_PROXYTYPE: + int i; + if (polyshape->getConvexPolyhedron()) { - const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); - btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin + const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron(); + for (i=0;i<poly->m_faces.size();i++) + { + btVector3 centroid(0,0,0); + int numVerts = poly->m_faces[i].m_indices.size(); + if (numVerts) + { + int lastV = poly->m_faces[i].m_indices[numVerts-1]; + for (int v=0;v<poly->m_faces[i].m_indices.size();v++) + { + int curVert = poly->m_faces[i].m_indices[v]; + centroid+=poly->m_vertices[curVert]; + getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color); + lastV = curVert; + } + } + centroid*= btScalar(1.f)/btScalar(numVerts); + if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals) + { + btVector3 normalColor(1,1,0); + btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]); + getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor); + } + + } - getDebugDrawer()->drawSphere(radius, worldTransform, color); - break; - } - case MULTI_SPHERE_SHAPE_PROXYTYPE: + + } else { - const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape); - - btTransform childTransform; - childTransform.setIdentity(); - - for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) + for (i=0;i<polyshape->getNumEdges();i++) { - childTransform.setOrigin(multiSphereShape->getSpherePosition(i)); - getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color); + btVector3 a,b; + polyshape->getEdge(i,a,b); + btVector3 wa = worldTransform * a; + btVector3 wb = worldTransform * b; + getDebugDrawer()->drawLine(wa,wb,color); } - - break; } - case CAPSULE_SHAPE_PROXYTYPE: - { - const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape); - btScalar radius = capsuleShape->getRadius(); - btScalar halfHeight = capsuleShape->getHalfHeight(); - int upAxis = capsuleShape->getUpAxis(); - getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color); - break; - } - case CONE_SHAPE_PROXYTYPE: + } + else + { + switch (shape->getShapeType()) { - const btConeShape* coneShape = static_cast<const btConeShape*>(shape); - btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); - btScalar height = coneShape->getHeight();//+coneShape->getMargin(); - int upAxis= coneShape->getConeUpIndex(); - getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color); - break; + case BOX_SHAPE_PROXYTYPE: + { + const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape); + btVector3 halfExtents = boxShape->getHalfExtentsWithMargin(); + getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color); + break; + } - } - case CYLINDER_SHAPE_PROXYTYPE: - { - const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); - int upAxis = cylinder->getUpAxis(); - btScalar radius = cylinder->getRadius(); - btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; - getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color); - break; - } + case SPHERE_SHAPE_PROXYTYPE: + { + const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); + btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin - case STATIC_PLANE_PROXYTYPE: - { - const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape); - btScalar planeConst = staticPlaneShape->getPlaneConstant(); - const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); - getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color); - break; + getDebugDrawer()->drawSphere(radius, worldTransform, color); + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + { + const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape); - } - default: - { + btTransform childTransform; + childTransform.setIdentity(); - if (shape->isConcave()) - { - btConcaveShape* concaveMesh = (btConcaveShape*) shape; + for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) + { + childTransform.setOrigin(multiSphereShape->getSpherePosition(i)); + getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color); + } - ///@todo pass camera, for some culling? no -> we are not a graphics lib - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + break; + } + case CAPSULE_SHAPE_PROXYTYPE: + { + const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape); - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); + btScalar radius = capsuleShape->getRadius(); + btScalar halfHeight = capsuleShape->getHalfHeight(); + int upAxis = capsuleShape->getUpAxis(); + getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color); + break; } + case CONE_SHAPE_PROXYTYPE: + { + const btConeShape* coneShape = static_cast<const btConeShape*>(shape); + btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); + btScalar height = coneShape->getHeight();//+coneShape->getMargin(); - if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + int upAxis= coneShape->getConeUpIndex(); + getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color); + break; + + } + case CYLINDER_SHAPE_PROXYTYPE: { - btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; - //todo: pass camera for some culling - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - //DebugDrawcallback drawCallback; - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); + const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); + int upAxis = cylinder->getUpAxis(); + btScalar radius = cylinder->getRadius(); + btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; + getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color); + break; } + case STATIC_PLANE_PROXYTYPE: + { + const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape); + btScalar planeConst = staticPlaneShape->getPlaneConstant(); + const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); + getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color); + break; - /// for polyhedral shapes - if (shape->isPolyhedral()) + } + default: { - btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; - int i; - for (i=0;i<polyshape->getNumEdges();i++) + if (shape->isConcave()) { - btVector3 a,b; - polyshape->getEdge(i,a,b); - btVector3 wa = worldTransform * a; - btVector3 wb = worldTransform * b; - getDebugDrawer()->drawLine(wa,wb,color); + btConcaveShape* concaveMesh = (btConcaveShape*) shape; + + ///@todo pass camera, for some culling? no -> we are not a graphics lib + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + + DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); } + if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + { + btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; + //todo: pass camera for some culling + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + //DebugDrawcallback drawCallback; + DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); + } + + } } } @@ -1327,7 +1398,7 @@ void btCollisionWorld::debugDrawWorld() if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) { int numManifolds = getDispatcher()->getNumManifolds(); - btVector3 color(0,0,0); + btVector3 color(1,0.65,0); for (int i=0;i<numManifolds;i++) { btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i); @@ -1343,7 +1414,7 @@ void btCollisionWorld::debugDrawWorld() } } - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)) + if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))) { int i; @@ -1352,7 +1423,7 @@ void btCollisionWorld::debugDrawWorld() btCollisionObject* colObj = m_collisionObjects[i]; if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0) { - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) + if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)) { btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.)); switch(colObj->getActivationState()) @@ -1386,12 +1457,14 @@ void btCollisionWorld::debugDrawWorld() btVector3 minAabb2,maxAabb2; - colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); - minAabb2 -= contactThreshold; - maxAabb2 += contactThreshold; - - minAabb.setMin(minAabb2); - maxAabb.setMax(maxAabb2); + if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) + { + colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); + minAabb2 -= contactThreshold; + maxAabb2 += contactThreshold; + minAabb.setMin(minAabb2); + maxAabb.setMax(maxAabb2); + } m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index b42e2c40b21..0a92d2d6e15 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -62,8 +62,8 @@ subject to the following restrictions: -#ifndef COLLISION_WORLD_H -#define COLLISION_WORLD_H +#ifndef BT_COLLISION_WORLD_H +#define BT_COLLISION_WORLD_H class btStackAlloc; class btCollisionShape; @@ -506,4 +506,4 @@ public: }; -#endif //COLLISION_WORLD_H +#endif //BT_COLLISION_WORLD_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index 255e0af668c..404574989ab 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef COMPOUND_COLLISION_ALGORITHM_H -#define COMPOUND_COLLISION_ALGORITHM_H +#ifndef BT_COMPOUND_COLLISION_ALGORITHM_H +#define BT_COMPOUND_COLLISION_ALGORITHM_H #include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" @@ -83,4 +83,4 @@ public: }; -#endif //COMPOUND_COLLISION_ALGORITHM_H +#endif //BT_COMPOUND_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h index 5738401401e..53d13b87151 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONVEX_2D_CONVEX_2D_ALGORITHM_H -#define CONVEX_2D_CONVEX_2D_ALGORITHM_H +#ifndef BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H +#define BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H #include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" @@ -92,4 +92,4 @@ public: }; -#endif //CONVEX_2D_CONVEX_2D_ALGORITHM_H +#endif //BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 268ec4b6c7e..d2b2c221426 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -91,7 +91,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody); - +#if 0 ///debug drawing of the overlapping triangles if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe )) { @@ -100,17 +100,8 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i 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); - - //btVector3 center = triangle[0] + triangle[1]+triangle[2]; - //center *= btScalar(0.333333); - //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color); - //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color); - //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color); - } - - - //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject); +#endif if (m_convexBody->getCollisionShape()->isConvex()) { @@ -119,7 +110,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i btCollisionShape* tmpShape = ob->getCollisionShape(); ob->internalSetTemporaryCollisionShape( &tm ); - + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr); if (m_resultOut->getBody0Internal() == m_triBody) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h index 984a4c39e8e..f718d1dec25 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONVEX_CONCAVE_COLLISION_ALGORITHM_H -#define CONVEX_CONCAVE_COLLISION_ALGORITHM_H +#ifndef BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H +#define BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H #include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" @@ -113,4 +113,4 @@ public: }; -#endif //CONVEX_CONCAVE_COLLISION_ALGORITHM_H +#endif //BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 94385fed909..dd1f3e2490f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -17,6 +17,7 @@ subject to the following restrictions: ///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums ///with reproduction case //define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1 +//#define ZERO_MARGIN #include "btConvexConvexAlgorithm.h" @@ -26,6 +27,8 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btCapsuleShape.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" + #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" @@ -48,7 +51,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" - +#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h" /////////// @@ -331,6 +334,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER + + #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { @@ -357,13 +362,14 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl } else #endif //USE_SEPDISTANCE_UTIL2 { - if (dispatchInfo.m_convexMaxDistanceUseCPT) - { - input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); - } else - { - input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); - } + //if (dispatchInfo.m_convexMaxDistanceUseCPT) + //{ + // input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); + //} else + //{ + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); +// } + input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; } @@ -371,7 +377,7 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl input.m_transformA = body0->getWorldTransform(); input.m_transformB = body1->getWorldTransform(); - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + @@ -389,6 +395,155 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl } #endif //USE_SEPDISTANCE_UTIL2 + if (min0->isPolyhedral() && min1->isPolyhedral()) + { + + + struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result + { + virtual void setShapeIdentifiersA(int partId0,int index0){} + virtual void setShapeIdentifiersB(int partId1,int index1){} + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + { + } + }; + + btDummyResult dummy; + + + btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0; + btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1; + if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron()) + { + + + + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + + btScalar minDist = -1e30f; + btVector3 sepNormalWorldSpace; + bool foundSepAxis = true; + + if (dispatchInfo.m_enableSatConvex) + { + foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( + *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0->getWorldTransform(), + body1->getWorldTransform(), + sepNormalWorldSpace); + } 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); +#endif //ZERO_MARGIN + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); + if (l2>SIMD_EPSILON) + { + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); + minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); + +#ifdef ZERO_MARGIN + foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f; +#else + foundSepAxis = gjkPairDetector.getCachedSeparatingDistance()<(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); + + } + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + return; + + } else + { + //we can also deal with convex versus triangle (without connectivity data) + if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE) + { + + 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]); + + //tri->initializePolyhedralFeatures(); + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + + btVector3 sepNormalWorldSpace; + btScalar minDist =-1e30f; + btScalar maxDist = threshold; + + bool foundSepAxis = false; + if (0) + { + polyhedronB->initializePolyhedralFeatures(); + foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( + *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0->getWorldTransform(), + body1->getWorldTransform(), + sepNormalWorldSpace); + // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); + + } else + { +#ifdef ZERO_MARGIN + gjkPairDetector.setIgnoreMargin(true); + gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); +#else + gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); +#endif//ZERO_MARGIN + + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); + if (l2>SIMD_EPSILON) + { + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + //minDist = gjkPairDetector.getCachedSeparatingDistance(); + //maxDist = threshold; + minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); + foundSepAxis = true; + } + } + + + if (foundSepAxis) + { + btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), + body0->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut); + } + + + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + + return; + } + + } + + + } + + gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points @@ -398,66 +553,70 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl int i; btVector3 v0,v1; btVector3 sepNormalWorldSpace; + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); - sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); - btPlaneSpace1(sepNormalWorldSpace,v0,v1); - - - bool perturbeA = true; - const btScalar angleLimit = 0.125f * SIMD_PI; - btScalar perturbeAngle; - btScalar radiusA = min0->getAngularMotionDisc(); - btScalar radiusB = min1->getAngularMotionDisc(); - if (radiusA < radiusB) - { - perturbeAngle = gContactBreakingThreshold /radiusA; - perturbeA = true; - } else + if (l2>SIMD_EPSILON) { - perturbeAngle = gContactBreakingThreshold / radiusB; - perturbeA = false; - } - if ( perturbeAngle > angleLimit ) - perturbeAngle = angleLimit; + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + + btPlaneSpace1(sepNormalWorldSpace,v0,v1); - btTransform unPerturbedTransform; - if (perturbeA) - { - unPerturbedTransform = input.m_transformA; - } else - { - unPerturbedTransform = input.m_transformB; - } - - for ( i=0;i<m_numPerturbationIterations;i++) - { - if (v0.length2()>SIMD_EPSILON) + + bool perturbeA = true; + const btScalar angleLimit = 0.125f * SIMD_PI; + btScalar perturbeAngle; + btScalar radiusA = min0->getAngularMotionDisc(); + btScalar radiusB = min1->getAngularMotionDisc(); + if (radiusA < radiusB) { - btQuaternion perturbeRot(v0,perturbeAngle); - btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); - btQuaternion rotq(sepNormalWorldSpace,iterationAngle); - - + perturbeAngle = gContactBreakingThreshold /radiusA; + perturbeA = true; + } else + { + perturbeAngle = gContactBreakingThreshold / radiusB; + perturbeA = false; + } + if ( perturbeAngle > angleLimit ) + perturbeAngle = angleLimit; + + btTransform unPerturbedTransform; if (perturbeA) { - input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis()); - input.m_transformB = body1->getWorldTransform(); -#ifdef DEBUG_CONTACTS - dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0); -#endif //DEBUG_CONTACTS + unPerturbedTransform = input.m_transformA; } else { - input.m_transformA = body0->getWorldTransform(); - input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis()); -#ifdef DEBUG_CONTACTS - dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0); -#endif + unPerturbedTransform = input.m_transformB; } - btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); - gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); + for ( i=0;i<m_numPerturbationIterations;i++) + { + if (v0.length2()>SIMD_EPSILON) + { + btQuaternion perturbeRot(v0,perturbeAngle); + btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); + btQuaternion rotq(sepNormalWorldSpace,iterationAngle); + + + if (perturbeA) + { + input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis()); + input.m_transformB = body1->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()); + #ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0); + #endif + } + + btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); + gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); + } } - } } @@ -487,7 +646,7 @@ btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, { (void)resultOut; (void)dispatchInfo; - ///rather than checking ALL pairs, only calculate TOI when motion exceeds threshold + ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold ///col0->m_worldTransform, diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index d38aff6862c..4380b80eb4d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONVEX_CONVEX_ALGORITHM_H -#define CONVEX_CONVEX_ALGORITHM_H +#ifndef BT_CONVEX_CONVEX_ALGORITHM_H +#define BT_CONVEX_CONVEX_ALGORITHM_H #include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" @@ -106,4 +106,4 @@ public: }; -#endif //CONVEX_CONVEX_ALGORITHM_H +#endif //BT_CONVEX_CONVEX_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp index dda85dc693f..b2e9bfaf593 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -96,23 +96,41 @@ void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0 if (!m_manifoldPtr) return; - btCollisionObject* convexObj = m_isSwapped? body1 : body0; + btCollisionObject* convexObj = m_isSwapped? body1 : body0; btCollisionObject* planeObj = m_isSwapped? body0: body1; btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); - + bool hasCollision = false; const btVector3& planeNormal = planeShape->getPlaneNormal(); - //const btScalar& planeConstant = planeShape->getPlaneConstant(); + const btScalar& planeConstant = planeShape->getPlaneConstant(); + btTransform planeInConvex; + planeInConvex= convexObj->getWorldTransform().inverse() * planeObj->getWorldTransform(); + btTransform convexInPlaneTrans; + convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexObj->getWorldTransform(); + + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + btVector3 vtxInPlane = convexInPlaneTrans(vtx); + btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); + + btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneWorld = planeObj->getWorldTransform() * vtxInPlaneProjected; - //first perform a collision query with the non-perturbated collision objects + hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); + resultOut->setPersistentManifold(m_manifoldPtr); + if (hasCollision) { - btQuaternion rotq(0,0,0,1); - collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut); + /// report a contact. internally this will be kept persistent, and contact reduction is done + btVector3 normalOnSurfaceB = planeObj->getWorldTransform().getBasis() * planeNormal; + btVector3 pOnB = vtxInPlaneWorld; + resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); } - if (resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold) + //the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones: + //they keep on rolling forever because of the additional off-center contact points + //so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc) + if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold) { btVector3 v0,v1; btPlaneSpace1(planeNormal,v0,v1); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h index f49ac45e772..b9494f5ad3b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONVEX_PLANE_COLLISION_ALGORITHM_H -#define CONVEX_PLANE_COLLISION_ALGORITHM_H +#ifndef BT_CONVEX_PLANE_COLLISION_ALGORITHM_H +#define BT_CONVEX_PLANE_COLLISION_ALGORITHM_H #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -61,7 +61,7 @@ public: CreateFunc() : m_numPerturbationIterations(1), - m_minimumPointsPerturbationThreshold(1) + m_minimumPointsPerturbationThreshold(0) { } @@ -80,5 +80,5 @@ public: }; -#endif //CONVEX_PLANE_COLLISION_ALGORITHM_H +#endif //BT_CONVEX_PLANE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index c27d8ce0752..7faee6faf50 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -296,3 +296,14 @@ void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int nu convexConvex->m_numPerturbationIterations = numPerturbationIterations; convexConvex->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; } + +void btDefaultCollisionConfiguration::setPlaneConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) +{ + btConvexPlaneCollisionAlgorithm::CreateFunc* cpCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_convexPlaneCF; + cpCF->m_numPerturbationIterations = numPerturbationIterations; + cpCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; + + btConvexPlaneCollisionAlgorithm::CreateFunc* pcCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_planeConvexCF; + pcCF->m_numPerturbationIterations = numPerturbationIterations; + pcCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 6aa0d8c270f..81ed424a3db 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -129,6 +129,8 @@ public: ///@todo we could add a per-object setting of those parameters, for level-of-detail collision detection. void setConvexConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3); + void setPlaneConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3); + }; #endif //BT_DEFAULT_COLLISION_CONFIGURATION diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h index e54721dec21..f03c9dc3833 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef EMPTY_ALGORITH -#define EMPTY_ALGORITH +#ifndef BT_EMPTY_ALGORITH +#define BT_EMPTY_ALGORITH #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "btCollisionCreateFunc.h" #include "btCollisionDispatcher.h" @@ -51,4 +51,4 @@ public: } ATTRIBUTE_ALIGNED(16); -#endif //EMPTY_ALGORITH +#endif //BT_EMPTY_ALGORITH diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp index 5cceb04dbb4..4353cdac0b1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -1,6 +1,7 @@ #include "btInternalEdgeUtility.h" #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" @@ -9,7 +10,6 @@ //#define DEBUG_INTERNAL_EDGE - #ifdef DEBUG_INTERNAL_EDGE #include <stdio.h> #endif //DEBUG_INTERNAL_EDGE @@ -456,8 +456,14 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE) return; - btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape(); - btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); + btBvhTriangleMeshShape* trimesh = 0; + + if( colObj0->getRootCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE ) + trimesh = ((btScaledBvhTriangleMeshShape*)colObj0->getRootCollisionShape())->getChildShape(); + else + trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape(); + + btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); if (!triangleInfoMapPtr) return; @@ -501,14 +507,63 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; localContactNormalOnB.normalize();//is this necessary? - - if ((info->m_edgeV0V1Angle)< SIMD_2_PI) + + // Get closest edge + int bestedge=-1; + btScalar disttobestedge=BT_LARGE_FLOAT; + // + // Edge 0 -> 1 + if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest ); + btScalar len=(contact-nearest).length(); + // + if( len < disttobestedge ) + { + bestedge=0; + disttobestedge=len; + } + } + // Edge 1 -> 2 + if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest ); + btScalar len=(contact-nearest).length(); + // + if( len < disttobestedge ) + { + bestedge=1; + disttobestedge=len; + } + } + // Edge 2 -> 0 + if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest ); + btScalar len=(contact-nearest).length(); + // + if( len < disttobestedge ) + { + bestedge=2; + disttobestedge=len; + } + } + +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f); + btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red ); +#endif + if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); #endif btScalar len = (contact-nearest).length(); if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) + if( bestedge==0 ) { btVector3 edge(v0-v1); isNearEdge = true; @@ -577,7 +632,11 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW - if ((info->m_edgeV1V2Angle)< SIMD_2_PI) +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green ); +#endif + + if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); @@ -587,6 +646,7 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* btScalar len = (contact-nearest).length(); if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) + if( bestedge==1 ) { isNearEdge = true; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW @@ -658,8 +718,11 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue ); +#endif - if ((info->m_edgeV2V0Angle)< SIMD_2_PI) + if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW @@ -668,6 +731,7 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* btScalar len = (contact-nearest).length(); if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) + if( bestedge==2 ) { isNearEdge = true; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW @@ -759,11 +823,17 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis()*tri_normal; } else { + btVector3 newNormal = tri_normal *frontFacing; + //if the tri_normal is pointing opposite direction as the current local contact normal, skip it + btScalar d = newNormal.dot(localContactNormalOnB) ; + if (d< 0) + { + return; + } //modify the normal to be the triangle normal (or backfacing normal) - cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *(tri_normal *frontFacing); + cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *newNormal; } - - + // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index fd684c056f7..bf24246ea2f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -64,8 +64,8 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b btAssert(m_manifoldPtr); //order in manifold needs to match -// if (depth > m_manifoldPtr->getContactBreakingThreshold()) - if (depth > m_manifoldPtr->getContactProcessingThreshold()) + if (depth > m_manifoldPtr->getContactBreakingThreshold()) +// if (depth > m_manifoldPtr->getContactProcessingThreshold()) return; bool isSwapped = m_manifoldPtr->getBody0() != m_body0; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 927e2bc4f76..18199b49752 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -14,8 +14,8 @@ subject to the following restrictions: */ -#ifndef MANIFOLD_RESULT_H -#define MANIFOLD_RESULT_H +#ifndef BT_MANIFOLD_RESULT_H +#define BT_MANIFOLD_RESULT_H class btCollisionObject; #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -125,4 +125,4 @@ public: }; -#endif //MANIFOLD_RESULT_H +#endif //BT_MANIFOLD_RESULT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index bb2a7f23985..871c64415a2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -47,6 +47,8 @@ void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btColl { btOverlappingPairCache* pairCachePtr = colWorld->getPairCache(); const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs(); + if (numOverlappingPairs) + { btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr(); for (int i=0;i<numOverlappingPairs;i++) @@ -63,6 +65,7 @@ void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btColl (colObj1)->getIslandTag()); } } + } } } @@ -190,7 +193,7 @@ class btPersistentManifoldSortPredicate { public: - SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) + SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) const { return getIslandId(lhs) < getIslandId(rhs); } @@ -327,11 +330,13 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio //kinematic objects don't merge islands, but wake up all connected objects if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) { - colObj1->activate(); + if (colObj0->hasContactResponse()) + colObj1->activate(); } if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) { - colObj0->activate(); + if (colObj1->hasContactResponse()) + colObj0->activate(); } if(m_splitIslands) { @@ -362,7 +367,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, { btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); int maxNumManifolds = dispatcher->getNumManifolds(); - callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); + callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); } else { @@ -372,8 +377,10 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, int numManifolds = int (m_islandmanifold.size()); - //we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) + //tried a radix sort, but quicksort/heapsort seems still faster + //@todo rewrite island management m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); + //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate()); //now process all active islands (sets of manifolds for now) @@ -427,7 +434,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, if (!islandSleeping) { - callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); + callback->processIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h index d059f5d6b0d..e24c6afeca1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SIMULATION_ISLAND_MANAGER_H -#define SIMULATION_ISLAND_MANAGER_H +#ifndef BT_SIMULATION_ISLAND_MANAGER_H +#define BT_SIMULATION_ISLAND_MANAGER_H #include "BulletCollision/CollisionDispatch/btUnionFind.h" #include "btCollisionCreateFunc.h" @@ -59,7 +59,7 @@ public: { virtual ~IslandCallback() {}; - virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; + virtual void processIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; }; void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback); @@ -77,5 +77,5 @@ public: }; -#endif //SIMULATION_ISLAND_MANAGER_H +#endif //BT_SIMULATION_ISLAND_MANAGER_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h index 47111d1c4af..60286ae0aa4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SPHERE_BOX_COLLISION_ALGORITHM_H -#define SPHERE_BOX_COLLISION_ALGORITHM_H +#ifndef BT_SPHERE_BOX_COLLISION_ALGORITHM_H +#define BT_SPHERE_BOX_COLLISION_ALGORITHM_H #include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -71,5 +71,5 @@ public: }; -#endif //SPHERE_BOX_COLLISION_ALGORITHM_H +#endif //BT_SPHERE_BOX_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h index 7d07512ca66..e55acf277e6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H -#define SPHERE_SPHERE_COLLISION_ALGORITHM_H +#ifndef BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H +#define BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H #include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -62,5 +62,5 @@ public: }; -#endif //SPHERE_SPHERE_COLLISION_ALGORITHM_H +#endif //BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h index 606c3635ae9..7c6c4d8f8d5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H -#define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H +#ifndef BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H +#define BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H #include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -65,5 +65,5 @@ public: }; -#endif //SPHERE_TRIANGLE_COLLISION_ALGORITHM_H +#endif //BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp index 4c4f58d44fa..5222933595d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp @@ -53,7 +53,7 @@ class btUnionFindElementSortPredicate { public: - bool operator() ( const btElement& lhs, const btElement& rhs ) + bool operator() ( const btElement& lhs, const btElement& rhs ) const { return lhs.m_id < rhs.m_id; } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h index 2cce335145b..ef2a29202f7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef UNION_FIND_H -#define UNION_FIND_H +#ifndef BT_UNION_FIND_H +#define BT_UNION_FIND_H #include "LinearMath/btAlignedObjectArray.h" @@ -126,4 +126,4 @@ class btUnionFind }; -#endif //UNION_FIND_H +#endif //BT_UNION_FIND_H |