From 151cd4b9dafa8a7475982883e14cb34165773452 Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Sat, 17 Mar 2007 00:09:12 +0000 Subject: [PATCH] prepare and added constraint solver optimizations, not activated yet. --- Extras/quickstep/OdeConstraintSolver.cpp | 8 +- Extras/quickstep/OdeConstraintSolver.h | 2 +- .../CollisionDispatch/btCollisionObject.h | 12 + .../CollisionDispatch/btManifoldResult.cpp | 19 +- .../btSimulationIslandManager.cpp | 21 +- .../btSimulationIslandManager.h | 3 +- .../NarrowPhaseCollision/btManifoldPoint.h | 7 +- .../btPersistentManifold.h | 16 +- .../ConstraintSolver/btConstraintSolver.h | 4 +- .../ConstraintSolver/btContactConstraint.cpp | 9 +- .../ConstraintSolver/btContactConstraint.h | 9 + .../btSequentialImpulseConstraintSolver.cpp | 643 +++++++++++++++++- .../btSequentialImpulseConstraintSolver.h | 7 +- .../ConstraintSolver/btsolverbody.h | 71 ++ .../ConstraintSolver/btsolverconstraint.h | 63 ++ .../Dynamics/btDiscreteDynamicsWorld.cpp | 18 +- src/BulletDynamics/Dynamics/btRigidBody.cpp | 4 +- .../Dynamics/btSimpleDynamicsWorld.cpp | 12 +- src/LinearMath/btAlignedObjectArray.h | 11 + src/LinearMath/btQuickprof.cpp | 2 +- src/LinearMath/btQuickprof.h | 12 +- 21 files changed, 897 insertions(+), 56 deletions(-) create mode 100644 src/BulletDynamics/ConstraintSolver/btsolverbody.h create mode 100644 src/BulletDynamics/ConstraintSolver/btsolverconstraint.h diff --git a/Extras/quickstep/OdeConstraintSolver.cpp b/Extras/quickstep/OdeConstraintSolver.cpp index 43d433369..e7c2b4bbe 100644 --- a/Extras/quickstep/OdeConstraintSolver.cpp +++ b/Extras/quickstep/OdeConstraintSolver.cpp @@ -26,6 +26,7 @@ subject to the following restrictions: #include "OdeContactJoint.h" #include "OdeSolverBody.h" #include +#include "LinearMath/btQuickprof.h" #include "LinearMath/btIDebugDraw.h" @@ -71,8 +72,10 @@ m_erp(0.4f) //iterative lcp and penalty method -btScalar OdeConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar OdeConstraintSolver::solveGroup(btCollisionObject** bodies,int numBulletBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) { + BEGIN_PROFILE("prepareConstraints"); + m_CurBody = 0; m_CurJoint = 0; @@ -95,6 +98,8 @@ btScalar OdeConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int } } + END_PROFILE("prepareConstraints"); + BEGIN_PROFILE("solveConstraints"); SolveInternal1(m_cfm,m_erp,odeBodies,numBodies,joints,numJoints,infoGlobal); //write back resulting velocities @@ -106,6 +111,7 @@ btScalar OdeConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int odeBodies[i]->m_originalBody->setAngularVelocity(odeBodies[i]->m_angularVelocity); } } + END_PROFILE("solveConstraints"); return 0.f; } diff --git a/Extras/quickstep/OdeConstraintSolver.h b/Extras/quickstep/OdeConstraintSolver.h index c84242b68..a8fd0b81e 100644 --- a/Extras/quickstep/OdeConstraintSolver.h +++ b/Extras/quickstep/OdeConstraintSolver.h @@ -45,7 +45,7 @@ public: virtual ~OdeConstraintSolver() {} - virtual btScalar solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info,btIDebugDraw* debugDrawer = 0); + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); ///setConstraintForceMixing, the cfm adds some positive value to the main diagonal ///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy' diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 1c0e566ac..9e33634e5 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -54,6 +54,8 @@ protected: int m_collisionFlags; int m_islandTag1; + int m_companionId; + int m_activationState1; btScalar m_deactivationTime; @@ -251,6 +253,16 @@ public: m_islandTag1 = tag; } + const int getCompanionId() const + { + return m_companionId; + } + + void setCompanionId(int id) + { + m_companionId = id; + } + const btScalar getHitFraction() const { return m_hitFraction; diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index 975101d3b..490acc0b6 100644 --- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -63,15 +63,22 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b bool isSwapped = m_manifoldPtr->getBody0() != m_body0; - btTransform transAInv = isSwapped? m_rootTransB.inverse() : m_rootTransA.inverse(); - btTransform transBInv = isSwapped? m_rootTransA.inverse() : m_rootTransB.inverse(); - btVector3 pointA = pointInWorld + normalOnBInWorld * depth; - btVector3 localA = transAInv(pointA ); - btVector3 localB = transBInv(pointInWorld); - btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + btVector3 localA; + btVector3 localB; + if (isSwapped) + { + localA = m_rootTransB.invXform(pointA ); + localB = m_rootTransA.invXform(pointInWorld); + } else + { + localA = m_rootTransA.invXform(pointA ); + localB = m_rootTransB.invXform(pointInWorld); + } + + btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); int insertIndex = m_manifoldPtr->getCacheEntry(newPt); diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 122071f80..463792b58 100644 --- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -8,7 +8,7 @@ #include "BulletCollision/CollisionDispatch/btCollisionWorld.h" #include - +#include "LinearMath/btQuickprof.h" btSimulationIslandManager::btSimulationIslandManager() { @@ -63,6 +63,7 @@ void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld { btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i]; collisionObject->setIslandTag(index); + collisionObject->setCompanionId(-1); collisionObject->setHitFraction(btScalar(1.)); index++; @@ -93,9 +94,11 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col if (collisionObject->mergesSimulationIslands()) { collisionObject->setIslandTag( m_unionFind.find(index) ); + collisionObject->setCompanionId(-1); } else { collisionObject->setIslandTag(-1); + collisionObject->setCompanionId(-2); } index++; } @@ -137,6 +140,11 @@ class btPersistentManifoldSortPredicate // void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback) { + + + BEGIN_PROFILE("islandUnionFindAndHeapSort"); + + //we are going to sort the unionfind array, and store the element id in the size //afterwards, we clean unionfind, to make sure no-one uses it anymore @@ -278,6 +286,10 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, //int islandId; + END_PROFILE("islandUnionFindAndHeapSort"); + + btAlignedObjectArray islandBodies; + //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated for (int startIslandIndex=0;startIslandIndexisActive()) islandSleeping = true; } @@ -319,12 +332,16 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, if (!islandSleeping) { - callback->ProcessIsland(startManifold,numIslandManifolds, islandId); + callback->ProcessIsland(&islandBodies[0],islandBodies.size(),startManifold,numIslandManifolds, islandId); } if (numIslandManifolds) { startManifoldIndex = endManifoldIndex; } + + islandBodies.resize(0); } + + } diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h index 1bde7db0e..69da7e899 100644 --- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -19,6 +19,7 @@ subject to the following restrictions: #include "../CollisionDispatch/btUnionFind.h" #include "btCollisionCreateFunc.h" +class btCollisionObject; class btCollisionWorld; class btDispatcher; @@ -49,7 +50,7 @@ public: { virtual ~IslandCallback() {}; - virtual void ProcessIsland(class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; + virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; }; void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback); diff --git a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index 67f3e68eb..f6a893151 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -29,7 +29,8 @@ class btManifoldPoint { public: btManifoldPoint() - :m_userPersistentData(0) + :m_userPersistentData(0), + m_lifeTime(0) { } @@ -76,12 +77,12 @@ class btManifoldPoint return m_lifeTime; } - btVector3 getPositionWorldOnA() { + const btVector3& getPositionWorldOnA() const { return m_positionWorldOnA; // return m_positionWorldOnB + m_normalWorldOnB * m_distance1; } - const btVector3& getPositionWorldOnB() + const btVector3& getPositionWorldOnB() const { return m_positionWorldOnB; } diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index eea34f2ba..c2dbd4384 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -115,11 +115,23 @@ public: } void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex) { - assert(validContactDistance(newPoint)); + btAssert(validContactDistance(newPoint)); - clearUserCache(m_pointCache[insertIndex]); +#define MAINTAIN_PERSISTENCY 1 +#ifdef MAINTAIN_PERSISTENCY + int lifeTime = m_pointCache[insertIndex].getLifeTime(); + btAssert(lifeTime>=0); + void* cache = m_pointCache[insertIndex].m_userPersistentData; m_pointCache[insertIndex] = newPoint; + + m_pointCache[insertIndex].m_userPersistentData = cache; + m_pointCache[insertIndex].m_lifeTime = lifeTime; +#else + clearUserCache(m_pointCache[insertIndex]); + m_pointCache[insertIndex] = newPoint; + +#endif } bool validContactDistance(const btManifoldPoint& pt) const diff --git a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index 8134620b9..7e8458c2c 100644 --- a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -20,10 +20,12 @@ subject to the following restrictions: class btPersistentManifold; class btRigidBody; +class btCollisionObject; class btTypedConstraint; struct btContactSolverInfo; struct btBroadphaseProxy; class btIDebugDraw; +class btStackAlloc; /// btConstraintSolver provides solver interface class btConstraintSolver @@ -33,7 +35,7 @@ public: virtual ~btConstraintSolver() {} - virtual btScalar solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer = 0) = 0; + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) = 0; }; diff --git a/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index e75242010..ee6922191 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -89,12 +89,13 @@ btScalar resolveSingleCollision( const btContactSolverInfo& solverInfo) { - const btVector3& pos1 = contactPoint.getPositionWorldOnA(); - const btVector3& pos2 = contactPoint.getPositionWorldOnB(); + const btVector3& pos1_ = contactPoint.getPositionWorldOnA(); + const btVector3& pos2_ = contactPoint.getPositionWorldOnB(); const btVector3& normal = contactPoint.m_normalWorldOnB; - btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); + //constant over all iterations + btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition(); btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); diff --git a/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/src/BulletDynamics/ConstraintSolver/btContactConstraint.h index f8f99cd68..0834dedde 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -110,4 +110,13 @@ btScalar resolveSingleFriction( const btContactSolverInfo& solverInfo ); + + +btScalar resolveSingleCollisionCombined( + btRigidBody& body1, + btRigidBody& body2, + btManifoldPoint& contactPoint, + const btContactSolverInfo& solverInfo + ); + #endif //CONTACT_CONSTRAINT_H diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index f878ec8c6..95100e9ed 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -25,6 +25,12 @@ subject to the following restrictions: #include "LinearMath/btMinMax.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" #include +#include "LinearMath/btStackAlloc.h" +#include "LinearMath/btQuickprof.h" +#include "btSolverBody.h" +#include "btSolverConstraint.h" + +#include "LinearMath/btAlignedObjectArray.h" #ifdef USE_PROFILE #include "LinearMath/btQuickprof.h" @@ -105,7 +111,7 @@ bool MyContactDestroyedCallback(void* userPersistentData) btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() -:m_solverMode(SOLVER_RANDMIZE_ORDER) //not using SOLVER_USE_WARMSTARTING +:m_solverMode(SOLVER_RANDMIZE_ORDER)//SOLVER_USE_WARMSTARTING)//SOLVER_RANDMIZE_ORDER) //not using SOLVER_USE_WARMSTARTING { gContactDestroyedCallback = &MyContactDestroyedCallback; @@ -120,10 +126,579 @@ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() } } -/// btSequentialImpulseConstraintSolver Sequentially applies impulses -btScalar btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) + +void initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody) +{ + int size = sizeof(btSolverBody); + int sizeofrb = sizeof(btRigidBody); + int sizemanifold = sizeof(btPersistentManifold); + int sizeofmp = sizeof(btManifoldPoint); + int sizeofPersistData = sizeof (btConstraintPersistentData); + + solverBody->m_angularVelocity = rigidbody->getAngularVelocity(); + solverBody->m_centerOfMassPosition = rigidbody->getCenterOfMassPosition(); + solverBody->m_friction = rigidbody->getFriction(); + solverBody->m_invInertiaWorld = rigidbody->getInvInertiaTensorWorld(); + solverBody->m_invMass = rigidbody->getInvMass(); + solverBody->m_linearVelocity = rigidbody->getLinearVelocity(); + solverBody->m_originalBody = rigidbody; +} + +btScalar penetrationResolveFactor = btScalar(0.9); +btScalar restitutionCurve(btScalar rel_vel, btScalar restitution) +{ + btScalar rest = restitution * -rel_vel; + return rest; +} + + + + + + +//velocity + friction +//response between two dynamic objects with friction +SIMD_FORCE_INLINE btScalar resolveSingleCollisionCombinedCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo) +{ + btScalar normalImpulse(0.f); + { + if (contactConstraint.m_penetration < 0.f) + return 0.f; + + // Optimized version of projected relative velocity, use precomputed cross products with normal + // body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1); + // body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); + // btVector3 vel = vel1 - vel2; + // btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel); + + btScalar rel_vel; + btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) + + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); + btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) + + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); + + rel_vel = vel1Dotn-vel2Dotn; + + + btScalar positionalError = contactConstraint.m_penetration; + btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping; + + btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv; + btScalar normalImpulse = penetrationImpulse+velocityImpulse; + + // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse + btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse; + btScalar sum = oldNormalImpulse + normalImpulse; + contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum; + + btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse; + btScalar velocitySum = oldVelocityImpulse + velocityImpulse; + contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum; + + normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse; + + if (body1.m_invMass) + { + body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass, + contactConstraint.m_angularComponentA,normalImpulse); + } + if (body2.m_invMass) + { + body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass, + contactConstraint.m_angularComponentB,-normalImpulse); + } + + } + + + + return normalImpulse; +} + + +#ifndef NO_FRICTION_TANGENTIALS + +SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo, + btScalar appliedNormalImpulse) { + + btScalar combinedFriction = contactConstraint.m_friction; + + btScalar limit = appliedNormalImpulse * combinedFriction; + + if (appliedNormalImpulse>btScalar(0.)) + //friction + { + + btScalar j1; + { + + btScalar rel_vel; + btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) + + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity); + btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) + + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity); + rel_vel = vel1Dotn-vel2Dotn; + + // calculate j that moves us to zero relative velocity + j1 = -rel_vel * contactConstraint.m_jacDiagABInv; + btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse; + contactConstraint.m_appliedImpulse = oldTangentImpulse + j1; + GEN_set_min(contactConstraint.m_appliedImpulse, limit); + GEN_set_max(contactConstraint.m_appliedImpulse, -limit); + j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse; + + } + + if (body1.m_invMass) + { + body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1); + } + if (body2.m_invMass) + { + body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1); + } + + } + return 0.f; +} + + +#else + +//velocity + friction +//response between two dynamic objects with friction +btScalar resolveSingleFrictionCacheFriendly( + btSolverBody& body1, + btSolverBody& body2, + btSolverConstraint& contactConstraint, + const btContactSolverInfo& solverInfo) +{ + + btVector3 vel1; + btVector3 vel2; + btScalar normalImpulse(0.f); + + { + const btVector3& normal = contactConstraint.m_contactNormal; + if (contactConstraint.m_penetration < 0.f) + return 0.f; + + + body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1); + body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = normal.dot(vel); + + btVector3 lat_vel = vel - normal * rel_vel; + btScalar lat_rel_vel = lat_vel.length2(); + + btScalar combinedFriction = contactConstraint.m_friction; + const btVector3& rel_pos1 = contactConstraint.m_rel_posA; + const btVector3& rel_pos2 = contactConstraint.m_rel_posB; + + + //if (contactConstraint.m_appliedVelocityImpulse > 0.f) + if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON) + { + lat_rel_vel = btSqrt(lat_rel_vel); + + lat_vel /= lat_rel_vel; + btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel); + btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel); + btScalar friction_impulse = lat_rel_vel / + (body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2))); + btScalar normal_impulse = contactConstraint.m_appliedVelocityImpulse * combinedFriction; + + GEN_set_min(friction_impulse, normal_impulse); + GEN_set_max(friction_impulse, -normal_impulse); + body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1); + body2.applyImpulse(lat_vel * friction_impulse, rel_pos2); + } + } + + return normalImpulse; +} + +#endif //NO_FRICTION_TANGENTIALS + +btAlignedObjectArray tmpSolverBodyPool; +btAlignedObjectArray tmpSolverConstraintPool; +btAlignedObjectArray tmpSolverFrictionConstraintPool; + + +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +{ + + if (!(numConstraints + numManifolds)) + return 0.f; + + BEGIN_PROFILE("gatherSolverData"); + + + + int sizeofSB = sizeof(btSolverBody); + int sizeofSC = sizeof(btSolverConstraint); + + + if (numManifolds) + { + //if m_stackAlloc, try to pack bodies/constraints to speed up solving +// btBlock* sablock; +// sablock = stackAlloc->beginBlock(); + + // int memsize = 16; +// unsigned char* stackMemory = stackAlloc->allocate(memsize); + + + tmpSolverBodyPool.reserve(numManifolds*2); + + { + for (int i=0;igetIslandTag() >= 0)) + { + btAssert(rb->getCompanionId() < 0); + int solverBodyId = tmpSolverBodyPool.size(); + btSolverBody& solverBody = tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,rb); + rb->setCompanionId(solverBodyId); + } + } + } + + + tmpSolverConstraintPool.reserve(numManifolds*4+numConstraints); + tmpSolverFrictionConstraintPool.reserve(numManifolds*4); + + { + int i; + for (i=0;igetBody0(); + btRigidBody* rb1 = (btRigidBody*)manifold->getBody1(); + + manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform()); + + int solverBodyIdA; + int solverBodyIdB; + + if (manifold->getNumContacts()) + { + + + + if (rb0->getIslandTag() >= 0) + { + solverBodyIdA = rb0->getCompanionId(); + } else + { + //create a static body + solverBodyIdA = tmpSolverBodyPool.size(); + btSolverBody& solverBody = tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,rb0); + } + + if (rb1->getIslandTag() >= 0) + { + solverBodyIdB = rb1->getCompanionId(); + } else + { + //create a static body + solverBodyIdB = tmpSolverBodyPool.size(); + btSolverBody& solverBody = tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,rb1); + } + } + + for (int j=0;jgetNumContacts();j++) + { + + btManifoldPoint& cp = manifold->getContactPoint(j); + + int frictionIndex = tmpSolverConstraintPool.size(); + + if (cp.getDistance() <= btScalar(0.)) + { + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btVector3 rel_pos1 = pos1 - rb0->getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2 - rb1->getCenterOfMassPosition(); + + + btScalar relaxation = 1.f; + + { + btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand(); + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D; + + + + { + //can be optimized, the cross products are already calculated + btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + } + + solverConstraint.m_contactNormal = cp.m_normalWorldOnB; + solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB); + + + btVector3 vel1 = rb0->getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2); + + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = cp.m_normalWorldOnB.dot(vel); + + + solverConstraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations); + solverConstraint.m_friction = cp.m_combinedFriction; + btScalar rest = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (rest <= btScalar(0.)) + { + rest = 0.f; + }; + + btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep; + if (rest > penVel) + { + rest = btScalar(0.); + } + solverConstraint.m_restitution = rest; + + solverConstraint.m_penetration *= -(infoGlobal.m_erp/infoGlobal.m_timeStep); + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedVelocityImpulse = 0.f; + + + btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0; + btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1; + } + + //create 2 '1d axis' constraints for 2 tangential friction directions + + //re-calculate friction direction every frame, todo: check if this is really needed + btVector3 frictionTangential0a, frictionTangential1b; + + btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b); + + { + btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand(); + solverConstraint.m_contactNormal = frictionTangential0a; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; + solverConstraint.m_frictionIndex = frictionIndex; + + solverConstraint.m_friction = cp.m_combinedFriction; + + solverConstraint.m_appliedImpulse = btScalar(0.); + solverConstraint.m_appliedVelocityImpulse = 0.f; + + btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + + { + btVector3 ftorqueAxis0 = rel_pos1.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos1CrossNormal = ftorqueAxis0; + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis0; + } + { + btVector3 ftorqueAxis0 = rel_pos2.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos2CrossNormal = ftorqueAxis0; + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis0; + } + + } + + + { + + btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand(); + solverConstraint.m_contactNormal = frictionTangential1b; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; + solverConstraint.m_frictionIndex = frictionIndex; + + solverConstraint.m_friction = cp.m_combinedFriction; + + solverConstraint.m_appliedImpulse = btScalar(0.); + solverConstraint.m_appliedVelocityImpulse = 0.f; + + btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + { + btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1; + } + { + btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1; + } + } + + } + } + } + } + } + END_PROFILE("gatherSolverData"); + + BEGIN_PROFILE("prepareConstraints"); + + btContactSolverInfo info = infoGlobal; + + { + int j; + for (j=0;jbuildJacobian(); + } + } + + END_PROFILE("prepareConstraints"); + + + BEGIN_PROFILE("solveConstraints"); + + //should traverse the contacts random order... + int iteration; + int j; + { + for ( iteration = 0;iterationsolveConstraint(info.m_timeStep); + } + + { + int numPoolConstraints = tmpSolverConstraintPool.size(); + for (j=0;jm_penetration = cp.getDistance(); + cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations); cpd->m_friction = cp.m_combinedFriction; cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution); - if (cpd->m_restitution <= 0.) //btScalar(0.)) + if (cpd->m_restitution <= btScalar(0.)) { cpd->m_restitution = btScalar(0.0); @@ -404,6 +985,44 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol } } + +btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) +{ + btScalar maxImpulse = btScalar(0.); + + { + + btVector3 color(0,1,0); + { + if (cp.getDistance() <= btScalar(0.)) + { + + if (iter == 0) + { + if (debugDrawer) + debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); + } + + { + + btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; + btScalar impulse = resolveSingleCollisionCombined( + *body0,*body1, + cp, + info); + + if (maxImpulse < impulse) + maxImpulse = impulse; + + } + } + } + } + return maxImpulse; +} + + + btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer) { diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 2f92bee63..642998cb7 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -67,7 +67,12 @@ public: virtual ~btSequentialImpulseConstraintSolver() {} - virtual btScalar solveGroup(btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0); + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc); + + virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + + btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer); + void setSolverMode(int mode) { diff --git a/src/BulletDynamics/ConstraintSolver/btsolverbody.h b/src/BulletDynamics/ConstraintSolver/btsolverbody.h new file mode 100644 index 000000000..e0d13f0e5 --- /dev/null +++ b/src/BulletDynamics/ConstraintSolver/btsolverbody.h @@ -0,0 +1,71 @@ +/* +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 BT_SOLVER_BODY_H +#define BT_SOLVER_BODY_H + +class btRigidBody; +#include "LinearMath/btVector3.h" +#include "LinearMath/btMatrix3x3.h" + + + + +ATTRIBUTE_ALIGNED16 (struct btSolverBody) +{ + btMatrix3x3 m_invInertiaWorld; + btVector3 m_centerOfMassPosition; + btVector3 m_linearVelocity; + btVector3 m_angularVelocity; + btRigidBody* m_originalBody; + float m_invMass; + float m_friction; + float m_unused; + btVector3 m_unused2; + + inline void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const + { + velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos); + } + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + { + m_linearVelocity += linearComponent*impulseMagnitude; + m_angularVelocity += angularComponent*impulseMagnitude; + } + + void writebackVelocity() + { + if (m_invMass) + { + m_originalBody->setLinearVelocity(m_linearVelocity); + m_originalBody->setAngularVelocity(m_angularVelocity); + } + } + + inline void applyImpulse(const btVector3& impulse,const btVector3& rel_pos) + { + if (m_invMass) + { + m_linearVelocity += impulse * m_invMass; + btVector3 torqueImpulse = rel_pos.cross(impulse); + m_angularVelocity += m_invInertiaWorld * torqueImpulse; + } + } + +}; + +#endif //BT_SOLVER_BODY_H \ No newline at end of file diff --git a/src/BulletDynamics/ConstraintSolver/btsolverconstraint.h b/src/BulletDynamics/ConstraintSolver/btsolverconstraint.h new file mode 100644 index 000000000..01e11f0a5 --- /dev/null +++ b/src/BulletDynamics/ConstraintSolver/btsolverconstraint.h @@ -0,0 +1,63 @@ + + +/* +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 BT_SOLVER_CONSTRAINT_H +#define BT_SOLVER_CONSTRAINT_H + +class btRigidBody; +#include "LinearMath/btVector3.h" +#include "LinearMath/btMatrix3x3.h" + +//#define NO_FRICTION_TANGENTIALS 1 + +///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. +ATTRIBUTE_ALIGNED16 (struct btSolverConstraint) +{ + btVector3 m_relpos1CrossNormal; + btVector3 m_relpos2CrossNormal; + btVector3 m_contactNormal; + btVector3 m_angularComponentA; + btVector3 m_angularComponentB; + + btScalar m_appliedVelocityImpulse; + int m_solverBodyIdA; + int m_solverBodyIdB; + btScalar m_friction; + btScalar m_restitution; + btScalar m_jacDiagABInv; + btScalar m_penetration; + btScalar m_appliedImpulse; + + int m_constraintType; + int m_frictionIndex; + int m_unusedPadding[2]; + + enum btSolverConstraintType + { + BT_SOLVER_CONTACT_1D = 0, + BT_SOLVER_FRICTION_1D + }; +}; + + + + + + +#endif //BT_SOLVER_CONSTRAINT_H + + diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 3cec7d753..f2692c3f4 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -421,8 +421,6 @@ class btSortConstraintOnIslandPredicate void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { - BEGIN_PROFILE("solveConstraints"); - struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback { @@ -431,7 +429,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btTypedConstraint** m_sortedConstraints; int m_numConstraints; btIDebugDraw* m_debugDrawer; - + btStackAlloc* m_stackAlloc; InplaceSolverIslandCallback( @@ -439,17 +437,19 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btConstraintSolver* solver, btTypedConstraint** sortedConstraints, int numConstraints, - btIDebugDraw* debugDrawer) + btIDebugDraw* debugDrawer, + btStackAlloc* stackAlloc) :m_solverInfo(solverInfo), m_solver(solver), m_sortedConstraints(sortedConstraints), m_numConstraints(numConstraints), - m_debugDrawer(debugDrawer) + m_debugDrawer(debugDrawer), + m_stackAlloc(stackAlloc) { } - virtual void ProcessIsland(btPersistentManifold** manifolds,int numManifolds, int islandId) + virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) { //also add all non-contact constraints/joints for this island btTypedConstraint** startConstraint = 0; @@ -474,12 +474,11 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) } } - m_solver->solveGroup( manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer); + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc); } }; - //sorted version of all btTypedConstraint, based on islandId btAlignedObjectArray sortedConstraints; sortedConstraints.resize( m_constraints.size()); @@ -497,14 +496,13 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0; - InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer); + InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer,m_stackAlloc); /// solve all the constraints for this island m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback); - END_PROFILE("solveConstraints"); } diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index 67a798626..d8808cd12 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -110,6 +110,8 @@ btRigidBody::btRigidBody( btScalar mass,const btTransform& worldTransform,btColl #endif //OBSOLETE_MOTIONSTATE_LESS + + //#define EXPERIMENTAL_JITTER_REMOVAL 1 #ifdef EXPERIMENTAL_JITTER_REMOVAL //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate @@ -119,7 +121,7 @@ btScalar gClippedAngvelThresholdSqr = btScalar(0.01); btScalar gClippedLinearThresholdSqr = btScalar(0.01); #endif //EXPERIMENTAL_JITTER_REMOVAL -btScalar gJitterVelocityDampingFactor = btScalar(1.); +btScalar gJitterVelocityDampingFactor = btScalar(0.9); void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) { diff --git a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 460564928..fdec1cd02 100644 --- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -71,7 +71,7 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b btContactSolverInfo infoGlobal; infoGlobal.m_timeStep = timeStep; - m_constraintSolver->solveGroup(manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer); + m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc); } ///integrate transforms @@ -89,7 +89,7 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) { m_gravity = gravity; - for (unsigned int i=0;i btProfiler::mProfileBlocks; diff --git a/src/LinearMath/btQuickprof.h b/src/LinearMath/btQuickprof.h index dbc7a46b8..9389fcd81 100644 --- a/src/LinearMath/btQuickprof.h +++ b/src/LinearMath/btQuickprof.h @@ -22,6 +22,12 @@ subject to the following restrictions: #ifndef QUICK_PROF_H #define QUICK_PROF_H +#include "btScalar.h" + +//#define USE_QUICKPROF 1 +//Don't use quickprof for now, because it contains STL. TODO: replace STL by Bullet container classes. + + //if you don't need btClock, you can comment next line #define USE_BT_CLOCK 1 @@ -224,13 +230,10 @@ class btClock #endif //USE_BT_CLOCK -//#define USE_QUICKPROF 1 -//Don't use quickprof for now, because it contains STL. TODO: replace STL by Bullet container classes. - #ifdef USE_QUICKPROF -//#include +#include #include #include #include @@ -355,6 +358,7 @@ public: /// Prints an error message to standard output. inline static void printError(const std::string& msg) { + //btAssert(0); std::cout << "[QuickProf error] " << msg << std::endl; }