From 5ed0cf5b7ff1a23ac7dfec1a4429c481a5c5859e Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Wed, 20 Sep 2006 00:49:33 +0000 Subject: [PATCH] improved performance, and allowed custom friction and contact solver models. See CcdPhysicsDemo #define USER_DEFINED_FRICTION_MODEL --- Bullet/BroadphaseCollision/AxisSweep3.cpp | 7 +- Bullet/BroadphaseCollision/AxisSweep3.h | 2 +- .../OverlappingPairCache.cpp | 29 ++++--- .../OverlappingPairCache.h | 4 +- .../BroadphaseCollision/SimpleBroadphase.cpp | 4 +- Bullet/BroadphaseCollision/SimpleBroadphase.h | 2 +- .../ConstraintSolver/ContactConstraint.h | 22 ++++- .../SequentialImpulseConstraintSolver.cpp | 68 ++++++++++++---- .../SequentialImpulseConstraintSolver.h | 23 +++++- BulletDynamics/Dynamics/RigidBody.cpp | 4 +- BulletDynamics/Dynamics/RigidBody.h | 5 +- Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp | 80 ++++++++++++++++--- Demos/OpenGL/DemoApplication.cpp | 5 +- .../CcdPhysics/CcdPhysicsEnvironment.cpp | 7 ++ .../CcdPhysics/CcdPhysicsEnvironment.h | 5 ++ 15 files changed, 211 insertions(+), 56 deletions(-) diff --git a/Bullet/BroadphaseCollision/AxisSweep3.cpp b/Bullet/BroadphaseCollision/AxisSweep3.cpp index da75b379c..33b376277 100644 --- a/Bullet/BroadphaseCollision/AxisSweep3.cpp +++ b/Bullet/BroadphaseCollision/AxisSweep3.cpp @@ -47,17 +47,14 @@ void AxisSweep3::SetAabb(BroadphaseProxy* proxy,const SimdVector3& aabbMin,const -AxisSweep3::AxisSweep3(const SimdPoint3& worldAabbMin,const SimdPoint3& worldAabbMax, int maxHandles, int maxOverlaps) -:OverlappingPairCache(maxOverlaps) +AxisSweep3::AxisSweep3(const SimdPoint3& worldAabbMin,const SimdPoint3& worldAabbMax, int maxHandles) +:OverlappingPairCache() { //assert(bounds.HasVolume()); // 1 handle is reserved as sentinel assert(maxHandles > 1 && maxHandles < 32767); - // doesn't need this limit right now, but I may want to use unsigned short indexes into overlaps array - assert(maxOverlaps > 0 && maxOverlaps < 65536); - // init bounds m_worldAabbMin = worldAabbMin; m_worldAabbMax = worldAabbMax; diff --git a/Bullet/BroadphaseCollision/AxisSweep3.h b/Bullet/BroadphaseCollision/AxisSweep3.h index 8bbe31643..bef8b66e0 100644 --- a/Bullet/BroadphaseCollision/AxisSweep3.h +++ b/Bullet/BroadphaseCollision/AxisSweep3.h @@ -91,7 +91,7 @@ private: void SortMaxUp(int axis, unsigned short edge, bool updateOverlaps = true); public: - AxisSweep3(const SimdPoint3& worldAabbMin,const SimdPoint3& worldAabbMax, int maxHandles = 20000, int maxOverlaps = 60000); + AxisSweep3(const SimdPoint3& worldAabbMin,const SimdPoint3& worldAabbMax, int maxHandles = 16384); virtual ~AxisSweep3(); virtual void RefreshOverlappingPairs() diff --git a/Bullet/BroadphaseCollision/OverlappingPairCache.cpp b/Bullet/BroadphaseCollision/OverlappingPairCache.cpp index be80f86bf..c099dc738 100644 --- a/Bullet/BroadphaseCollision/OverlappingPairCache.cpp +++ b/Bullet/BroadphaseCollision/OverlappingPairCache.cpp @@ -21,11 +21,11 @@ subject to the following restrictions: #include "Dispatcher.h" #include "CollisionAlgorithm.h" +int gOverlappingPairs = 0; -OverlappingPairCache::OverlappingPairCache(int maxOverlap): -m_blockedForChanges(false), -//m_NumOverlapBroadphasePair(0), -m_maxOverlap(maxOverlap) +OverlappingPairCache::OverlappingPairCache(): +m_blockedForChanges(false) +//m_NumOverlapBroadphasePair(0) { } @@ -36,14 +36,17 @@ OverlappingPairCache::~OverlappingPairCache() } -void OverlappingPairCache::RemoveOverlappingPair(BroadphasePair& pair) +void OverlappingPairCache::RemoveOverlappingPair(BroadphasePair& findPair) { - CleanOverlappingPair(pair); - std::set::iterator it = m_overlappingPairSet.find(pair); + + std::set::iterator it = m_overlappingPairSet.find(findPair); // assert(it != m_overlappingPairSet.end()); if (it != m_overlappingPairSet.end()) { + gOverlappingPairs--; + BroadphasePair* pair = (BroadphasePair*)(&(*it)); + CleanOverlappingPair(*pair); m_overlappingPairSet.erase(it); } } @@ -79,6 +82,7 @@ void OverlappingPairCache::AddOverlappingPair(BroadphaseProxy* proxy0,Broadphase BroadphasePair pair(*proxy0,*proxy1); m_overlappingPairSet.insert(pair); + gOverlappingPairs++; } @@ -179,10 +183,17 @@ void OverlappingPairCache::ProcessAllOverlappingPairs(OverlapCallback* callback) CleanOverlappingPair(*pair); std::set::iterator it2 = it; - it++; //why does next line not compile under OS X?? - //it = m_overlappingPairSet.erase(it2); +#ifdef MAC_OSX_FIXED_STL_SET + it2++; + it = m_overlappingPairSet.erase(it); + assert(it == it2); +#else + it++; m_overlappingPairSet.erase(it2); +#endif //MAC_OSX_FIXED_STL_SET + + gOverlappingPairs--; } else { it++; diff --git a/Bullet/BroadphaseCollision/OverlappingPairCache.h b/Bullet/BroadphaseCollision/OverlappingPairCache.h index 22b1ca37a..e0fd5a4c5 100644 --- a/Bullet/BroadphaseCollision/OverlappingPairCache.h +++ b/Bullet/BroadphaseCollision/OverlappingPairCache.h @@ -37,14 +37,12 @@ class OverlappingPairCache : public BroadphaseInterface //avoid brute-force finding all the time std::set m_overlappingPairSet; - int m_maxOverlap; - //during the dispatch, check that user doesn't destroy/create proxy bool m_blockedForChanges; public: - OverlappingPairCache(int maxOverlap); + OverlappingPairCache(); virtual ~OverlappingPairCache(); void ProcessAllOverlappingPairs(OverlapCallback*); diff --git a/Bullet/BroadphaseCollision/SimpleBroadphase.cpp b/Bullet/BroadphaseCollision/SimpleBroadphase.cpp index 633ebbe38..fd2fbaec8 100644 --- a/Bullet/BroadphaseCollision/SimpleBroadphase.cpp +++ b/Bullet/BroadphaseCollision/SimpleBroadphase.cpp @@ -35,8 +35,8 @@ void SimpleBroadphase::validate() } -SimpleBroadphase::SimpleBroadphase(int maxProxies,int maxOverlap) - :OverlappingPairCache(maxOverlap), +SimpleBroadphase::SimpleBroadphase(int maxProxies) + :OverlappingPairCache(), m_firstFreeProxy(0), m_numProxies(0), m_maxProxies(maxProxies) diff --git a/Bullet/BroadphaseCollision/SimpleBroadphase.h b/Bullet/BroadphaseCollision/SimpleBroadphase.h index 700dcce71..0b3054d90 100644 --- a/Bullet/BroadphaseCollision/SimpleBroadphase.h +++ b/Bullet/BroadphaseCollision/SimpleBroadphase.h @@ -66,7 +66,7 @@ protected: virtual void RefreshOverlappingPairs(); public: - SimpleBroadphase(int maxProxies=4096,int maxOverlap=8192); + SimpleBroadphase(int maxProxies=16384); virtual ~SimpleBroadphase(); diff --git a/BulletDynamics/ConstraintSolver/ContactConstraint.h b/BulletDynamics/ConstraintSolver/ContactConstraint.h index 8304d412d..fb5f1fd75 100644 --- a/BulletDynamics/ConstraintSolver/ContactConstraint.h +++ b/BulletDynamics/ConstraintSolver/ContactConstraint.h @@ -24,6 +24,20 @@ class RigidBody; struct ContactSolverInfo; class ManifoldPoint; +enum { + DEFAULT_CONTACT_SOLVER_TYPE=0, + CONTACT_SOLVER_TYPE1, + CONTACT_SOLVER_TYPE2, + USER_CONTACT_SOLVER_TYPE1, + MAX_CONTACT_SOLVER_TYPES +}; + + +typedef float (*ContactSolverFunc)(RigidBody& body1, + RigidBody& body2, + class ManifoldPoint& contactPoint, + const ContactSolverInfo& info); + struct ConstraintPersistentData { inline ConstraintPersistentData() @@ -35,7 +49,9 @@ struct ConstraintPersistentData m_persistentLifeTime(0), m_restitution(0.f), m_friction(0.f), - m_penetration(0.f) + m_penetration(0.f), + m_contactSolverFunc(0), + m_frictionSolverFunc(0) { } @@ -55,7 +71,9 @@ struct ConstraintPersistentData float m_penetration; SimdVector3 m_frictionWorldTangential0; SimdVector3 m_frictionWorldTangential1; - + + ContactSolverFunc m_contactSolverFunc; + ContactSolverFunc m_frictionSolverFunc; }; diff --git a/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cpp b/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cpp index 26637bb99..32cda736a 100644 --- a/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cpp +++ b/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cpp @@ -32,7 +32,7 @@ subject to the following restrictions: int totalCpd = 0; - +int gTotalContactPoints = 0; bool MyContactDestroyedCallback(void* userPersistentData) { @@ -48,8 +48,17 @@ bool MyContactDestroyedCallback(void* userPersistentData) SequentialImpulseConstraintSolver::SequentialImpulseConstraintSolver() { gContactDestroyedCallback = &MyContactDestroyedCallback; -} + //initialize default friction/contact funcs + int i,j; + for (i=0;iGetBody0(); RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1(); - float maxImpulse = 0.f; //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop - if (iter == 0) { manifoldPtr->RefreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform()); int numpoints = manifoldPtr->GetNumContacts(); + gTotalContactPoints += numpoints; + SimdVector3 color(0,1,0); for (int i=0;im_jacDiagABInv = 1.f / jacDiagAB; - + //Dependent on Rigidbody A and B types, fetch the contact/friction response func + //perhaps do a similar thing for friction/restutution combiner funcs... + + cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType]; + cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType]; + SimdVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1); SimdVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2); SimdVector3 vel = vel1 - vel2; @@ -250,6 +275,15 @@ float SequentialImpulseConstraintSolver::Solve(PersistentManifold* manifoldPtr, } } +} + +float SequentialImpulseConstraintSolver::Solve(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer) +{ + + RigidBody* body0 = (RigidBody*)manifoldPtr->GetBody0(); + RigidBody* body1 = (RigidBody*)manifoldPtr->GetBody1(); + + float maxImpulse = 0.f; { const int numpoints = manifoldPtr->GetNumContacts(); @@ -276,14 +310,12 @@ float SequentialImpulseConstraintSolver::Solve(PersistentManifold* manifoldPtr, { - - //float dist = cp.GetDistance(); - //printf("dist(%i)=%f\n",j,dist); - float impulse = resolveSingleCollision( + ConstraintPersistentData* cpd = (ConstraintPersistentData*) cp.m_userPersistentData; + float impulse = cpd->m_contactSolverFunc( *body0,*body1, cp, info); - + if (maxImpulse < impulse) maxImpulse = impulse; @@ -312,13 +344,15 @@ float SequentialImpulseConstraintSolver::SolveFriction(PersistentManifold* manif // j = numpoints-1-i; ManifoldPoint& cp = manifoldPtr->GetContactPoint(j); - if (cp.GetDistance() <= 0.f) + if (cp.GetDistance() <= 0.f) { - resolveSingleFriction( + ConstraintPersistentData* cpd = (ConstraintPersistentData*) cp.m_userPersistentData; + cpd->m_frictionSolverFunc( *body0,*body1, cp, info); + } } diff --git a/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.h b/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.h index 598a4cfa9..c51006abf 100644 --- a/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.h +++ b/BulletDynamics/ConstraintSolver/SequentialImpulseConstraintSolver.h @@ -19,6 +19,10 @@ subject to the following restrictions: #include "ConstraintSolver.h" class IDebugDraw; +#include "ContactConstraint.h" + + + /// SequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses /// The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com /// Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP) @@ -27,12 +31,29 @@ class SequentialImpulseConstraintSolver : public ConstraintSolver { float Solve(PersistentManifold* manifold, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer); float SolveFriction(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,int iter,IDebugDraw* debugDrawer); - + void PrepareConstraints(PersistentManifold* manifoldPtr, const ContactSolverInfo& info,IDebugDraw* debugDrawer); + + ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; + ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; public: SequentialImpulseConstraintSolver(); + ///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody + ///See RigidBody::m_contactSolverType and RigidBody::m_frictionSolverType + void SetContactSolverFunc(ContactSolverFunc func,int type0,int type1) + { + m_contactDispatch[type0][type1] = func; + } + + ///Advanced: Override the default friction solving function for contacts, for certain types of rigidbody + ///See RigidBody::m_contactSolverType and RigidBody::m_frictionSolverType + void SetFrictionSolverFunc(ContactSolverFunc func,int type0,int type1) + { + m_frictionDispatch[type0][type1] = func; + } + virtual ~SequentialImpulseConstraintSolver() {} virtual float SolveGroup(PersistentManifold** manifold,int numManifolds,const ContactSolverInfo& info, IDebugDraw* debugDrawer=0); diff --git a/BulletDynamics/Dynamics/RigidBody.cpp b/BulletDynamics/Dynamics/RigidBody.cpp index c268515e3..8be8ff196 100644 --- a/BulletDynamics/Dynamics/RigidBody.cpp +++ b/BulletDynamics/Dynamics/RigidBody.cpp @@ -32,7 +32,9 @@ RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdSc m_angularVelocity(0.f,0.f,0.f), m_linearDamping(0.f), m_angularDamping(0.5f), - m_kinematicTimeStep(0.f) + m_kinematicTimeStep(0.f), + m_contactSolverType(0), + m_frictionSolverType(0) { //moved to CollisionObject diff --git a/BulletDynamics/Dynamics/RigidBody.h b/BulletDynamics/Dynamics/RigidBody.h index ba2399d2d..d8392046f 100644 --- a/BulletDynamics/Dynamics/RigidBody.h +++ b/BulletDynamics/Dynamics/RigidBody.h @@ -236,7 +236,10 @@ public: m_broadphaseProxy = broadphaseProxy; } - + //for experimental overriding of friction/contact solver func + int m_contactSolverType; + int m_frictionSolverType; + /// for ode solver-binding dMatrix3 m_R;//temp diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp index bddce553e..273e3146a 100644 --- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp +++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp @@ -13,8 +13,10 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -//#define USE_PARALLEL_DISPATCHER 1 +//#define USER_DEFINED_FRICTION_MODEL 1 +//#define PRINT_CONTACT_STATISTICS 1 +#define REGISTER_CUSTOM_COLLISION_ALGORITHM 1 #include "CcdPhysicsEnvironment.h" #include "ParallelPhysicsEnvironment.h" @@ -33,7 +35,7 @@ subject to the following restrictions: #include "CollisionShapes/TriangleIndexVertexArray.h" #include "CollisionShapes/BvhTriangleMeshShape.h" #include "CollisionShapes/TriangleMesh.h" - +#include "ConstraintSolver/SequentialImpulseConstraintSolver.h" #include "Dynamics/RigidBody.h" #include "CollisionDispatch/CollisionDispatcher.h" @@ -76,9 +78,9 @@ bool useCompound = false;//true;//false; #ifdef _DEBUG -const int numObjects = 120; +const int gNumObjects = 120; #else -const int numObjects = 120;//try this in release mode: 3000. never go above 4095, unless you increate maxNumObjects value in DemoApplication.cp +const int gNumObjects = 120;//try this in release mode: 3000. never go above 16384, unless you increate maxNumObjects value in DemoApplication.cp #endif const int maxNumObjects = 32760; @@ -147,6 +149,10 @@ int main(int argc,char** argv) +extern int gNumManifold; +extern int gOverlappingPairs; +extern int gTotalContactPoints; + void CcdPhysicsDemo::clientMoveAndDisplay() { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); @@ -166,6 +172,14 @@ void CcdPhysicsDemo::clientMoveAndDisplay() Profiler::endBlock("render"); #endif glFlush(); + //some additional debugging info +#ifdef PRINT_CONTACT_STATISTICS + printf("num manifolds: %i\n",gNumManifold); + printf("num gOverlappingPairs: %i\n",gOverlappingPairs); + printf("num gTotalContactPoints : %i\n",gTotalContactPoints ); +#endif //PRINT_CONTACT_STATISTICS + + gTotalContactPoints = 0; glutSwapBuffers(); } @@ -185,7 +199,6 @@ void CcdPhysicsDemo::displayCallback(void) { renderme(); - glFlush(); glutSwapBuffers(); } @@ -245,6 +258,13 @@ void CcdPhysicsDemo::clientResetScene() } +///User-defined friction model, the most simple friction model available: no friction +float myFrictionModel( RigidBody& body1, RigidBody& body2, ManifoldPoint& contactPoint, const ContactSolverInfo& solverInfo ) +{ + //don't do any friction + return 0.f; +} + void CcdPhysicsDemo::initPhysics() { @@ -254,11 +274,13 @@ void CcdPhysicsDemo::initPhysics() SimdVector3 worldAabbMin(-10000,-10000,-10000); SimdVector3 worldAabbMax(10000,10000,10000); - OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax,maxProxies,maxOverlap); + OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax,maxProxies); // OverlappingPairCache* broadphase = new SimpleBroadphase; +#ifdef REGISTER_CUSTOM_COLLISION_ALGORITHM dispatcher->RegisterCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,new SphereSphereCollisionAlgorithm::CreateFunc); - +#endif //REGISTER_CUSTOM_COLLISION_ALGORITHM + #ifdef USE_PARALLEL_DISPATCHER m_physicsEnvironmentPtr = new ParallelPhysicsEnvironment(dispatcher2,broadphase); @@ -271,13 +293,23 @@ void CcdPhysicsDemo::initPhysics() m_physicsEnvironmentPtr->setDebugDrawer(&debugDrawer); +#ifdef USER_DEFINED_FRICTION_MODEL + SequentialImpulseConstraintSolver* solver = (SequentialImpulseConstraintSolver*) m_physicsEnvironmentPtr->GetConstraintSolver(); + //solver->SetContactSolverFunc(ContactSolverFunc func,USER_CONTACT_SOLVER_TYPE1,DEFAULT_CONTACT_SOLVER_TYPE); + solver->SetFrictionSolverFunc(myFrictionModel,USER_CONTACT_SOLVER_TYPE1,DEFAULT_CONTACT_SOLVER_TYPE); + solver->SetFrictionSolverFunc(myFrictionModel,DEFAULT_CONTACT_SOLVER_TYPE,USER_CONTACT_SOLVER_TYPE1); + solver->SetFrictionSolverFunc(myFrictionModel,USER_CONTACT_SOLVER_TYPE1,USER_CONTACT_SOLVER_TYPE1); + //m_physicsEnvironmentPtr->setNumIterations(2); +#endif //USER_DEFINED_FRICTION_MODEL + + int i; SimdTransform tr; tr.setIdentity(); - for (i=0;i0) { @@ -301,7 +333,7 @@ void CcdPhysicsDemo::initPhysics() compoundShape->AddChildShape(ident,new SphereShape(0.9));// } - for (i=0;iSetMargin(0.05f); @@ -311,7 +343,30 @@ void CcdPhysicsDemo::initPhysics() SimdTransform trans; trans.setIdentity(); - trans.setOrigin(SimdVector3(0,-30+i*CUBE_HALF_EXTENTS*2,0)); + if (i>0) + { + //stack them + int colsize = 10; + int row = (i*CUBE_HALF_EXTENTS*2)/(colsize*2*CUBE_HALF_EXTENTS); + int row2 = row; + int col = (i)%(colsize)-colsize/2; + + + if (col>3) + { + col=11; + row2 |=1; + } + + SimdVector3 pos(col*2*CUBE_HALF_EXTENTS + (row2%2)*CUBE_HALF_EXTENTS, + row*2*CUBE_HALF_EXTENTS+CUBE_HALF_EXTENTS+EXTRA_HEIGHT,0); + + trans.setOrigin(pos); + } else + { + trans.setOrigin(SimdVector3(0,-30,0)); + } + float mass = 1.f; if (!isDyna) @@ -324,7 +379,10 @@ void CcdPhysicsDemo::initPhysics() //Experimental: better estimation of CCD Time of Impact: ctrl->GetRigidBody()->m_ccdSweptShereRadius = 0.2*CUBE_HALF_EXTENTS; - +#ifdef USER_DEFINED_FRICTION_MODEL + ///Advanced use: override the friction solver + ctrl->GetRigidBody()->m_frictionSolverType = USER_CONTACT_SOLVER_TYPE1; +#endif //USER_DEFINED_FRICTION_MODEL } diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp index 4cc8fa221..ac2473439 100644 --- a/Demos/OpenGL/DemoApplication.cpp +++ b/Demos/OpenGL/DemoApplication.cpp @@ -23,11 +23,11 @@ subject to the following restrictions: #include "CollisionShapes/CollisionShape.h" #include "CollisionShapes/BoxShape.h" #include "GL_ShapeDrawer.h" - +#include "quickprof.h" #include "BMF_Api.h" int numObjects = 0; -const int maxNumObjects = 4096; +const int maxNumObjects = 16384; DefaultMotionState ms[maxNumObjects]; CcdPhysicsController* physObjects[maxNumObjects]; SimdTransform startTransforms[maxNumObjects]; @@ -889,6 +889,7 @@ void DemoApplication::renderme() BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; + } } diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index 83c79e7f7..2f0b4adb8 100644 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -1707,18 +1707,25 @@ PHY_IVehicle* CcdPhysicsEnvironment::getVehicleConstraint(int constraintId) #endif //NEW_BULLET_VEHICLE_SUPPORT +int currentController = 0; +int numController = 0; + void CcdPhysicsEnvironment::UpdateAabbs(float timeStep) { std::vector::iterator i; BroadphaseInterface* scene = GetBroadphase(); + numController = m_controllers.size(); + currentController = 0; + // // update aabbs, only for moving objects (!) // for (i=m_controllers.begin(); !(i==m_controllers.end()); i++) { + currentController++; CcdPhysicsController* ctrl = (*i); RigidBody* body = ctrl->GetRigidBody(); diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h index 3f03371cc..cd510f1c5 100644 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h +++ b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h @@ -234,6 +234,11 @@ protected: return m_islandManager; } + class ConstraintSolver* GetConstraintSolver() + { + return m_solver; + } + protected: