diff --git a/Demos/Box2dDemo/Box2dDemo.cpp b/Demos/Box2dDemo/Box2dDemo.cpp index c94f2943f..d1c698bed 100644 --- a/Demos/Box2dDemo/Box2dDemo.cpp +++ b/Demos/Box2dDemo/Box2dDemo.cpp @@ -13,13 +13,18 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#include "btBox2dShape.h" +#include "BulletCollision/CollisionShapes/btBox2dShape.h" #include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" -#include "btBox2dBox2dCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h" + + +#include "BulletCollision/CollisionShapes/btConvex2dShape.h" +#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" ///create 125 (5x5x5) dynamic object -#define ARRAY_SIZE_X 17 -#define ARRAY_SIZE_Y 17 +#define ARRAY_SIZE_X 1 +#define ARRAY_SIZE_Y 2 #define ARRAY_SIZE_Z 1 //maximum number of objects (and allow user to shoot additional boxes) @@ -97,10 +102,19 @@ void Box2dDemo::initPhysics() ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + btVoronoiSimplexSolver* simplex = new btVoronoiSimplexSolver(); + btMinkowskiPenetrationDepthSolver* pdSolver = new btMinkowskiPenetrationDepthSolver(); + + + btConvex2dConvex2dAlgorithm::CreateFunc* convexAlgo2d = new btConvex2dConvex2dAlgorithm::CreateFunc(simplex,pdSolver); - m_dispatcher->registerCollisionCreateFunc(CUSTOM_CONVEX_SHAPE_TYPE,CUSTOM_CONVEX_SHAPE_TYPE,new btBox2dBox2dCollisionAlgorithm::CreateFunc); + m_dispatcher->registerCollisionCreateFunc(CONVEX_2D_SHAPE_PROXYTYPE,CONVEX_2D_SHAPE_PROXYTYPE,convexAlgo2d); + m_dispatcher->registerCollisionCreateFunc(BOX_2D_SHAPE_PROXYTYPE,CONVEX_2D_SHAPE_PROXYTYPE,convexAlgo2d); + m_dispatcher->registerCollisionCreateFunc(CONVEX_2D_SHAPE_PROXYTYPE,BOX_2D_SHAPE_PROXYTYPE,convexAlgo2d); + m_dispatcher->registerCollisionCreateFunc(BOX_2D_SHAPE_PROXYTYPE,BOX_2D_SHAPE_PROXYTYPE,new btBox2dBox2dCollisionAlgorithm::CreateFunc()); m_broadphase = new btDbvtBroadphase(); + //m_broadphase = new btSimpleBroadphase(); ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver; @@ -149,11 +163,24 @@ void Box2dDemo::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)); - btCollisionShape* colShape = new btBox2dShape(btVector3(SCALING*1,SCALING*1,0.)); - colShape->setMargin(0.); + btScalar u = 1*SCALING-0.04; + btVector3 points[3] = {btVector3(0,u,0),btVector3(-u,-u,0),btVector3(u,-u,0)}; + btConvexShape* colShape= new btConvex2dShape(new btBoxShape(btVector3(SCALING*1,SCALING*1,0.04))); + //btCollisionShape* colShape = new btBox2dShape(btVector3(SCALING*1,SCALING*1,0.04)); + + btConvexShape* colShape2= new btConvex2dShape(new btConvexHullShape(&points[0].getX(),3)); + btConvexShape* colShape3= new btConvex2dShape(new btCylinderShapeZ(btVector3(SCALING*1,SCALING*1,0.04))); + + + + + + //btUniformScalingShape* colShape = new btUniformScalingShape(convexColShape,1.f); + colShape->setMargin(0.03); //btCollisionShape* colShape = new btSphereShape(btScalar(1.)); m_collisionShapes.push_back(colShape); + m_collisionShapes.push_back(colShape2); + /// Create Dynamic Objects btTransform startTransform; @@ -183,12 +210,25 @@ void Box2dDemo::initPhysics() for (int j = i; j < ARRAY_SIZE_Y; ++j) { - startTransform.setOrigin(y); + startTransform.setOrigin(y-btVector3(-10,0,0)); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); - btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia); + btRigidBody::btRigidBodyConstructionInfo rbInfo(0,0,0); + switch (j%3) + { +#if 0 + case 0: + rbInfo = btRigidBody::btRigidBodyConstructionInfo(mass,myMotionState,colShape,localInertia); + break; + case 1: + rbInfo = btRigidBody::btRigidBodyConstructionInfo(mass,myMotionState,colShape3,localInertia); + break; +#endif + default: + rbInfo = btRigidBody::btRigidBodyConstructionInfo(mass,myMotionState,colShape3,localInertia); + } btRigidBody* body = new btRigidBody(rbInfo); //body->setContactProcessingThreshold(colShape->getContactBreakingThreshold()); body->setActivationState(ISLAND_SLEEPING); @@ -199,7 +239,8 @@ void Box2dDemo::initPhysics() body->setActivationState(ISLAND_SLEEPING); - y += deltaY; + y += -0.8*deltaY; + //y += deltaY; } x += deltaX; diff --git a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index be261ec40..5e7584dbc 100644 --- a/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -46,6 +46,8 @@ IMPLICIT_CONVEX_SHAPES_START_HERE, UNIFORM_SCALING_SHAPE_PROXYTYPE, MINKOWSKI_SUM_SHAPE_PROXYTYPE, MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE, + BOX_2D_SHAPE_PROXYTYPE, + CONVEX_2D_SHAPE_PROXYTYPE, CUSTOM_CONVEX_SHAPE_TYPE, //concave shapes CONCAVE_SHAPES_START_HERE, @@ -152,6 +154,12 @@ BT_DECLARE_ALIGNED_ALLOCATOR(); { return (proxyType == STATIC_PLANE_PROXYTYPE); } + + static SIMD_FORCE_INLINE bool isConvex2d(int proxyType) + { + return (proxyType == BOX_2D_SHAPE_PROXYTYPE) || (proxyType == CONVEX_2D_SHAPE_PROXYTYPE); + } + } ; diff --git a/Demos/Box2dDemo/btBox2dBox2dCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp similarity index 95% rename from Demos/Box2dDemo/btBox2dBox2dCollisionAlgorithm.cpp rename to src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp index 0654d7be2..757a35973 100644 --- a/Demos/Box2dDemo/btBox2dBox2dCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btBoxShape.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h" -#include "btBox2dShape.h" +#include "BulletCollision/CollisionShapes/btBox2dShape.h" #define USE_PERSISTENT_CONTACTS 1 @@ -51,7 +51,7 @@ btBox2dBox2dCollisionAlgorithm::~btBox2dBox2dCollisionAlgorithm() void b2CollidePolygons(btManifoldResult* manifold, const btBox2dShape* polyA, const btTransform& xfA, const btBox2dShape* polyB, const btTransform& xfB); -#include +//#include void btBox2dBox2dCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { if (!m_manifoldPtr) diff --git a/Demos/Box2dDemo/btBox2dBox2dCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h similarity index 100% rename from Demos/Box2dDemo/btBox2dBox2dCollisionAlgorithm.h rename to src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h diff --git a/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp new file mode 100644 index 000000000..9d5d530c7 --- /dev/null +++ b/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp @@ -0,0 +1,247 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btConvex2dConvex2dAlgorithm.h" + +//#include +#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" + + +#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionDispatch/btManifoldResult.h" + +#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h" +#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" +#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" + + + +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" + +#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" + +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" + + +btConvex2dConvex2dAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver) +{ + m_numPerturbationIterations = 0; + m_minimumPointsPerturbationThreshold = 3; + m_simplexSolver = simplexSolver; + m_pdSolver = pdSolver; +} + +btConvex2dConvex2dAlgorithm::CreateFunc::~CreateFunc() +{ +} + +btConvex2dConvex2dAlgorithm::btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold) +: btActivatingCollisionAlgorithm(ci,body0,body1), +m_simplexSolver(simplexSolver), +m_pdSolver(pdSolver), +m_ownManifold (false), +m_manifoldPtr(mf), +m_lowLevelOfDetail(false), + m_numPerturbationIterations(numPerturbationIterations), +m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold) +{ + (void)body0; + (void)body1; +} + + + + +btConvex2dConvex2dAlgorithm::~btConvex2dConvex2dAlgorithm() +{ + if (m_ownManifold) + { + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); + } +} + +void btConvex2dConvex2dAlgorithm ::setLowLevelOfDetail(bool useLowLevel) +{ + m_lowLevelOfDetail = useLowLevel; +} + + + +extern btScalar gContactBreakingThreshold; + + +// +// Convex-Convex collision algorithm +// +void btConvex2dConvex2dAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + + if (!m_manifoldPtr) + { + //swapped? + m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1); + m_ownManifold = true; + } + resultOut->setPersistentManifold(m_manifoldPtr); + + //comment-out next line to test multi-contact generation + //resultOut->getPersistentManifold()->clearManifold(); + + + btConvexShape* min0 = static_cast(body0->getCollisionShape()); + btConvexShape* min1 = static_cast(body1->getCollisionShape()); + + btVector3 normalOnB; + btVector3 pointOnBWorld; + + { + + + btGjkPairDetector::ClosestPointInput input; + + btGjkPairDetector gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver); + //TODO: if (dispatchInfo.m_useContinuous) + gjkPairDetector.setMinkowskiA(min0); + gjkPairDetector.setMinkowskiB(min1); + + { + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); + input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; + } + + input.m_stackAlloc = dispatchInfo.m_stackAllocator; + input.m_transformA = body0->getWorldTransform(); + input.m_transformB = body1->getWorldTransform(); + + gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + + btVector3 v0,v1; + btVector3 sepNormalWorldSpace; + + } + + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + +} + + + + +btScalar btConvex2dConvex2dAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +{ + (void)resultOut; + (void)dispatchInfo; + ///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, + btScalar resultFraction = btScalar(1.); + + + btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2(); + btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2(); + + if (squareMot0 < col0->getCcdSquareMotionThreshold() && + squareMot1 < col1->getCcdSquareMotionThreshold()) + return resultFraction; + + + //An adhoc way of testing the Continuous Collision Detection algorithms + //One object is approximated as a sphere, to simplify things + //Starting in penetration should report no time of impact + //For proper CCD, better accuracy and handling of 'allowed' penetration should be added + //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) + + + /// Convex0 against sphere for Convex1 + { + btConvexShape* convex0 = static_cast(col0->getCollisionShape()); + + btSphereShape sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btConvexCast::CastResult result; + btVoronoiSimplexSolver voronoiSimplex; + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + ///Simplification, one object is simplified as a sphere + btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex); + //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), + col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + { + + //store result.m_fraction in both bodies + + if (col0->getHitFraction()> result.m_fraction) + col0->setHitFraction( result.m_fraction ); + + if (col1->getHitFraction() > result.m_fraction) + col1->setHitFraction( result.m_fraction); + + if (resultFraction > result.m_fraction) + resultFraction = result.m_fraction; + + } + + + + + } + + /// Sphere (for convex0) against Convex1 + { + btConvexShape* convex1 = static_cast(col1->getCollisionShape()); + + btSphereShape sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation + btConvexCast::CastResult result; + btVoronoiSimplexSolver voronoiSimplex; + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + ///Simplification, one object is simplified as a sphere + btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex); + //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); + if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(), + col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result)) + { + + //store result.m_fraction in both bodies + + if (col0->getHitFraction() > result.m_fraction) + col0->setHitFraction( result.m_fraction); + + if (col1->getHitFraction() > result.m_fraction) + col1->setHitFraction( result.m_fraction); + + if (resultFraction > result.m_fraction) + resultFraction = result.m_fraction; + + } + } + + return resultFraction; + +} + diff --git a/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h b/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h new file mode 100644 index 000000000..f3916bcc5 --- /dev/null +++ b/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h @@ -0,0 +1,95 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef CONVEX_2D_CONVEX_2D_ALGORITHM_H +#define CONVEX_2D_CONVEX_2D_ALGORITHM_H + +#include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" +#include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "LinearMath/btTransformUtil.h" //for btConvexSeparatingDistanceUtil + +class btConvexPenetrationDepthSolver; + + +///The convex2dConvex2dAlgorithm collision algorithm support 2d collision detection for btConvex2dShape +///Currently it requires the btMinkowskiPenetrationDepthSolver, it has support for 2d penetration depth computation +class btConvex2dConvex2dAlgorithm : public btActivatingCollisionAlgorithm +{ + btSimplexSolverInterface* m_simplexSolver; + btConvexPenetrationDepthSolver* m_pdSolver; + + + bool m_ownManifold; + btPersistentManifold* m_manifoldPtr; + bool m_lowLevelOfDetail; + + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + +public: + + btConvex2dConvex2dAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver, int numPerturbationIterations, int minimumPointsPerturbationThreshold); + + + virtual ~btConvex2dConvex2dAlgorithm(); + + virtual void processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual btScalar calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut); + + virtual void getAllContactManifolds(btManifoldArray& manifoldArray) + { + ///should we use m_ownManifold to avoid adding duplicates? + if (m_manifoldPtr && m_ownManifold) + manifoldArray.push_back(m_manifoldPtr); + } + + + void setLowLevelOfDetail(bool useLowLevel); + + + const btPersistentManifold* getManifold() + { + return m_manifoldPtr; + } + + struct CreateFunc :public btCollisionAlgorithmCreateFunc + { + + btConvexPenetrationDepthSolver* m_pdSolver; + btSimplexSolverInterface* m_simplexSolver; + int m_numPerturbationIterations; + int m_minimumPointsPerturbationThreshold; + + CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver); + + virtual ~CreateFunc(); + + virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) + { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvex2dConvex2dAlgorithm)); + return new(mem) btConvex2dConvex2dAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver,m_numPerturbationIterations,m_minimumPointsPerturbationThreshold); + } + }; + + +}; + +#endif //CONVEX_2D_CONVEX_2D_ALGORITHM_H diff --git a/Demos/Box2dDemo/btBox2dShape.cpp b/src/BulletCollision/CollisionShapes/btBox2dShape.cpp similarity index 100% rename from Demos/Box2dDemo/btBox2dShape.cpp rename to src/BulletCollision/CollisionShapes/btBox2dShape.cpp diff --git a/Demos/Box2dDemo/btBox2dShape.h b/src/BulletCollision/CollisionShapes/btBox2dShape.h similarity index 95% rename from Demos/Box2dDemo/btBox2dShape.h rename to src/BulletCollision/CollisionShapes/btBox2dShape.h index aa24b7a7e..a282e2302 100644 --- a/Demos/Box2dDemo/btBox2dShape.h +++ b/src/BulletCollision/CollisionShapes/btBox2dShape.h @@ -97,7 +97,7 @@ public: m_normals[2].setValue(0,1,0); m_normals[3].setValue(-1,0,0); - m_shapeType = CUSTOM_CONVEX_SHAPE_TYPE; + m_shapeType = BOX_2D_SHAPE_PROXYTYPE; btVector3 margin(getMargin(),getMargin(),getMargin()); m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; }; diff --git a/src/BulletCollision/CollisionShapes/btCollisionShape.h b/src/BulletCollision/CollisionShapes/btCollisionShape.h index 37f74ae46..215758ef5 100644 --- a/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -60,6 +60,11 @@ public: return btBroadphaseProxy::isPolyhedral(getShapeType()); } + SIMD_FORCE_INLINE bool isConvex2d() const + { + return btBroadphaseProxy::isConvex2d(getShapeType()); + } + SIMD_FORCE_INLINE bool isConvex() const { return btBroadphaseProxy::isConvex(getShapeType()); diff --git a/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp b/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp new file mode 100644 index 000000000..1b8e3a670 --- /dev/null +++ b/src/BulletCollision/CollisionShapes/btConvex2dShape.cpp @@ -0,0 +1,92 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btConvex2dShape.h" + +btConvex2dShape::btConvex2dShape( btConvexShape* convexChildShape): +btConvexShape (), m_childConvexShape(convexChildShape) +{ + m_shapeType = CONVEX_2D_SHAPE_PROXYTYPE; +} + +btConvex2dShape::~btConvex2dShape() +{ +} + + + +btVector3 btConvex2dShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const +{ + return m_childConvexShape->localGetSupportingVertexWithoutMargin(vec); +} + +void btConvex2dShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const +{ + m_childConvexShape->batchedUnitVectorGetSupportingVertexWithoutMargin(vectors,supportVerticesOut,numVectors); +} + + +btVector3 btConvex2dShape::localGetSupportingVertex(const btVector3& vec)const +{ + return m_childConvexShape->localGetSupportingVertex(vec); +} + + +void btConvex2dShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + ///this linear upscaling is not realistic, but we don't deal with large mass ratios... + m_childConvexShape->calculateLocalInertia(mass,inertia); +} + + + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version +void btConvex2dShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + m_childConvexShape->getAabb(t,aabbMin,aabbMax); +} + +void btConvex2dShape::getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + m_childConvexShape->getAabbSlow(t,aabbMin,aabbMax); +} + +void btConvex2dShape::setLocalScaling(const btVector3& scaling) +{ + m_childConvexShape->setLocalScaling(scaling); +} + +const btVector3& btConvex2dShape::getLocalScaling() const +{ + return m_childConvexShape->getLocalScaling(); +} + +void btConvex2dShape::setMargin(btScalar margin) +{ + m_childConvexShape->setMargin(margin); +} +btScalar btConvex2dShape::getMargin() const +{ + return m_childConvexShape->getMargin(); +} + +int btConvex2dShape::getNumPreferredPenetrationDirections() const +{ + return m_childConvexShape->getNumPreferredPenetrationDirections(); +} + +void btConvex2dShape::getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const +{ + m_childConvexShape->getPreferredPenetrationDirection(index,penetrationVector); +} diff --git a/src/BulletCollision/CollisionShapes/btConvex2dShape.h b/src/BulletCollision/CollisionShapes/btConvex2dShape.h new file mode 100644 index 000000000..eb8fa8c46 --- /dev/null +++ b/src/BulletCollision/CollisionShapes/btConvex2dShape.h @@ -0,0 +1,80 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_CONVEX_2D_SHAPE_H +#define BT_CONVEX_2D_SHAPE_H + +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types + +///The btConvex2dShape allows to use arbitrary convex shapes are 2d convex shapes, with the Z component assumed to be 0. +///For 2d boxes, the btBox2dShape is recommended. +class btConvex2dShape : public btConvexShape +{ + btConvexShape* m_childConvexShape; + + public: + + btConvex2dShape( btConvexShape* convexChildShape); + + virtual ~btConvex2dShape(); + + virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; + + virtual btVector3 localGetSupportingVertex(const btVector3& vec)const; + + virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + + btConvexShape* getChildShape() + { + return m_childConvexShape; + } + + const btConvexShape* getChildShape() const + { + return m_childConvexShape; + } + + virtual const char* getName()const + { + return "Convex2dShape"; + } + + + + /////////////////////////// + + + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version + void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + virtual void getAabbSlow(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + + virtual void setLocalScaling(const btVector3& scaling) ; + virtual const btVector3& getLocalScaling() const ; + + virtual void setMargin(btScalar margin); + virtual btScalar getMargin() const; + + virtual int getNumPreferredPenetrationDirections() const; + + virtual void getPreferredPenetrationDirection(int index, btVector3& penetrationVector) const; + + +}; + +#endif //BT_CONVEX_2D_SHAPE_H diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 8f58df390..1a5619573 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -39,7 +39,8 @@ int gNumGjkChecks = 0; btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_penetrationDepthSolver(penetrationDepthSolver), +:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)), +m_penetrationDepthSolver(penetrationDepthSolver), m_simplexSolver(simplexSolver), m_minkowskiA(objectA), m_minkowskiB(objectB), @@ -53,7 +54,7 @@ m_catchDegeneracies(1) { } btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,int shapeTypeA,int shapeTypeB,btScalar marginA, btScalar marginB, btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver) -:m_cachedSeparatingAxis(btScalar(0.),btScalar(0.),btScalar(1.)), +:m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)), m_penetrationDepthSolver(penetrationDepthSolver), m_simplexSolver(simplexSolver), m_minkowskiA(objectA), @@ -92,6 +93,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu localTransA.getOrigin() -= positionOffset; localTransB.getOrigin() -= positionOffset; + bool check2d = m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d(); btScalar marginA = m_marginA; btScalar marginB = m_marginB; @@ -171,12 +173,19 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu spu_printf("got local supporting vertices\n"); #endif + if (check2d) + { + pWorld[2] = 0.f; + qWorld[2] = 0.f; + } + btVector3 w = pWorld - qWorld; delta = m_cachedSeparatingAxis.dot(w); // potential exit, they don't overlap if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) { + m_degenerateSimplex = 10; checkSimplex=true; //checkPenetration = false; break; @@ -198,6 +207,9 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu if (f0 <= btScalar(0.)) { m_degenerateSimplex = 2; + } else + { + m_degenerateSimplex = 11; } checkSimplex = true; break; @@ -231,6 +243,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu btScalar previousSquaredDistance = squaredDistance; squaredDistance = newCachedSeparatingAxis.length2(); +#if 0 +///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo if (squaredDistance>previousSquaredDistance) { m_degenerateSimplex = 7; @@ -238,6 +252,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu checkSimplex = false; break; } +#endif // m_cachedSeparatingAxis = newCachedSeparatingAxis; @@ -248,6 +263,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu { m_simplexSolver->backup_closest(m_cachedSeparatingAxis); checkSimplex = true; + m_degenerateSimplex = 12; + break; } @@ -278,6 +295,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu { //do we need this backup_closest here ? m_simplexSolver->backup_closest(m_cachedSeparatingAxis); + m_degenerateSimplex = 13; break; } } @@ -286,7 +304,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu { m_simplexSolver->compute_points(pointOnA, pointOnB); normalInB = pointOnA-pointOnB; - btScalar lenSqr = m_cachedSeparatingAxis.length2(); + btScalar lenSqr =m_cachedSeparatingAxis.length2(); + //valid normal if (lenSqr < 0.0001) { @@ -390,6 +409,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu pointOnA -= m_cachedSeparatingAxis * marginA ; pointOnB += m_cachedSeparatingAxis * marginB ; normalInB = m_cachedSeparatingAxis; + normalInB.normalize(); isValid = true; m_lastUsedMethod = 6; } else @@ -404,16 +424,19 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu } } - if (isValid) + + + if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared))) { -#ifdef __SPU__ - //spu_printf("distance\n"); -#endif //__CELLOS_LV2__ +#if 0 +///some debugging +// if (check2d) + { + printf("n = %2.3f,%2.3f,%2.3f. ",normalInB[0],normalInB[1],normalInB[2]); + printf("distance = %2.3f exit=%d deg=%d\n",distance,m_lastUsedMethod,m_degenerateSimplex); + } +#endif - -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("output 1\n"); -#endif m_cachedSeparatingAxis = normalInB; m_cachedSeparatingDistance = distance; @@ -422,10 +445,6 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu pointOnB+positionOffset, distance); -#ifdef DEBUG_SPU_COLLISION_DETECTION - spu_printf("output 2\n"); -#endif - //printf("gjk add:%f",distance); } diff --git a/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp b/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp index 0dff93fa1..8b8238251 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.cpp @@ -78,6 +78,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s (void)stackAlloc; (void)v; + bool check2d= convexA->isConvex2d() && convexB->isConvex2d(); struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result { @@ -132,7 +133,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s for (i=0;ibatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections); convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections); for (i=0;i0.01) + { + + seperatingAxisInA = seperatingAxisInABatch[i]; + seperatingAxisInB = seperatingAxisInBBatch[i]; + + pInA = supportVerticesABatch[i]; + qInB = supportVerticesBBatch[i]; + + pWorld = transA(pInA); + qWorld = transB(qInB); + if (check2d) + { + pWorld[2] = 0.f; + qWorld[2] = 0.f; + } + + w = qWorld - pWorld; + btScalar delta = norm.dot(w); + //find smallest delta + if (delta < minProj) + { + minProj = delta; + minNorm = norm; + minA = pWorld; + minB = qWorld; + } } } #else @@ -264,7 +280,8 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s if (minProj < btScalar(0.)) return false; - minProj += (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); + btScalar extraSeparation = 0.5f;///scale dependent + minProj += extraSeparation+(convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual()); @@ -305,6 +322,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);//minProj; btIntermediateResult res; + gjkdet.setCachedSeperatingAxis(-minNorm); gjkdet.getClosestPoints(input,res,debugDraw); btScalar correctedMinNorm = minProj - res.m_depth; @@ -313,12 +331,14 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s //the penetration depth is over-estimated, relax it btScalar penetration_relaxation= btScalar(1.); minNorm*=penetration_relaxation; + if (res.m_hasResult) { pa = res.m_pointInWorld - minNorm * correctedMinNorm; pb = res.m_pointInWorld; + v = minNorm; #ifdef DEBUG_DRAW if (debugDraw) diff --git a/src/LinearMath/btHashMap.h b/src/LinearMath/btHashMap.h index 98f587c0b..fbe07d5be 100644 --- a/src/LinearMath/btHashMap.h +++ b/src/LinearMath/btHashMap.h @@ -171,8 +171,8 @@ class btHashMap for(i=0;i