Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

prepare and added constraint solver optimizations, not activated yet.

Browse Source
This commit is contained in:
ejcoumans
2007-03-17 00:09:12 +00:00
parent ea97ed3e30
commit 151cd4b9da
21 changed files with 897 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
Extras/quickstep/OdeConstraintSolver.cpp
View File

@@ -26,6 +26,7 @@ subject to the following restrictions:
#include "OdeContactJoint.h"
#include "OdeSolverBody.h"
#include <new.h>
#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;
}

2
Extras/quickstep/OdeConstraintSolver.h
View File

@@ -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'

12
src/BulletCollision/CollisionDispatch/btCollisionObject.h
View File

@@ -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;

19
src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
View File

@@ -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);

21
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
View File

@@ -8,7 +8,7 @@
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
#include <stdio.h>
#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<btCollisionObject*> islandBodies;
//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
for (int startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
@@ -291,6 +303,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
{
int i = getUnionFind().getElement(endIslandIndex).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
islandBodies.push_back(colObj0);
if (!colObj0->isActive())
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);
}
}

3
src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h
View File

@@ -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);

7
src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
View File

@@ -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;
}

16
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
View File

@@ -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

4
src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
View File

@@ -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;
};

9
src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
View File

@@ -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);

9
src/BulletDynamics/ConstraintSolver/btContactConstraint.h
View File

@@ -110,4 +110,13 @@ btScalar resolveSingleFriction(
const btContactSolverInfo& solverInfo
);
btScalar resolveSingleCollisionCombined(
btRigidBody& body1,
btRigidBody& body2,
btManifoldPoint& contactPoint,
const btContactSolverInfo& solverInfo
);
#endif //CONTACT_CONSTRAINT_H

643
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -25,6 +25,12 @@ subject to the following restrictions:
#include "LinearMath/btMinMax.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
#include <new>
#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<btSolverBody> tmpSolverBodyPool;
btAlignedObjectArray<btSolverConstraint> tmpSolverConstraintPool;
btAlignedObjectArray<btSolverConstraint> 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;i<numBodies;i++)
{
btRigidBody* rb = btRigidBody::upcast(bodies[i]);
if (rb && (rb->getIslandTag() >= 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;i<numManifolds;i++)
{
btPersistentManifold* manifold = manifoldPtr[i];
btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
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;j<manifold->getNumContacts();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;j<numConstraints;j++)
{
btTypedConstraint* constraint = constraints[j];
constraint->buildJacobian();
}
}
END_PROFILE("prepareConstraints");
BEGIN_PROFILE("solveConstraints");
//should traverse the contacts random order...
int iteration;
int j;
{
for ( iteration = 0;iteration<info.m_numIterations;iteration++)
{
for (j=0;j<numConstraints;j++)
{
btTypedConstraint* constraint = constraints[j];
constraint->solveConstraint(info.m_timeStep);
}
{
int numPoolConstraints = tmpSolverConstraintPool.size();
for (j=0;j<numPoolConstraints;j++)
{
btSolverConstraint& solveManifold = tmpSolverConstraintPool[j];
switch (solveManifold.m_constraintType)
{
case btSolverConstraint::BT_SOLVER_CONTACT_1D:
{
resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info);
break;
}
case btSolverConstraint::BT_SOLVER_FRICTION_1D:
{
break;
}
default:
{
//undefined constraint type?
btAssert(0);
}
};
}
}
{
int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++)
{
btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[j];
switch (solveManifold.m_constraintType)
{
case btSolverConstraint::BT_SOLVER_CONTACT_1D:
{
break;
}
case btSolverConstraint::BT_SOLVER_FRICTION_1D:
{
btScalar appliedNormalImpulse = tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
resolveSingleFrictionCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info,appliedNormalImpulse);
break;
}
default:
{
//undefined constraint type?
btAssert(0);
}
};
}
}
}
}
for (int i=0;i<tmpSolverBodyPool.size();i++)
{
tmpSolverBodyPool[i].writebackVelocity();
}
END_PROFILE("solveConstraints");
tmpSolverBodyPool.resize(0);
tmpSolverConstraintPool.resize(0);
tmpSolverFrictionConstraintPool.resize(0);
return 0.f;
}
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
{
//return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
BEGIN_PROFILE("prepareConstraints");
btContactSolverInfo info = infoGlobal;
@@ -160,11 +735,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold**
}
}
END_PROFILE("prepareConstraints");
BEGIN_PROFILE("solveConstraints");
//should traverse the contacts random order...
int iteration;
{
for ( iteration = 0;iteration<numiter-1;iteration++)
for ( iteration = 0;iteration<numiter;iteration++)
{
int j;
if (m_solverMode & SOLVER_RANDMIZE_ORDER)
@@ -202,22 +782,23 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold**
}
}
END_PROFILE("solveConstraints");
#ifdef USE_PROFILE
btProfiler::endBlock("solve");
#endif //USE_PROFILE
return btScalar(0.);
}
btScalar penetrationResolveFactor = btScalar(0.9);
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
{
btScalar rest = restitution * -rel_vel;
return rest;
}
void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
@@ -304,10 +885,10 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
btScalar combinedRestitution = cp.m_combinedRestitution;
cpd->m_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)
{

7
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -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)
{

71
src/BulletDynamics/ConstraintSolver/btsolverbody.h Normal file
View File

@@ -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

63
src/BulletDynamics/ConstraintSolver/btsolverconstraint.h Normal file
View File

@@ -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

18
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -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<btTypedConstraint*> 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");
}

4
src/BulletDynamics/Dynamics/btRigidBody.cpp
View File

@@ -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)
{

12
src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
View File

@@ -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<m_collisionObjects.size();i++)
for ( int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);
@@ -118,7 +118,7 @@ void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body)
void btSimpleDynamicsWorld::updateAabbs()
{
btTransform predictedTrans;
for (unsigned int i=0;i<m_collisionObjects.size();i++)
for ( int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);
@@ -138,7 +138,7 @@ void btSimpleDynamicsWorld::updateAabbs()
void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
{
btTransform predictedTrans;
for (unsigned int i=0;i<m_collisionObjects.size();i++)
for ( int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);
@@ -157,7 +157,7 @@ void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep)
void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
{
for (unsigned int i=0;i<m_collisionObjects.size();i++)
for ( int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);
@@ -180,7 +180,7 @@ void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
void btSimpleDynamicsWorld::synchronizeMotionStates()
{
//todo: iterate over awake simulation islands!
for (unsigned int i=0;i<m_collisionObjects.size();i++)
for ( int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);

11
src/LinearMath/btAlignedObjectArray.h
View File

@@ -136,6 +136,17 @@ class btAlignedObjectArray
}
SIMD_FORCE_INLINE T& expand()
{
int sz = size();
if( sz == capacity() )
{
reserve( allocSize(size()) );
}
m_size++;
return m_data[sz];
}
SIMD_FORCE_INLINE void push_back(const T& _Val)
{

2
src/LinearMath/btQuickprof.cpp
View File

@@ -27,7 +27,7 @@ subject to the following restrictions:
// Note: We must declare these private static variables again here to
// avoid link errors.
bool btProfiler::mEnabled = false;
hidden::Clock btProfiler::mClock;
btClock btProfiler::mClock;
unsigned long int btProfiler::mCurrentCycleStartMicroseconds = 0;
unsigned long int btProfiler::mLastCycleDurationMicroseconds = 0;
std::map<std::string, hidden::ProfileBlock*> btProfiler::mProfileBlocks;

12
src/LinearMath/btQuickprof.h
View File

@@ -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 <iostream>
#include <iostream>
#include <fstream>
#include <string>
#include <map>
@@ -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;
}