diff options
author | erwin.coumans <erwin.coumans@08e121b0-ae19-0410-a57b-3be3395fd4fd> | 2009-09-17 19:45:22 +0000 |
---|---|---|
committer | erwin.coumans <erwin.coumans@08e121b0-ae19-0410-a57b-3be3395fd4fd> | 2009-09-17 19:45:22 +0000 |
commit | f65e829ca08c0856d1923e7008e2663486949493 (patch) | |
tree | 4885c5885347d55697b2ed90691a1b5321637791 | |
parent | 3da9c832aef0eea74ecc8221d834e9a879f43a43 (diff) | |
download | bullet3-f65e829ca08c0856d1923e7008e2663486949493.tar.gz |
Add support for generic 2d convex shapes, through wrapper class btConvex2dShape. See Bullet/Demos/Box2dDemo for example wrapping a btCylinderShape and 2d btConvexHullShape.
Add some extra degeneracy debugging check in btGjkPairDetector
14 files changed, 656 insertions, 49 deletions
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 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 <stdio.h>
+//#include <stdio.h>
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 index 2766c3fc2..2766c3fc2 100644 --- a/Demos/Box2dDemo/btBox2dBox2dCollisionAlgorithm.h +++ b/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 <stdio.h>
+#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<btConvexShape*>(body0->getCollisionShape());
+ btConvexShape* min1 = static_cast<btConvexShape*>(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<btConvexShape*>(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<btConvexShape*>(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 index cfc54fc4f..cfc54fc4f 100644 --- a/Demos/Box2dDemo/btBox2dShape.cpp +++ b/src/BulletCollision/CollisionShapes/btBox2dShape.cpp diff --git a/Demos/Box2dDemo/btBox2dShape.h b/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) - { -#ifdef __SPU__ - //spu_printf("distance\n"); -#endif //__CELLOS_LV2__ + + if (isValid && ((distance < 0) || (distance*distance < input.m_maximumDistanceSquared))) + { +#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;i<numSampleDirections;i++) { - const btVector3& norm = sPenetrationDirections[i]; + btVector3 norm = sPenetrationDirections[i]; seperatingAxisInABatch[i] = (-norm) * transA.getBasis() ; seperatingAxisInBBatch[i] = norm * transB.getBasis() ; } @@ -173,29 +174,44 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s + convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch,supportVerticesABatch,numSampleDirections); convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch,supportVerticesBBatch,numSampleDirections); for (i=0;i<numSampleDirections;i++) { - const btVector3& norm = sPenetrationDirections[i]; - seperatingAxisInA = seperatingAxisInABatch[i]; - seperatingAxisInB = seperatingAxisInBBatch[i]; + btVector3 norm = sPenetrationDirections[i]; + if (check2d) + { + norm[2] = 0.f; + } + if (norm.length2()>0.01) + { - pInA = supportVerticesABatch[i]; - qInB = supportVerticesBBatch[i]; + seperatingAxisInA = seperatingAxisInABatch[i]; + seperatingAxisInB = seperatingAxisInBBatch[i]; - pWorld = transA(pInA); - qWorld = transB(qInB); - w = qWorld - pWorld; - btScalar delta = norm.dot(w); - //find smallest delta - if (delta < minProj) - { - minProj = delta; - minNorm = norm; - minA = pWorld; - minB = qWorld; + 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<curHashtableSize;i++) { - const Value& value = m_valueArray[i]; - const Key& key = m_keyArray[i]; + //const Value& value = m_valueArray[i]; + //const Key& key = m_keyArray[i]; int hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity()-1); // New hash value with new mask m_next[i] = m_hashTable[hashValue]; |