ASSERT -> btAssert

Added btStackAlloc to Bullet (right now only used by btGjkEpa)
removed default constructors of btCollisionWorld/btDiscreteDynamicsWorld, to reduce link-time dependencies
This commit is contained in:
ejcoumans 2006-11-29 01:52:09 +00:00
parent 43ab3c67c4
commit 6738ed329d
41 changed files with 206 additions and 269 deletions

View File

@ -133,7 +133,17 @@ void BspDemo::initPhysics(char* bspfilename)
m_forwardAxis = 1;
///Setup a Physics Simulation Environment
m_dynamicsWorld = new btDiscreteDynamicsWorld();
btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
btOverlappingPairCache* pairCache = new btAxisSweep3(worldMin,worldMax);
//btOverlappingPairCache* broadphase = new btSimpleBroadphase();
btConstraintSolver* constraintSolver = new btSequentialImpulseConstraintSolver();
//ConstraintSolver* solver = new OdeConstraintSolver;
m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver);
m_dynamicsWorld->setGravity(-m_cameraUp * 10);
m_dynamicsWorld->setDebugDrawer(&debugDrawer);

View File

@ -720,7 +720,7 @@ bool ColladaConverter::saveAs(const char* filename)
{
for (int i=0;i<m_numObjects;i++)
{
assert(m_colladadomNodes[i]);
btAssert(m_colladadomNodes[i]);
if (!m_colladadomNodes[i]->getTranslate_array().getCount())
{
domTranslate* transl = (domTranslate*) m_colladadomNodes[i]->createAndPlace("translate");

View File

@ -160,7 +160,14 @@ void ColladaDemo::initPhysics(const char* filename)
m_cameraUp = btVector3(0,0,1);
m_forwardAxis = 1;
m_dynamicsWorld = new btDiscreteDynamicsWorld();
btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
btOverlappingPairCache* pairCache = new btAxisSweep3(worldMin,worldMax);
btConstraintSolver* constraintSolver = new btSequentialImpulseConstraintSolver();
m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver);
//m_dynamicsWorld = new btSimpleDynamicsWorld();
m_dynamicsWorld->setDebugDrawer(&debugDrawer);

View File

@ -151,11 +151,14 @@ void ConcaveDemo::initPhysics()
btCollisionShape* trimeshShape = new btBvhTriangleMeshShape(indexVertexArrays);
//btConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
//btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
//btOverlappingPairCache* broadphase = new btSimpleBroadphase();
m_dynamicsWorld = new btDiscreteDynamicsWorld();
btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
btOverlappingPairCache* pairCache = new btAxisSweep3(worldMin,worldMax);
btConstraintSolver* constraintSolver = new btSequentialImpulseConstraintSolver();
m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver);
float mass = 0.f;
btTransform startTransform;

View File

@ -75,12 +75,16 @@ void drawLimit()
void ConstraintDemo::initPhysics()
{
//ConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
//ConstraintSolver* solver = new OdeConstraintSolver;
//btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
btOverlappingPairCache* pairCache = new btAxisSweep3(worldMin,worldMax);
//btOverlappingPairCache* broadphase = new btSimpleBroadphase();
m_dynamicsWorld = new btDiscreteDynamicsWorld();
btConstraintSolver* constraintSolver = new btSequentialImpulseConstraintSolver();
//ConstraintSolver* solver = new OdeConstraintSolver;
m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver);
m_dynamicsWorld->setDebugDrawer(&debugDrawer);
btCollisionShape* shape = new btBoxShape(btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));

View File

@ -32,6 +32,8 @@
#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
#include "LinearMath/btStackAlloc.h"
//We can use the Bullet EPA or sampling penetration depth solver, but comparison might be useful
//#define COMPARE_WITH_SOLID35_AND_OTHER_EPA 1
#ifdef COMPARE_WITH_SOLID35_AND_OTHER_EPA
@ -279,6 +281,9 @@ static float gDepth;
}
};
//2 Mb by default, could be made smaller
btStackAlloc gStackAlloc(1024*1024*2);
static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1)
{
static btSimplexSolverInterface simplexSolver;
@ -323,6 +328,7 @@ static bool TestEPA(const MyConvex& hull0, const MyConvex& hull1)
btDiscreteCollisionDetectorInterface::ClosestPointInput input;
input.m_transformA = hull0.mTransform;
input.m_transformB = hull1.mTransform;
input.m_stackAlloc = &gStackAlloc;
MyResult output;
GJK.getClosestPoints(input, output, 0);

View File

@ -1634,7 +1634,8 @@ void ConcaveDemo::initPhysics()
//btOverlappingPairCache* broadphase = new btSimpleBroadphase();
btOverlappingPairCache* broadphase = new btSimpleBroadphase();
m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase);
btConstraintSolver* constraintSolver = new btSequentialImpulseConstraintSolver();
m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,constraintSolver);
m_gimpactCollisionCreateFunc = new btConcaveConcaveCollisionAlgorithm::CreateFunc;
dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,GIMPACT_SHAPE_PROXYTYPE,m_gimpactCollisionCreateFunc);
@ -1865,7 +1866,7 @@ void ConcaveDemo::clientMoveAndDisplay()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
float dt = m_clock.getTimeMilliseconds() * 0.001f;
float dt = float(m_clock.getTimeMicroseconds()) * 0.000001f;
m_clock.reset();
m_dynamicsWorld->stepSimulation(dt);

View File

@ -110,8 +110,8 @@ void UserCollisionAlgorithm::initPhysics()
btOverlappingPairCache* broadphase = new btAxisSweep3(-maxAabb,maxAabb);//SimpleBroadphase();
dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,GIMPACT_SHAPE_PROXYTYPE,new btSphereSphereCollisionAlgorithm::CreateFunc);
m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase);
btConstraintSolver* constraintSolver = new btSequentialImpulseConstraintSolver();
m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,constraintSolver);
float mass = 0.f;
btTransform startTransform;

View File

@ -109,7 +109,12 @@ void VehicleDemo::setupPhysics()
#endif
btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50));
m_dynamicsWorld = new btDiscreteDynamicsWorld();
btCollisionDispatcher* dispatcher = new btCollisionDispatcher();
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
btOverlappingPairCache* pairCache = new btAxisSweep3(worldMin,worldMax);
btConstraintSolver* constraintSolver = new btSequentialImpulseConstraintSolver();
m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver);
#ifdef FORCE_ZAXIS_UP
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
#endif

View File

@ -151,7 +151,7 @@ void gim_closest_point_triangle_segment(GIM_TRIANGLE_DATA * triangle, vec3f s1,v
VEC_COPY(closest_points[1],sdiff);//normal
}
}
if(out_edge>2) return ;// ???? ASSERT this please
if(out_edge>2) return ;// ???? btAssert this please
if(GIM_IS_ZERO(dis))
{
@ -191,7 +191,7 @@ int gim_triangle_capsule_collision(GIM_TRIANGLE_DATA * triangle, GIM_CAPSULE_DAT
}
vec3f vec;
while(old_contact_size<contacts->m_size)
while(old_contact_size<contacts->m_size)
{
//Scale the normal for pointing to triangle
VEC_SCALE(pcontact->m_normal,-1.0f,pcontact->m_normal);
@ -202,7 +202,7 @@ int gim_triangle_capsule_collision(GIM_TRIANGLE_DATA * triangle, GIM_CAPSULE_DAT
pcontact->m_depth = capsule->m_radius - pcontact->m_depth;
pcontact++;
old_contact_size++;
old_contact_size++;
}
return 1;

View File

@ -29,6 +29,7 @@ enum btCollisionDispatcherId
};
class btPersistentManifold;
class btStackAlloc;
struct btDispatcherInfo
{
@ -45,7 +46,8 @@ struct btDispatcherInfo
m_useContinuous(false),
m_debugDraw(0),
m_enableSatConvex(false),
m_enableSPU(false)
m_enableSPU(false),
m_stackAllocator(0)
{
}
@ -57,6 +59,7 @@ struct btDispatcherInfo
class btIDebugDraw* m_debugDraw;
bool m_enableSatConvex;
bool m_enableSPU;
btStackAlloc* m_stackAllocator;
};

View File

@ -101,7 +101,7 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1)
{
gNumManifold++;
//ASSERT(gNumManifold < 65535);
//btAssert(gNumManifold < 65535);
btCollisionObject* body0 = (btCollisionObject*)b0;
@ -145,7 +145,7 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold)
{
#define USE_DISPATCH_REGISTRY_ARRAY 1
#ifdef USE_DISPATCH_REGISTRY_ARRAY
btCollisionAlgorithmConstructionInfo ci;
@ -194,6 +194,7 @@ btCollisionAlgorithmCreateFunc* btCollisionDispatcher::internalFindCreateFunc(in
}
#ifndef USE_DISPATCH_REGISTRY_ARRAY
btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold)
{
@ -232,6 +233,7 @@ btCollisionAlgorithm* btCollisionDispatcher::internalFindAlgorithm(btCollisionOb
return new btEmptyAlgorithm(ci);
}
#endif //USE_DISPATCH_REGISTRY_ARRAY
bool btCollisionDispatcher::needsResponse(btCollisionObject* body0,btCollisionObject* body1)
{

View File

@ -30,7 +30,7 @@ class btOverlappingPairCache;
#include "btCollisionCreateFunc.h"
#define USE_DISPATCH_REGISTRY_ARRAY 1
///btCollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs.
@ -56,7 +56,9 @@ class btCollisionDispatcher : public btDispatcher
btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc;
btCollisionAlgorithmCreateFunc* m_emptyCreateFunc;
#ifndef USE_DISPATCH_REGISTRY_ARRAY
btCollisionAlgorithm* internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold = 0);
#endif //USE_DISPATCH_REGISTRY_ARRAY
public:

View File

@ -25,6 +25,7 @@ subject to the following restrictions:
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
#include "LinearMath/btAabbUtil2.h"
#include "LinearMath/btQuickprof.h"
#include "LinearMath/btStackAlloc.h"
//When the user doesn't provide dispatcher or broadphase, create basic versions (and delete them in destructor)
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
@ -32,26 +33,22 @@ subject to the following restrictions:
#include <algorithm>
btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache)
btCollisionWorld::btCollisionWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache, int stackSize)
:m_dispatcher1(dispatcher),
m_broadphasePairCache(pairCache),
m_ownsDispatcher(false),
m_ownsBroadphasePairCache(false)
{
}
btCollisionWorld::btCollisionWorld()
: m_dispatcher1(new btCollisionDispatcher()),
m_broadphasePairCache(new btSimpleBroadphase()),
m_ownsDispatcher(true),
m_ownsBroadphasePairCache(true)
{
m_stackAlloc = new btStackAlloc(stackSize);
m_dispatchInfo.m_stackAllocator = m_stackAlloc;
}
btCollisionWorld::~btCollisionWorld()
{
m_stackAlloc->destroy();
delete m_stackAlloc;
//clean up remaining objects
std::vector<btCollisionObject*>::iterator i;

View File

@ -64,7 +64,7 @@ subject to the following restrictions:
#ifndef COLLISION_WORLD_H
#define COLLISION_WORLD_H
class btStackAlloc;
class btCollisionShape;
class btBroadphaseInterface;
#include "LinearMath/btVector3.h"
@ -88,6 +88,10 @@ protected:
btDispatcher* m_dispatcher1;
btDispatcherInfo m_dispatchInfo;
btStackAlloc* m_stackAlloc;
btOverlappingPairCache* m_broadphasePairCache;
bool m_ownsDispatcher;
@ -95,11 +99,8 @@ protected:
public:
//this constructor will create and own a dispatcher and paircache and delete it at destruction
btCollisionWorld();
//this constructor doesn't own the dispatcher and paircache/broadphase
btCollisionWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache);
btCollisionWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache, int stackSize = 2*1024*1024);
virtual ~btCollisionWorld();
@ -237,7 +238,12 @@ public:
void removeCollisionObject(btCollisionObject* collisionObject);
virtual void performDiscreteCollisionDetection( btDispatcherInfo& dispatchInfo);
btDispatcherInfo& getDispatchInfo()
{
return m_dispatchInfo;
}
};

View File

@ -61,37 +61,28 @@ subject to the following restrictions:
#endif //USE_HULL
bool gUseEpa = true;
btConvexConvexAlgorithm::CreateFunc::CreateFunc()
{
m_ownsSolvers = true;
m_simplexSolver = new btVoronoiSimplexSolver();
m_pdSolver = new btGjkEpaPenetrationDepthSolver;
}
#ifdef WIN32
void DrawRasterizerLine(const float* from,const float* to,int color);
#endif
btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
{
m_ownsSolvers = false;
m_simplexSolver = simplexSolver;
m_pdSolver = pdSolver;
}
//#define PROCESS_SINGLE_CONTACT
#ifdef WIN32
bool gForceBoxBox = false;//false;//true;
#else
bool gForceBoxBox = false;//false;//true;
#endif
bool gBoxBoxUseGjk = true;//true;//false;
bool gDisableConvexCollision = false;
btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1)
btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
: btCollisionAlgorithm(ci),
m_gjkPairDetector(0,0,&m_simplexSolver,0),
m_useEpa(!gUseEpa),
m_gjkPairDetector(0,0,simplexSolver,pdSolver),
m_ownManifold (false),
m_manifoldPtr(mf),
m_lowLevelOfDetail(false)
{
checkPenetrationDepthSolver();
}
@ -115,27 +106,6 @@ void btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
static btGjkEpaPenetrationDepthSolver gEpaPenetrationDepthSolver;
static btMinkowskiPenetrationDepthSolver gPenetrationDepthSolver;
void btConvexConvexAlgorithm::checkPenetrationDepthSolver()
{
if (m_useEpa != gUseEpa)
{
m_useEpa = gUseEpa;
if (m_useEpa)
{
m_gjkPairDetector.setPenetrationDepthSolver(&gEpaPenetrationDepthSolver);
} else
{
m_gjkPairDetector.setPenetrationDepthSolver(&gPenetrationDepthSolver);
}
}
}
//
// Convex-Convex collision algorithm
@ -164,7 +134,6 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
}
#else
checkPenetrationDepthSolver();
btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
@ -176,7 +145,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl
m_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_maximumDistanceSquared = 1e30f;
input.m_transformA = body0->getWorldTransform();
@ -209,7 +179,6 @@ float btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btC
if (disableCcd)
return 1.f;
checkPenetrationDepthSolver();
//An adhoc way of testing the Continuous Collision Detection algorithms
//One object is approximated as a sphere, to simplify things

View File

@ -28,23 +28,17 @@ class btConvexPenetrationDepthSolver;
///ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations.
class btConvexConvexAlgorithm : public btCollisionAlgorithm
{
//ConvexPenetrationDepthSolver* m_penetrationDepthSolver;
btVoronoiSimplexSolver m_simplexSolver;
btGjkPairDetector m_gjkPairDetector;
bool m_useEpa;
public:
bool m_ownManifold;
btPersistentManifold* m_manifoldPtr;
bool m_lowLevelOfDetail;
void checkPenetrationDepthSolver();
public:
btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1);
btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1, btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
virtual ~btConvexConvexAlgorithm();
@ -62,9 +56,16 @@ public:
struct CreateFunc :public btCollisionAlgorithmCreateFunc
{
btConvexPenetrationDepthSolver* m_pdSolver;
btSimplexSolverInterface* m_simplexSolver;
bool m_ownsSolvers;
CreateFunc(btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver);
CreateFunc();
virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1)
{
return new btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1);
return new btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver);
}
};

View File

@ -220,7 +220,7 @@ public:
edgeVert1 = 7;
break;
default:
ASSERT(0);
btAssert(0);
}

View File

@ -15,7 +15,6 @@ subject to the following restrictions:
//#define DISABLE_BVH
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"

View File

@ -76,7 +76,7 @@ void btStridingMeshInterface::InternalProcessAllTriangles(btInternalTriangleInde
break;
}
default:
ASSERT((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT));
btAssert((gfxindextype == PHY_INTEGER) || (gfxindextype == PHY_SHORT));
}
unLockReadOnlyVertexBase(part);

View File

@ -32,7 +32,7 @@ btTriangleIndexVertexArray::btTriangleIndexVertexArray(int numTriangles,int* tri
void btTriangleIndexVertexArray::getLockedVertexIndexBase(unsigned char **vertexbase, int& numverts,PHY_ScalarType& type, int& vertexStride,unsigned char **indexbase,int & indexstride,int& numfaces,PHY_ScalarType& indicestype,int subpart)
{
ASSERT(subpart< getNumSubParts() );
btAssert(subpart< getNumSubParts() );
btIndexedMesh& mesh = m_indexedMeshes[subpart];

View File

@ -59,7 +59,7 @@ public:
virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax)const
{
// ASSERT(0);
// btAssert(0);
getAabbSlow(t,aabbMin,aabbMax);
}
@ -116,7 +116,7 @@ public:
virtual void calculateLocalInertia(btScalar mass,btVector3& inertia)
{
ASSERT(0);
btAssert(0);
inertia.setValue(0.f,0.f,0.f);
}

View File

@ -17,6 +17,7 @@ subject to the following restrictions:
#ifndef CONVEX_PENETRATION_DEPTH_H
#define CONVEX_PENETRATION_DEPTH_H
class btStackAlloc;
class btVector3;
#include "btSimplexSolverInterface.h"
class btConvexShape;
@ -33,7 +34,7 @@ public:
btConvexShape* convexA,btConvexShape* convexB,
const btTransform& transA,const btTransform& transB,
btVector3& v, btPoint3& pa, btPoint3& pb,
class btIDebugDraw* debugDraw
class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
) = 0;

View File

@ -18,7 +18,7 @@ subject to the following restrictions:
#define DISCRETE_COLLISION_DETECTOR1_INTERFACE_H
#include "LinearMath/btTransform.h"
#include "LinearMath/btVector3.h"
class btStackAlloc;
/// This interface is made to be used by an iterative approach to do TimeOfImpact calculations
/// This interface allows to query for closest points and penetration depth between two (convex) objects
@ -42,13 +42,15 @@ struct btDiscreteCollisionDetectorInterface
struct ClosestPointInput
{
ClosestPointInput()
:m_maximumDistanceSquared(1e30f)
:m_maximumDistanceSquared(1e30f),
m_stackAlloc(0)
{
}
btTransform m_transformA;
btTransform m_transformB;
btScalar m_maximumDistanceSquared;
btStackAlloc* m_stackAlloc;
};
virtual ~btDiscreteCollisionDetectorInterface() {};

View File

@ -26,6 +26,15 @@ Nov.2006
#include "btGjkEpa.h"
#include <string.h> //for memset
#include <LinearMath/btStackAlloc.h>
#if defined(DEBUG) || defined (_DEBUG)
#include <stdio.h> //for debug printf
#ifdef __SPU__
#include <spu_printf.h>
#define printf spu_printf
#endif //__SPU__
#endif
namespace gjkepa_impl
{
@ -58,21 +67,18 @@ typedef btMatrix3x3 Rotation;
// Const
//
static const U chkPrecision =1/U(sizeof(F)==4);
static const F cstInf =F(1/sin(0.));
static const F cstPi =F(acos(-1.));
static const F cst2Pi =cstPi*2;
static const U GJK_maxiterations =128;
static const U GJK_hashsize =1<<6;
static const U GJK_hashmask =GJK_hashsize-1;
static const F GJK_insimplex_eps =F(0.0001);
static const F GJK_sqinsimplex_eps =GJK_insimplex_eps*GJK_insimplex_eps;
static const U EPA_maxiterations =256;
static const F EPA_inface_eps =F(0.01);
static const F EPA_accuracy =F(0.001);
#define cstInf SIMD_INFINITY
#define cstPi SIMD_PI
#define cst2Pi SIMD_2_PI
#define GJK_maxiterations (128)
#define GJK_hashsize (1<<6)
#define GJK_hashmask (GJK_hashsize-1)
#define GJK_insimplex_eps F(0.0001)
#define GJK_sqinsimplex_eps (GJK_insimplex_eps*GJK_insimplex_eps)
#define EPA_maxiterations 256
#define EPA_inface_eps F(0.01)
#define EPA_accuracy F(0.001)
//
// Utils
@ -95,80 +101,7 @@ throw(object); }
template <typename T> static inline void Raise(const T&) {}
#endif
struct Block
{
Block* previous;
U1* address;
};
//
// StackAlloc
//
struct StackAlloc
{
StackAlloc() { ctor(); }
StackAlloc(U size) { ctor();Create(size); }
~StackAlloc() { Free(); }
void Create(U size)
{
Free();
data = new U1[size];
totalsize = size;
}
void Free()
{
if(usedsize==0)
{
if(!ischild) delete[] data;
data = 0;
usedsize = 0;
} else Raise(L"StackAlloc is still in use");
}
U1* Allocate(U size)
{
const U nus(usedsize+size);
if(nus<totalsize)
{
usedsize=nus;
return(data+(usedsize-size));
}
Raise(L"Not enough memory");
return(0);
}
Block* BeginBlock()
{
Block* pb = (Block*)Allocate(sizeof(Block));
pb->previous = current;
pb->address = data+usedsize;
current = pb;
return(pb);
}
void EndBlock(Block* block)
{
if(block==current)
{
current = block->previous;
usedsize = (U)((block->address-data)-sizeof(Block));
} else Raise(L"Unmatched blocks");
}
private:
void ctor()
{
data = 0;
totalsize = 0;
usedsize = 0;
current = 0;
ischild = false;
}
U1* data;
U totalsize;
U usedsize;
Block* current;
Z ischild;
};
//
// GJK
@ -185,8 +118,8 @@ struct GJK
Vector3 v;
He* n;
};
StackAlloc* sa;
Block* sablock;
btStackAlloc* sa;
btBlock* sablock;
He* table[GJK_hashsize];
Rotation wrotations[2];
Vector3 positions[2];
@ -198,7 +131,7 @@ struct GJK
F margin;
Z failed;
//
GJK(StackAlloc* psa,
GJK(btStackAlloc* psa,
const Rotation& wrot0,const Vector3& pos0,const btConvexShape* shape0,
const Rotation& wrot1,const Vector3& pos1,const btConvexShape* shape1,
F pmargin=0)
@ -206,14 +139,14 @@ struct GJK
wrotations[0]=wrot0;positions[0]=pos0;shapes[0]=shape0;
wrotations[1]=wrot1;positions[1]=pos1;shapes[1]=shape1;
sa =psa;
sablock =sa->BeginBlock();
sablock =sa->beginBlock();
margin =pmargin;
failed =false;
}
//
~GJK()
{
sa->EndBlock(sablock);
sa->endBlock(sablock);
}
// vdh : very dumm hash
static inline U Hash(const Vector3& v)
@ -243,7 +176,7 @@ struct GJK
const U h(Hash(ray));
He* e = (He*)(table[h]);
while(e) { if(e->v==ray) { --order;return(false); } else e=e->n; }
e=(He*)sa->Allocate(sizeof(He));e->v=ray;e->n=table[h];table[h]=e;
e=(He*)sa->allocate(sizeof(He));e->v=ray;e->n=table[h];table[h]=e;
Support(ray,simplex[++order]);
return(ray.dot(SPXW(order))>0);
}
@ -401,7 +334,7 @@ struct EPA
};
//
GJK* gjk;
StackAlloc* sa;
btStackAlloc* sa;
Face* root;
U nfaces;
U iterations;
@ -464,7 +397,7 @@ c) const
//
inline Face* NewFace(const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* c)
{
Face* pf = (Face*)sa->Allocate(sizeof(Face));
Face* pf = (Face*)sa->allocate(sizeof(Face));
if(Set(pf,a,b,c))
{
if(root) root->prev=pf;
@ -506,7 +439,7 @@ c) const
//
GJK::Mkv* Support(const Vector3& w) const
{
GJK::Mkv* v =(GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv));
GJK::Mkv* v =(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv));
gjk->Support(w,*v);
return(v);
}
@ -540,7 +473,7 @@ ff)
//
inline F EvaluatePD(F accuracy=EPA_accuracy)
{
Block* sablock = sa->BeginBlock();
btBlock* sablock = sa->beginBlock();
Face* bestface = 0;
U markid(1);
depth = -cstInf;
@ -579,7 +512,7 @@ U eidx[9][4]={{0,0,4,0},{0,1,2,1},{0,2,1,2},{1,1,5,2},{1,0,2,0},{2,2,3,2},{3,1,5
U i;
for( i=0;i<=gjk->order;++i) {
basemkv[i]=(GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv));*basemkv[i]=gjk->simplex[i];
basemkv[i]=(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv));*basemkv[i]=gjk->simplex[i];
}
for( i=0;i<nfidx;++i,pfidx+=3) {
basefaces[i]=NewFace(basemkv[pfidx[0]],basemkv[pfidx[1]],basemkv[pfidx[2]]);
@ -589,7 +522,7 @@ Link(basefaces[peidx[0]],peidx[1],basefaces[peidx[2]],peidx[3]); }
}
if(0==nfaces)
{
sa->EndBlock(sablock);
sa->endBlock(sablock);
return(depth);
}
/* Expand hull */
@ -632,7 +565,7 @@ nf+=BuildHorizon(markid,w,*bf->f[i],bf->e[i],cf,ff); }
nearest[0] = features[0][0]*b.x()+features[0][1]*b.y()+features[0][2]*b.z();
nearest[1] = features[1][0]*b.x()+features[1][1]*b.y()+features[1][2]*b.z();
} else failed=true;
sa->EndBlock(sablock);
sa->endBlock(sablock);
return(depth);
}
};
@ -644,17 +577,17 @@ nf+=BuildHorizon(markid,w,*bf->f[i],bf->e[i],cf,ff); }
using namespace gjkepa_impl;
/* Need some kind of stackalloc , create a static one till bullet provide
one. */
static const U g_sasize((1024<<10)*2);
static StackAlloc g_sa(g_sasize);
//
bool btGjkEpaSolver::Collide(btConvexShape *shape0,const btTransform &wtrs0,
btConvexShape *shape1,const btTransform &wtrs1,
btScalar radialmargin,
btStackAlloc* stackAlloc,
sResults& results)
{
/* Initialize */
results.witnesses[0] =
results.witnesses[1] =
@ -664,7 +597,7 @@ results.status = sResults::Separated;
results.epa_iterations = 0;
results.gjk_iterations = 0;
/* Use GJK to locate origin */
GJK gjk(&g_sa,
GJK gjk(stackAlloc,
wtrs0.getBasis(),wtrs0.getOrigin(),shape0,
wtrs1.getBasis(),wtrs1.getOrigin(),shape1,
radialmargin+EPA_accuracy);

View File

@ -23,6 +23,8 @@ Nov.2006
#define _05E48D53_04E0_49ad_BB0A_D74FE62E7366_
#include "BulletCollision/CollisionShapes/btConvexShape.h"
class btStackAlloc;
///btGjkEpaSolver contributed under zlib by Nathanael Presson
struct btGjkEpaSolver
{
@ -44,6 +46,7 @@ struct sResults
static bool Collide(btConvexShape* shape0,const btTransform& wtrs0,
btConvexShape* shape1,const btTransform& wtrs1,
btScalar radialmargin,
btStackAlloc* stackAlloc,
sResults& results);
};

View File

@ -23,7 +23,7 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim
btConvexShape* pConvexA, btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
class btIDebugDraw* debugDraw )
class btIDebugDraw* debugDraw, btStackAlloc* stackAlloc )
{
@ -32,7 +32,7 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& sim
btGjkEpaSolver::sResults results;
if(btGjkEpaSolver::Collide( pConvexA,transformA,
pConvexB,transformB,
radialmargin,results))
radialmargin,stackAlloc,results))
{
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);

View File

@ -29,7 +29,7 @@ class btGjkEpaPenetrationDepthSolver : public btConvexPenetrationDepthSolver
btConvexShape* pConvexA, btConvexShape* pConvexB,
const btTransform& transformA, const btTransform& transformB,
btVector3& v, btPoint3& wWitnessOnA, btPoint3& wWitnessOnB,
class btIDebugDraw* debugDraw );
class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc );
private :

View File

@ -20,6 +20,10 @@ subject to the following restrictions:
#if defined(DEBUG) || defined (_DEBUG)
#include <stdio.h> //for debug printf
#ifdef __SPU__
#include <spu_printf.h>
#define printf spu_printf
#endif //__SPU__
#endif
//must be above the machine epsilon
@ -29,10 +33,7 @@ subject to the following restrictions:
int gNumDeepPenetrationChecks = 0;
int gNumGjkChecks = 0;
#ifdef __SPU__
#include <spu_printf.h>
#define printf spu_printf
#endif //__SPU__
btGjkPairDetector::btGjkPairDetector(btConvexShape* objectA,btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver)
:m_cachedSeparatingAxis(0.f,0.f,1.f),
@ -202,7 +203,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
normalInB *= rlen; //normalize
btScalar s = btSqrt(squaredDistance);
ASSERT(s > btScalar(0.0));
btAssert(s > btScalar(0.0));
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
pointOnB += m_cachedSeparatingAxis * (marginB / s);
distance = ((1.f/rlen) - margin);
@ -236,7 +237,7 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
m_minkowskiA,m_minkowskiB,
localTransA,localTransB,
m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
debugDraw
debugDraw,input.m_stackAlloc
);
if (isValid2)

View File

@ -74,7 +74,7 @@ bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& s
btConvexShape* convexA,btConvexShape* convexB,
const btTransform& transA,const btTransform& transB,
btVector3& v, btPoint3& pa, btPoint3& pb,
class btIDebugDraw* debugDraw
class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
)
{

View File

@ -28,7 +28,7 @@ public:
btConvexShape* convexA,btConvexShape* convexB,
const btTransform& transA,const btTransform& transB,
btVector3& v, btPoint3& pa, btPoint3& pb,
class btIDebugDraw* debugDraw
class btIDebugDraw* debugDraw,btStackAlloc* stackAlloc
);
};

View File

@ -86,13 +86,13 @@ public:
inline const btManifoldPoint& getContactPoint(int index) const
{
ASSERT(index < m_cachedPoints);
btAssert(index < m_cachedPoints);
return m_pointCache[index];
}
inline btManifoldPoint& getContactPoint(int index)
{
ASSERT(index < m_cachedPoints);
btAssert(index < m_cachedPoints);
return m_pointCache[index];
}

View File

@ -50,7 +50,7 @@ public:
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
ASSERT(m_Adiag > 0.0f);
btAssert(m_Adiag > 0.0f);
}
//angular constraint between two different rigidbodies
@ -67,7 +67,7 @@ public:
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
ASSERT(m_Adiag > 0.0f);
btAssert(m_Adiag > 0.0f);
}
//angular constraint between two different rigidbodies
@ -83,7 +83,7 @@ public:
m_1MinvJt = inertiaInvB * m_bJ;
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
ASSERT(m_Adiag > 0.0f);
btAssert(m_Adiag > 0.0f);
}
//constraint on one rigidbody
@ -101,7 +101,7 @@ public:
m_1MinvJt = btVector3(0.f,0.f,0.f);
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
ASSERT(m_Adiag > 0.0f);
btAssert(m_Adiag > 0.0f);
}
btScalar getDiagonal() const { return m_Adiag; }

View File

@ -51,7 +51,7 @@ void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
if (fabs(len) >= SIMD_EPSILON)
return;
ASSERT(len < SIMD_EPSILON);
btAssert(len < SIMD_EPSILON);
//this jacobian entry could be re-used for all iterations
@ -133,7 +133,7 @@ void btSolve2LinearConstraint::resolveBilateralPairConstraint(
if (fabs(len) >= SIMD_EPSILON)
return;
ASSERT(len < SIMD_EPSILON);
btAssert(len < SIMD_EPSILON);
//this jacobian entry could be re-used for all iterations

View File

@ -57,19 +57,6 @@ subject to the following restrictions:
#include <algorithm>
btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btConstraintSolver* constraintSolver)
:btDynamicsWorld(),
m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver),
m_debugDrawer(0),
m_gravity(0,-10,0),
m_localTime(1.f/60.f),
m_profileTimings(0)
{
m_islandManager = new btSimulationIslandManager();
m_ownsIslandManager = true;
m_ownsConstraintSolver = (constraintSolver==0);
}
btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver)
:btDynamicsWorld(dispatcher,pairCache),
@ -267,27 +254,27 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep)
///apply gravity, predict motion
predictUnconstraintMotion(timeStep);
btDispatcherInfo dispatchInfo;
btDispatcherInfo& dispatchInfo = getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_debugDraw = getDebugDrawer();
///perform collision detection
performDiscreteCollisionDetection(dispatchInfo);
calculateSimulationIslands();
btContactSolverInfo infoGlobal;
infoGlobal.m_timeStep = timeStep;
getSolverInfo().m_timeStep = timeStep;
///solve non-contact constraints
solveNoncontactConstraints(infoGlobal);
solveNoncontactConstraints(getSolverInfo());
///solve contact constraints
solveContactConstraints(infoGlobal);
solveContactConstraints(getSolverInfo());
///CallbackTriggers();

View File

@ -23,7 +23,8 @@ class btOverlappingPairCache;
class btConstraintSolver;
class btSimulationIslandManager;
class btTypedConstraint;
struct btContactSolverInfo;
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
class btRaycastVehicle;
class btIDebugDraw;
@ -52,6 +53,9 @@ protected:
bool m_ownsIslandManager;
bool m_ownsConstraintSolver;
btContactSolverInfo m_solverInfo;
std::vector<btRaycastVehicle*> m_vehicles;
int m_profileTimings;
@ -83,11 +87,8 @@ public:
///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver=0);
btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver);
///this btDiscreteDynamicsWorld will create and own dispatcher, pairCache and constraintSolver, and deletes it in the destructor.
btDiscreteDynamicsWorld(btConstraintSolver* constraintSolver = 0);
virtual ~btDiscreteDynamicsWorld();
///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
@ -144,6 +145,11 @@ public:
virtual const btTypedConstraint* getConstraint(int index) const;
btContactSolverInfo& getSolverInfo()
{
return m_solverInfo;
}
};

View File

@ -27,9 +27,6 @@ class btDynamicsWorld : public btCollisionWorld
{
public:
btDynamicsWorld()
{
}
btDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache)
:btCollisionWorld(dispatcher,pairCache)

View File

@ -22,13 +22,7 @@ subject to the following restrictions:
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
btSimpleDynamicsWorld::btSimpleDynamicsWorld()
:m_constraintSolver(new btSequentialImpulseConstraintSolver),
m_ownsConstraintSolver(true),
m_debugDrawer(0),
m_gravity(0,0,-10)
{
}
btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver)
:btDynamicsWorld(dispatcher,pairCache),

View File

@ -46,8 +46,6 @@ protected:
public:
///this btSimpleDynamicsWorld constructor creates and owns dispatcher, broadphase pairCache and constraintSolver
btSimpleDynamicsWorld();
///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver
btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver);

View File

@ -386,21 +386,21 @@ void btRaycastVehicle::applyEngineForce(btScalar force, int wheel)
const btWheelInfo& btRaycastVehicle::getWheelInfo(int index) const
{
ASSERT((index >= 0) && (index < getNumWheels()));
btAssert((index >= 0) && (index < getNumWheels()));
return m_wheelInfo[index];
}
btWheelInfo& btRaycastVehicle::getWheelInfo(int index)
{
ASSERT((index >= 0) && (index < getNumWheels()));
btAssert((index >= 0) && (index < getNumWheels()));
return m_wheelInfo[index];
}
void btRaycastVehicle::setBrake(float brake,int wheelIndex)
{
ASSERT((wheelIndex >= 0) && (wheelIndex < getNumWheels()));
btAssert((wheelIndex >= 0) && (wheelIndex < getNumWheels()));
getWheelInfo(wheelIndex).m_brake;
}

View File

@ -37,7 +37,7 @@ subject to the following restrictions:
//#define ATTRIBUTE_ALIGNED16(a) __declspec(align(16)) a
#define ATTRIBUTE_ALIGNED16(a) a
#include <assert.h>
#define ASSERT assert
#define btAssert assert
#else
//non-windows systems
@ -47,7 +47,7 @@ subject to the following restrictions:
#ifndef assert
#include <assert.h>
#endif
#define ASSERT assert
#define btAssert assert
#endif