From 60bf59924610190c35d0d61a9ea89ab2247ebe7c Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Fri, 28 Sep 2012 07:14:48 +0000 Subject: [PATCH] perform GrahamScanConvexHull2D around an arbitrary oriented 2D plane in 3D, to fix some convex hull face merging problems add compound shape support to BulletXmlWorldImporter and fix some compile issue under Debian (hopefully) object picking change in the demos: create a ball-socket picking constraint when holding shift while mouse dragging, otherwise a fixed (6dof) constraint add assert in constraint solver, when both objects have their inertia tensor rows set to zero btPolyhedralContactClipping: add edge-edge contact point in findSeparatingAxis (similar to the default GJK case) --- Demos/BasicDemo/BasicDemo.cpp | 9 +- Demos/BasicDemo/main.cpp | 2 - .../BulletXmlImportDemo.cpp | 24 ++- .../BulletXmlImportDemo/BulletXmlImportDemo.h | 4 + Demos/BulletXmlImportDemo/CMakeLists.txt | 3 - Demos/BulletXmlImportDemo/main.cpp | 8 +- Demos/FractureDemo/CMakeLists.txt | 1 + Demos/FractureDemo/btFractureBody.cpp | 5 +- Demos/OpenGL/DemoApplication.cpp | 9 +- .../BulletWorldImporter/btWorldImporter.cpp | 9 +- .../btBulletXmlWorldImporter.cpp | 37 ++-- .../btBulletXmlWorldImporter.h | 2 +- .../btCollisionDispatcher.cpp | 2 +- .../btConvexConvexAlgorithm.cpp | 63 ++++++- .../CollisionShapes/btConvexHullShape.cpp | 31 ++-- .../CollisionShapes/btConvexHullShape.h | 2 +- .../CollisionShapes/btConvexPolyhedron.cpp | 26 +-- .../CollisionShapes/btConvexPolyhedron.h | 2 +- .../btPolyhedralConvexShape.cpp | 94 +++++----- .../CollisionShapes/btPolyhedralConvexShape.h | 3 +- .../btPolyhedralContactClipping.cpp | 165 ++++++++++++++++-- .../btPolyhedralContactClipping.h | 2 +- .../btSequentialImpulseConstraintSolver.cpp | 5 +- src/LinearMath/btGrahamScan2dConvexHull.h | 30 ++-- 24 files changed, 385 insertions(+), 153 deletions(-) diff --git a/Demos/BasicDemo/BasicDemo.cpp b/Demos/BasicDemo/BasicDemo.cpp index 8f2bfc8ba..ae223626f 100644 --- a/Demos/BasicDemo/BasicDemo.cpp +++ b/Demos/BasicDemo/BasicDemo.cpp @@ -34,6 +34,9 @@ subject to the following restrictions: #include "btBulletDynamicsCommon.h" #include //printf debugging +#include "GLDebugDrawer.h" + +static GLDebugDrawer gDebugDraw; void BasicDemo::clientMoveAndDisplay() @@ -100,11 +103,13 @@ void BasicDemo::initPhysics() m_solver = sol; m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); + m_dynamicsWorld->setDebugDrawer(&gDebugDraw); m_dynamicsWorld->setGravity(btVector3(0,-10,0)); ///create a few basic rigid bodies - btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); + btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); + //groundShape->initializePolyhedralFeatures(); // btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50); m_collisionShapes.push_back(groundShape); @@ -138,7 +143,7 @@ void BasicDemo::initPhysics() //create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance - btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1)); + btBoxShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1)); //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); m_collisionShapes.push_back(colShape); diff --git a/Demos/BasicDemo/main.cpp b/Demos/BasicDemo/main.cpp index bd715e675..d666630a8 100644 --- a/Demos/BasicDemo/main.cpp +++ b/Demos/BasicDemo/main.cpp @@ -19,8 +19,6 @@ subject to the following restrictions: #include "LinearMath/btHashMap.h" - - int main(int argc,char** argv) { diff --git a/Demos/BulletXmlImportDemo/BulletXmlImportDemo.cpp b/Demos/BulletXmlImportDemo/BulletXmlImportDemo.cpp index 3ea21828c..2487749df 100644 --- a/Demos/BulletXmlImportDemo/BulletXmlImportDemo.cpp +++ b/Demos/BulletXmlImportDemo/BulletXmlImportDemo.cpp @@ -28,15 +28,24 @@ subject to the following restrictions: #include //printf debugging + +#include "GLDebugDrawer.h" +GLDebugDrawer gDebugDrawer; + + void BulletXmlImportDemo::initPhysics() { - setTexturing(true); + setTexturing(true); setShadows(true); - setCameraDistance(btScalar(30.)); + setupEmptyDynamicsWorld(); + + m_dynamicsWorld->setDebugDrawer(&gDebugDrawer); + + btBulletXmlWorldImporter* importer = new btBulletXmlWorldImporter(m_dynamicsWorld); importer->loadFile("bullet_basic.xml"); // importer->loadFile("bulletser.xml"); @@ -54,6 +63,8 @@ void BulletXmlImportDemo::clientMoveAndDisplay() ///step the simulation if (m_dynamicsWorld) { + + m_dynamicsWorld->stepSimulation(ms / 1000000.f); m_dynamicsWorld->debugDrawWorld(); } @@ -90,6 +101,7 @@ void BulletXmlImportDemo::setupEmptyDynamicsWorld() ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + btGImpactCollisionAlgorithm::registerAlgorithm(m_dispatcher); m_broadphase = new btDbvtBroadphase(); @@ -101,7 +113,7 @@ void BulletXmlImportDemo::setupEmptyDynamicsWorld() //btGImpactCollisionAlgorithm::registerAlgorithm((btCollisionDispatcher*)m_dynamicsWorld->getDispatcher()); - m_dynamicsWorld->setGravity(btVector3(0,-10,0)); + } @@ -115,7 +127,11 @@ BulletXmlImportDemo::~BulletXmlImportDemo() exitPhysics(); } - +void BulletXmlImportDemo::clientResetScene() +{ + exitPhysics(); + initPhysics(); +} void BulletXmlImportDemo::exitPhysics() diff --git a/Demos/BulletXmlImportDemo/BulletXmlImportDemo.h b/Demos/BulletXmlImportDemo/BulletXmlImportDemo.h index c2eff6321..b107ae0f6 100644 --- a/Demos/BulletXmlImportDemo/BulletXmlImportDemo.h +++ b/Demos/BulletXmlImportDemo/BulletXmlImportDemo.h @@ -55,9 +55,13 @@ class BulletXmlImportDemo : public PlatformDemoApplication BulletXmlImportDemo() { + //m_idle=true; + setCameraDistance(btScalar(30.)); } virtual ~BulletXmlImportDemo(); + virtual void clientResetScene(); + void initPhysics(); void setupEmptyDynamicsWorld(); diff --git a/Demos/BulletXmlImportDemo/CMakeLists.txt b/Demos/BulletXmlImportDemo/CMakeLists.txt index 1636e36d5..8d8eb414b 100644 --- a/Demos/BulletXmlImportDemo/CMakeLists.txt +++ b/Demos/BulletXmlImportDemo/CMakeLists.txt @@ -11,9 +11,6 @@ SET(GLUT_ROOT ${BULLET_PHYSICS_SOURCE_DIR}/Glut) # You shouldn't have to modify anything below this line ######################################################## -IF(BUILD_AMD_OPENCL_DEMOS) - SUBDIRS(AMD) -ENDIF() INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src diff --git a/Demos/BulletXmlImportDemo/main.cpp b/Demos/BulletXmlImportDemo/main.cpp index d70709ded..b20ff8191 100644 --- a/Demos/BulletXmlImportDemo/main.cpp +++ b/Demos/BulletXmlImportDemo/main.cpp @@ -15,12 +15,11 @@ subject to the following restrictions: #include "BulletXmlImportDemo.h" #include "GlutStuff.h" -#include "GLDebugDrawer.h" + #include "btBulletDynamicsCommon.h" #include "LinearMath/btHashMap.h" - #ifdef USE_AMD_OPENCL @@ -84,7 +83,7 @@ bool initCL( void* glCtx, void* glDC ) int main(int argc,char** argv) { - GLDebugDrawer gDebugDrawer; + #ifdef USE_AMD_OPENCL bool initialized = initCL(0,0); @@ -94,8 +93,7 @@ int main(int argc,char** argv) BulletXmlImportDemo serializeDemo; serializeDemo.initPhysics(); - serializeDemo.getDynamicsWorld()->setDebugDrawer(&gDebugDrawer); - + #ifdef CHECK_MEMORY_LEAKS serializeDemo.exitPhysics(); diff --git a/Demos/FractureDemo/CMakeLists.txt b/Demos/FractureDemo/CMakeLists.txt index 487a207da..1f7900b23 100644 --- a/Demos/FractureDemo/CMakeLists.txt +++ b/Demos/FractureDemo/CMakeLists.txt @@ -62,6 +62,7 @@ ENDIF() POST_BUILD # COMMAND copy /Y ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR} COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR} + COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR}/Debug ) ENDIF(CMAKE_CL_64) ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES) diff --git a/Demos/FractureDemo/btFractureBody.cpp b/Demos/FractureDemo/btFractureBody.cpp index 1c5065b77..aa46c2b62 100644 --- a/Demos/FractureDemo/btFractureBody.cpp +++ b/Demos/FractureDemo/btFractureBody.cpp @@ -22,12 +22,13 @@ void btFractureBody::recomputeConnectivity(btCollisionWorld* world) struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback { bool m_connected; - MyContactResultCallback() :m_connected(false) + btScalar m_margin; + MyContactResultCallback() :m_connected(false),m_margin(0.05) { } virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) { - if (cp.getDistance()<=0) + if (cp.getDistance()<=m_margin) m_connected = true; return 1.f; } diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp index 163a13b4e..65541b9d9 100644 --- a/Demos/OpenGL/DemoApplication.cpp +++ b/Demos/OpenGL/DemoApplication.cpp @@ -33,7 +33,7 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" #include "GLDebugFont.h" -static bool use6Dof = false; + extern bool gDisableDeactivation; int numObjects = 0; const int maxNumObjects = 16384; @@ -535,7 +535,7 @@ void DemoApplication::setShootBoxShape () if (!m_shootBoxShape) { btBoxShape* box = new btBoxShape(btVector3(0.5,0.5,0.5)); - box->initializePolyhedralFeatures(); + // box->initializePolyhedralFeatures(); m_shootBoxShape = box; } } @@ -776,7 +776,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y) - if (use6Dof) + if ((m_modifierKeys& BT_ACTIVE_SHIFT)==0) { btTransform tr; tr.setIdentity(); @@ -822,8 +822,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y) } - use6Dof = !use6Dof; - + //save mouse position for dragging gOldPickingPos = rayTo; gHitPos = pickPos; diff --git a/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp index 48e247a08..44c77304d 100644 --- a/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp @@ -215,7 +215,10 @@ btCollisionShape* btWorldImporter::convertCollisionShape( btCollisionShapeData* { case BOX_SHAPE_PROXYTYPE: { - shape = createBoxShape(implicitShapeDimensions/localScaling+margin); + btBoxShape* box= (btBoxShape*)createBoxShape(implicitShapeDimensions/localScaling+margin); + //box->initializePolyhedralFeatures(); + shape = box; + break; } case SPHERE_SHAPE_PROXYTYPE: @@ -333,6 +336,7 @@ btCollisionShape* btWorldImporter::convertCollisionShape( btCollisionShapeData* { hullShape->addPoint(tmpPoints[i]); } + hullShape->setMargin(bsd->m_collisionMargin); //hullShape->initializePolyhedralFeatures(); shape = hullShape; break; @@ -345,7 +349,8 @@ btCollisionShape* btWorldImporter::convertCollisionShape( btCollisionShapeData* if (shape) { - //shape->setMargin(bsd->m_collisionMargin); + shape->setMargin(bsd->m_collisionMargin); + btVector3 localScaling; localScaling.deSerializeFloat(bsd->m_localScaling); shape->setLocalScaling(localScaling); diff --git a/Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.cpp b/Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.cpp index c155f7ca7..66b0f76a6 100644 --- a/Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.cpp +++ b/Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.cpp @@ -238,7 +238,7 @@ void btBulletXmlWorldImporter::deSerializeConvexHullShapeData(TiXmlNode* pParent SET_INT_VALUE(pParent,convexHullData,m_numUnscaledPoints); m_collisionShapeData.push_back((btCollisionShapeData*)convexHullData); - m_pointerLookup.insert(ptr,convexHullData); + m_pointerLookup.insert((void*)ptr,convexHullData); } void btBulletXmlWorldImporter::deSerializeCompoundShapeChildData(TiXmlNode* pParent) @@ -292,7 +292,7 @@ void btBulletXmlWorldImporter::deSerializeCompoundShapeChildData(TiXmlNode* pPar { m_compoundShapeChildDataArrays.push_back(compoundChildArrayPtr); btCompoundShapeChildData* cd = &compoundChildArrayPtr->at(0); - m_pointerLookup.insert(ptr,cd); + m_pointerLookup.insert((void*)ptr,cd); } } @@ -328,7 +328,7 @@ void btBulletXmlWorldImporter::deSerializeCompoundShapeData(TiXmlNode* pParent) SET_FLOAT_VALUE(pParent, compoundData,m_collisionMargin); m_collisionShapeData.push_back((btCollisionShapeData*)compoundData); - m_pointerLookup.insert(ptr,compoundData); + m_pointerLookup.insert((void*)ptr,compoundData); } @@ -348,12 +348,17 @@ void btBulletXmlWorldImporter::deSerializeStaticPlaneShapeData(TiXmlNode* pParen SET_FLOAT_VALUE(pParent, planeData,m_planeConstant); m_collisionShapeData.push_back((btCollisionShapeData*)planeData); - m_pointerLookup.insert(ptr,planeData); + m_pointerLookup.insert((void*)ptr,planeData); } void btBulletXmlWorldImporter::deSerializeDynamicsWorldData(TiXmlNode* pParent) { + btContactSolverInfo solverInfo; + //btVector3 gravity(0,0,0); + + //setDynamicsWorldInfo(gravity,solverInfo); + //gravity and world info } @@ -371,12 +376,13 @@ void btBulletXmlWorldImporter::deSerializeConvexInternalShapeData(TiXmlNode* pPa deSerializeCollisionShapeData(xmlShapeData,&convexShape->m_collisionShapeData); + SET_FLOAT_VALUE(pParent,convexShape,m_collisionMargin) SET_VECTOR4_VALUE(pParent,convexShape,m_localScaling) SET_VECTOR4_VALUE(pParent,convexShape,m_implicitShapeDimensions) m_collisionShapeData.push_back((btCollisionShapeData*)convexShape); - m_pointerLookup.insert(ptr,convexShape); + m_pointerLookup.insert((void*)ptr,convexShape); } @@ -433,7 +439,7 @@ void btBulletXmlWorldImporter::deSerializeGeneric6DofConstraintData(TiXmlNode* p SET_INT_VALUE(pParent, dof6Data,m_useOffsetForConstraintFrame); m_constraintData.push_back((btTypedConstraintData*)dof6Data); - m_pointerLookup.insert(ptr,dof6Data); + m_pointerLookup.insert((void*)ptr,dof6Data); } void btBulletXmlWorldImporter::deSerializeRigidBodyFloatData(TiXmlNode* pParent) @@ -500,7 +506,7 @@ void btBulletXmlWorldImporter::deSerializeRigidBodyFloatData(TiXmlNode* pParent) m_rigidBodyData.push_back(rbData); - m_pointerLookup.insert(ptr,rbData); + m_pointerLookup.insert((void*)ptr,rbData); // rbData->m_collisionObjectData.m_collisionShape = (void*) (int)atof(txt); } @@ -558,13 +564,13 @@ void btBulletXmlWorldImporter::fixupConstraintData(btTypedConstraintData* tcd) { if (tcd->m_rbA) { - btRigidBodyData** ptrptr = (btRigidBodyData**)m_pointerLookup.find((intptr_t)tcd->m_rbA); + btRigidBodyData** ptrptr = (btRigidBodyData**)m_pointerLookup.find(tcd->m_rbA); btAssert(ptrptr); tcd->m_rbA = ptrptr? *ptrptr : 0; } if (tcd->m_rbB) { - btRigidBodyData** ptrptr = (btRigidBodyData**)m_pointerLookup.find((intptr_t)tcd->m_rbB); + btRigidBodyData** ptrptr = (btRigidBodyData**)m_pointerLookup.find(tcd->m_rbB); btAssert(ptrptr); tcd->m_rbB = ptrptr? *ptrptr : 0; } @@ -581,7 +587,7 @@ void btBulletXmlWorldImporter::fixupCollisionDataPointers(btCollisionShapeData* { btCompoundShapeData* compound = (btCompoundShapeData*) shapeData; int ptr = (intptr_t) compound->m_childShapePtr; - void** cdptr = m_pointerLookup.find(ptr); + void** cdptr = m_pointerLookup.find((void*)ptr); btCompoundShapeChildData** c = (btCompoundShapeChildData**)cdptr; btAssert(c); if (c) @@ -598,7 +604,7 @@ void btBulletXmlWorldImporter::fixupCollisionDataPointers(btCollisionShapeData* { btConvexHullShapeData* convexData = (btConvexHullShapeData*)shapeData; int ptr = (intptr_t)convexData->m_unscaledPointsFloatPtr; - btVector3FloatData** ptrptr = (btVector3FloatData**)m_pointerLookup.find(ptr); + btVector3FloatData** ptrptr = (btVector3FloatData**)m_pointerLookup.find((void*)ptr); btAssert(ptrptr); if (ptrptr) { @@ -646,7 +652,7 @@ void btBulletXmlWorldImporter::auto_serialize_root_level_children(TiXmlNode* pPa for (int i=0;isize();c++) { btCompoundShapeChildData* childData = &childDataArray->at(c); - int hashKey = (intptr_t) childData->m_childShape; - btCollisionShapeData** ptrptr = (btCollisionShapeData**)m_pointerLookup[hashKey]; + btCollisionShapeData** ptrptr = (btCollisionShapeData**)m_pointerLookup[childData->m_childShape]; btAssert(ptrptr); if (ptrptr) { @@ -734,8 +739,8 @@ void btBulletXmlWorldImporter::auto_serialize_root_level_children(TiXmlNode* pPa for (int i=0;im_collisionObjectData.m_collisionShape; - void** ptrptr = m_pointerLookup.find(hashKey); + + void** ptrptr = m_pointerLookup.find(rbData->m_collisionObjectData.m_collisionShape); //btAssert(ptrptr); rbData->m_collisionObjectData.m_broadphaseHandle = 0; rbData->m_collisionObjectData.m_rootCollisionShape = 0; diff --git a/Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.h b/Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.h index aad45c33c..034d1911a 100644 --- a/Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.h +++ b/Extras/Serialize/BulletXmlWorldImporter/btBulletXmlWorldImporter.h @@ -43,7 +43,7 @@ protected: btAlignedObjectArray* > m_compoundShapeChildDataArrays; btAlignedObjectArray m_rigidBodyData; btAlignedObjectArray m_constraintData; - btHashMap m_pointerLookup; + btHashMap m_pointerLookup; int m_fileVersion; bool m_fileOk; diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index e5418c0ab..d1ef1ffc9 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -84,7 +84,7 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObj btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(),body1->getContactProcessingThreshold()); - void* mem = 0; + void* mem = 0; if (m_persistentManifoldPoolAllocator->getFreeCount()) { diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index fda3e7a0f..62f98a846 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -407,9 +407,51 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* { } }; + + + 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; @@ -431,32 +473,35 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), - sepNormalWorldSpace); + 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(), @@ -498,7 +543,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), body0Wrap->getWorldTransform(), body1Wrap->getWorldTransform(), - sepNormalWorldSpace); + sepNormalWorldSpace,*resultOut); // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); } else diff --git a/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index 21351986a..4d0ca1451 100644 --- a/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -201,13 +201,11 @@ const char* btConvexHullShape::serialize(void* dataBuffer, btSerializer* seriali return "btConvexHullShapeData"; } -void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const +void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const { #if 1 - min = FLT_MAX; - max = -FLT_MAX; - btVector3 witnesPtMin; - btVector3 witnesPtMax; + minProj = FLT_MAX; + maxProj = -FLT_MAX; int numVerts = m_unscaledPoints.size(); for(int i=0;i max) + if(dp > maxProj) { - max = dp; + maxProj = dp; witnesPtMax=pt; } } #else btVector3 localAxis = dir*trans.getBasis(); - btVector3 vtx1 = trans(localGetSupportingVertex(localAxis)); - btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis)); + witnesPtMin = trans(localGetSupportingVertex(localAxis)); + witnesPtMax = trans(localGetSupportingVertex(-localAxis)); - min = vtx1.dot(dir); - max = vtx2.dot(dir); + minProj = witnesPtMin.dot(dir); + maxProj = witnesPtMax.dot(dir); #endif - if(min>max) + if(minProj>maxProj) { - btScalar tmp = min; - min = max; - max = tmp; + btSwap(minProj,maxProj); + btSwap(witnesPtMin,witnesPtMax); } diff --git a/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/src/BulletCollision/CollisionShapes/btConvexHullShape.h index 95a2af6a3..f4e8f644b 100644 --- a/src/BulletCollision/CollisionShapes/btConvexHullShape.h +++ b/src/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -73,7 +73,7 @@ public: virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; - virtual void project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const; + virtual void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; //debugging diff --git a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp index 112ef7cbe..c31df91a7 100644 --- a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp +++ b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -274,23 +274,29 @@ void btConvexPolyhedron::initialize() #endif } - -void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const +void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const { - min = FLT_MAX; - max = -FLT_MAX; + minProj = FLT_MAX; + maxProj = -FLT_MAX; int numVerts = m_vertices.size(); for(int i=0;i max) max = dp; + if(dp < minProj) + { + minProj = dp; + witnesPtMin = pt; + } + if(dp > maxProj) + { + maxProj = dp; + witnesPtMax = pt; + } } - if(min>max) + if(minProj>maxProj) { - btScalar tmp = min; - min = max; - max = tmp; + btSwap(minProj,maxProj); + btSwap(witnesPtMin,witnesPtMax); } } diff --git a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h index f10c83758..75679eb3b 100644 --- a/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h +++ b/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h @@ -56,7 +56,7 @@ ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron void initialize(); bool testContainment() const; - void project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const; + void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; }; diff --git a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index 83b899136..b32e30a70 100644 --- a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -37,7 +37,7 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape() } -bool btPolyhedralConvexShape::initializePolyhedralFeatures() +bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin) { if (m_polyhedron) @@ -56,31 +56,34 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures() btVector3& newVertex = orgVertices.expand(); getVertex(i,newVertex); } - -#if 0 - btAlignedObjectArray planeEquations; - btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations); - - btAlignedObjectArray shiftedPlaneEquations; - for (int p=0;p planeEquations; + btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations); + + btAlignedObjectArray shiftedPlaneEquations; + for (int p=0;p tmpVertices; + + btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices); + + conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f); + } else + { + + conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f); } - btAlignedObjectArray tmpVertices; - - btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices); - btConvexHullComputer conv; - conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f); - -#else - btConvexHullComputer conv; - conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f); - -#endif - btAlignedObjectArray faceNormals; @@ -193,7 +196,8 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures() { //do the merge: use Graham Scan 2d convex hull - btAlignedObjectArray orgpoints; + btAlignedObjectArray orgpoints; + btVector3 averageFaceNormal(0,0,0); for (int i=0;im_vertices[orgIndex]; - btVector3 rotatedPt = quatRotate(rotationArc,pt); - rotatedPt.setZ(0); + bool found = false; for (int i=0;i hull; - GrahamScanConvexHull2D(orgpoints,hull); + btAlignedObjectArray hull; + + averageFaceNormal.normalize(); + GrahamScanConvexHull2D(orgpoints,hull,averageFaceNormal); for (int i=0;im_faces.push_back(combinedFace); + m_polyhedron->m_faces.push_back(combinedFace); } } if(!did_merge) { for (int i=0;im_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); + btFace face = tmpFaces[coplanarFaceGroup[i]]; + m_polyhedron->m_faces.push_back(face); } - } + } diff --git a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h index cc689a6b1..961d001a9 100644 --- a/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ b/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -40,7 +40,8 @@ public: virtual ~btPolyhedralConvexShape(); ///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges) - virtual bool initializePolyhedralFeatures(); + ///experimental/work-in-progress + virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin=0); const btConvexPolyhedron* getConvexPolyhedron() const { diff --git a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp index 33a8a0639..2be45550e 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp @@ -77,12 +77,15 @@ void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertex } -static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth) +static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth, btVector3& witnessPointA, btVector3& witnessPointB) { btScalar Min0,Max0; btScalar Min1,Max1; - hullA.project(transA,sep_axis, Min0, Max0); - hullB.project(transB, sep_axis, Min1, Max1); + btVector3 witnesPtMinA,witnesPtMaxA; + btVector3 witnesPtMinB,witnesPtMaxB; + + hullA.project(transA,sep_axis, Min0, Max0,witnesPtMinA,witnesPtMaxA); + hullB.project(transB, sep_axis, Min1, Max1,witnesPtMinB,witnesPtMaxB); if(Max0=0.0f); btScalar d1 = Max1 - Min0; btAssert(d1>=0.0f); - depth = d0 hlenA ) + tA = hlenA; + } + + tB = tA * dirA_dot_dirB - dirB_dot_trans; + + if ( tB < -hlenB ) { + tB = -hlenB; + tA = tB * dirA_dot_dirB + dirA_dot_trans; + + if ( tA < -hlenA ) + tA = -hlenA; + else if ( tA > hlenA ) + tA = hlenA; + } else if ( tB > hlenB ) { + tB = hlenB; + tA = tB * dirA_dot_dirB + dirA_dot_trans; + + if ( tA < -hlenA ) + tA = -hlenA; + else if ( tA > hlenA ) + tA = hlenA; + } + + // compute the closest points relative to segment centers. + + offsetA = dirA * tA; + offsetB = dirB * tB; + + ptsVector = translation - offsetA + offsetB; +} -bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep) + + +bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut) { gActualSATPairTests++; @@ -182,9 +255,9 @@ bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& for(int i=0;i0.0f) + if (edgeA>=0&&edgeB>=0) + { +// printf("edge-edge\n"); + //add an edge-edge contact + + btVector3 ptsVector; + btVector3 offsetA; + btVector3 offsetB; + btScalar tA; + btScalar tB; + + btVector3 translation = witnessPointB-witnessPointA; + + btVector3 dirA = worldEdgeA; + btVector3 dirB = worldEdgeB; + + btScalar hlenB = 1e30f; + btScalar hlenA = 1e30f; + + btSegmentsClosestPoints(ptsVector,offsetA,offsetB,tA,tB, + translation, + dirA, hlenA, + dirB,hlenB); + + btScalar nlSqrt = ptsVector.length2(); + if (nlSqrt>SIMD_EPSILON) + { + btScalar nl = btSqrt(nlSqrt); + ptsVector *= 1.f/nl; + if (ptsVector.dot(DeltaC2)<0.f) + { + ptsVector*=-1.f; + } + btVector3 ptOnB = witnessPointB + offsetB; + btScalar distance = nl; + resultOut.addContactPoint(ptsVector, ptOnB,-distance); + } + + } + + + if((DeltaC2.dot(sep))<0.0f) sep = -sep; return true; @@ -359,8 +488,8 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin()); for (int i=0;isize();i++) { - - btScalar depth = planeNormalWS.dot(pVtxIn->at(i))+planeEqWS; + btVector3 vtx = pVtxIn->at(i); + btScalar depth = planeNormalWS.dot(vtx)+planeEqWS; if (depth <=minDist) { // printf("clamped: depth=%f to minDist=%f\n",depth,minDist); @@ -395,6 +524,9 @@ void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatin } + + + void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) { @@ -404,6 +536,7 @@ void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatin //const btVector3 DeltaC2 = c0 - c1; + int closestFaceB=-1; btScalar dmax = -FLT_MAX; { diff --git a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h index 99103df20..b87bd4f32 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h @@ -35,7 +35,7 @@ struct btPolyhedralContactClipping static void clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut); static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut); - static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep); + static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep, btDiscreteCollisionDetectorInterface::Result& resultOut); ///the clipFace method is used internally static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS); diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index ae5da36a3..cd8b87469 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -1146,8 +1146,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); sum += iMJlB.dot(solverConstraint.m_contactNormal); sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); - - solverConstraint.m_jacDiagABInv = btScalar(1.)/sum; + btScalar fsum = btFabs(sum); + btAssert(fsum > SIMD_EPSILON); + solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f; } diff --git a/src/LinearMath/btGrahamScan2dConvexHull.h b/src/LinearMath/btGrahamScan2dConvexHull.h index d227148df..fb5ebb7b8 100644 --- a/src/LinearMath/btGrahamScan2dConvexHull.h +++ b/src/LinearMath/btGrahamScan2dConvexHull.h @@ -21,9 +21,9 @@ subject to the following restrictions: #include "btVector3.h" #include "btAlignedObjectArray.h" -struct GrahamVector2 : public btVector3 +struct GrahamVector3 : public btVector3 { - GrahamVector2(const btVector3& org, int orgIndex) + GrahamVector3(const btVector3& org, int orgIndex) :btVector3(org), m_orgIndex(orgIndex) { @@ -39,7 +39,7 @@ struct btAngleCompareFunc { : m_anchor(anchor) { } - bool operator()(const GrahamVector2& a, const GrahamVector2& b) const { + bool operator()(const GrahamVector3& a, const GrahamVector3& b) const { if (a.m_angle != b.m_angle) return a.m_angle < b.m_angle; else @@ -56,32 +56,38 @@ struct btAngleCompareFunc { } }; -inline void GrahamScanConvexHull2D(btAlignedObjectArray& originalPoints, btAlignedObjectArray& hull) +inline void GrahamScanConvexHull2D(btAlignedObjectArray& originalPoints, btAlignedObjectArray& hull, const btVector3& normalAxis) { + btVector3 axis0,axis1; + btPlaneSpace1(normalAxis,axis0,axis1); + + if (originalPoints.size()<=1) { for (int i=0;i& original while (!isConvex&& hull.size()>1) { btVector3& a = hull[hull.size()-2]; btVector3& b = hull[hull.size()-1]; - isConvex = btCross(a-b,a-originalPoints[i]).dot(btVector3(0,0,1))> 0; + isConvex = btCross(a-b,a-originalPoints[i]).dot(normalAxis)> 0; if (!isConvex) hull.pop_back(); else