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

Add the option for the btSimulationIslandManager to avoid splitting islands (for constraint solving)

Browse Source 
Move the convertContact inside constraint solver to its own method
This commit is contained in:
erwin.coumans
2009-02-13 02:34:46 +00:00
parent 26d7757135
commit d886c06fa5
10 changed files with 452 additions and 363 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -24,7 +24,8 @@ subject to the following restrictions:
//#include <stdio.h>
#include "LinearMath/btQuickprof.h"
btSimulationIslandManager::btSimulationIslandManager()
btSimulationIslandManager::btSimulationIslandManager():
m_splitIslands(true)
{
}
@@ -251,11 +252,11 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
int i;
int maxNumManifolds = dispatcher->getNumManifolds();
#define SPLIT_ISLANDS 1
#ifdef SPLIT_ISLANDS
//#define SPLIT_ISLANDS 1
//#ifdef SPLIT_ISLANDS
#endif //SPLIT_ISLANDS
//#endif //SPLIT_ISLANDS
for (i=0;i<maxNumManifolds ;i++)
@@ -279,11 +280,12 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
{
colObj0->activate();
}
#ifdef SPLIT_ISLANDS
// //filtering for response
if (dispatcher->needsResponse(colObj0,colObj1))
m_islandmanifold.push_back(manifold);
#endif //SPLIT_ISLANDS
if(m_splitIslands)
{
//filtering for response
if (dispatcher->needsResponse(colObj0,colObj1))
m_islandmanifold.push_back(manifold);
}
}
}
}
@@ -303,84 +305,86 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
BT_PROFILE("processIslands");
#ifndef SPLIT_ISLANDS
btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
#else
// Sort manifolds, based on islands
// Sort the vector using predicate and std::sort
//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
int numManifolds = int (m_islandmanifold.size());
//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
//now process all active islands (sets of manifolds for now)
int startManifoldIndex = 0;
int endManifoldIndex = 1;
//int islandId;
// printf("Start Islands\n");
//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
if(!m_splitIslands)
{
int islandId = getUnionFind().getElement(startIslandIndex).m_id;
btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
int maxNumManifolds = dispatcher->getNumManifolds();
callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
}
else
{
// Sort manifolds, based on islands
// Sort the vector using predicate and std::sort
//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
int numManifolds = int (m_islandmanifold.size());
bool islandSleeping = false;
for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
{
int i = getUnionFind().getElement(endIslandIndex).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
m_islandBodies.push_back(colObj0);
if (!colObj0->isActive())
islandSleeping = true;
}
//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
//find the accompanying contact manifold for this islandId
int numIslandManifolds = 0;
btPersistentManifold** startManifold = 0;
//now process all active islands (sets of manifolds for now)
if (startManifoldIndex<numManifolds)
int startManifoldIndex = 0;
int endManifoldIndex = 1;
//int islandId;
// printf("Start Islands\n");
//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
{
int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
if (curIslandId == islandId)
{
startManifold = &m_islandmanifold[startManifoldIndex];
for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
{
int islandId = getUnionFind().getElement(startIslandIndex).m_id;
bool islandSleeping = false;
for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
{
int i = getUnionFind().getElement(endIslandIndex).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
m_islandBodies.push_back(colObj0);
if (!colObj0->isActive())
islandSleeping = true;
}
//find the accompanying contact manifold for this islandId
int numIslandManifolds = 0;
btPersistentManifold** startManifold = 0;
if (startManifoldIndex<numManifolds)
{
int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
if (curIslandId == islandId)
{
startManifold = &m_islandmanifold[startManifoldIndex];
for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
{
}
/// Process the actual simulation, only if not sleeping/deactivated
numIslandManifolds = endManifoldIndex-startManifoldIndex;
}
/// Process the actual simulation, only if not sleeping/deactivated
numIslandManifolds = endManifoldIndex-startManifoldIndex;
}
}
if (!islandSleeping)
{
callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
// printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
}
if (numIslandManifolds)
{
startManifoldIndex = endManifoldIndex;
}
if (!islandSleeping)
{
callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
// printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
m_islandBodies.resize(0);
}
if (numIslandManifolds)
{
startManifoldIndex = endManifoldIndex;
}
m_islandBodies.resize(0);
}
#endif //SPLIT_ISLANDS
} // else if(!splitIslands)
}

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

@@ -35,6 +35,7 @@ class btSimulationIslandManager
btAlignedObjectArray<btPersistentManifold*> m_islandmanifold;
btAlignedObjectArray<btCollisionObject* > m_islandBodies;
bool m_splitIslands;
public:
btSimulationIslandManager();
@@ -65,6 +66,15 @@ public:
void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
bool getSplitIslands()
{
return m_splitIslands;
}
void setSplitIslands(bool doSplitIslands)
{
m_splitIslands = doSplitIslands;
}
};
#endif //SIMULATION_ISLAND_MANAGER_H

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

@@ -120,7 +120,6 @@ SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstra
body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
}
SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
#ifdef USE_SIMD
@@ -269,7 +268,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
solverConstraint.m_appliedImpulse = 0.f;
// solverConstraint.m_appliedPushImpulse = 0.f;
solverConstraint.m_penetration = 0.f;
{
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
@@ -324,10 +323,8 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
rel_vel = vel1Dotn+vel2Dotn;
btScalar positionalError = 0.f;
positionalError = 0;//-solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
solverConstraint.m_restitution=0.f;
btSimdScalar velocityError = solverConstraint.m_restitution - rel_vel;
btSimdScalar velocityError = - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_cfm = 0.f;
@@ -364,6 +361,249 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
}
#include <stdio.h>
void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
{
btCollisionObject* colObj0=0,*colObj1=0;
colObj0 = (btCollisionObject*)manifold->getBody0();
colObj1 = (btCollisionObject*)manifold->getBody1();
int solverBodyIdA=-1;
int solverBodyIdB=-1;
if (manifold->getNumContacts())
{
solverBodyIdA = getOrInitSolverBody(*colObj0);
solverBodyIdB = getOrInitSolverBody(*colObj1);
}
btVector3 rel_pos1;
btVector3 rel_pos2;
btScalar relaxation;
for (int j=0;j<manifold->getNumContacts();j++)
{
btManifoldPoint& cp = manifold->getContactPoint(j);
///this is a bad test and results in jitter -> always solve for those zero-distanc contacts!
///-> if (cp.getDistance() <= btScalar(0.))
//if (cp.getDistance() <= manifold->getContactBreakingThreshold())
{
const btVector3& pos1 = cp.getPositionWorldOnA();
const btVector3& pos2 = cp.getPositionWorldOnB();
rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
relaxation = 1.f;
btScalar rel_vel;
btVector3 vel;
int frictionIndex = m_tmpSolverContactConstraintPool.size();
{
btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
btRigidBody* rb0 = btRigidBody::upcast(colObj0);
btRigidBody* rb1 = btRigidBody::upcast(colObj1);
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_originalContactPoint = &cp;
btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1 : btVector3(0,0,0);
{
#ifdef COMPUTE_IMPULSE_DENOM
btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
#else
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
if (rb0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
}
if (rb1)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
}
#endif //COMPUTE_IMPULSE_DENOM
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 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
vel = vel1 - vel2;
rel_vel = cp.m_normalWorldOnB.dot(vel);
btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
solverConstraint.m_friction = cp.m_combinedFriction;
btScalar restitution = 0.f;
if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
{
restitution = 0.f;
} else
{
restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (restitution <= btScalar(0.))
{
restitution = 0.f;
};
}
///warm starting (or zero if disabled)
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (rb0)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
if (rb1)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
} else
{
solverConstraint.m_appliedImpulse = 0.f;
}
// solverConstraint.m_appliedPushImpulse = 0.f;
{
btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0))
+ solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
rel_vel = vel1Dotn+vel2Dotn;
btScalar positionalError = 0.f;
positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
btScalar velocityError = restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
/////setup the friction constraints
if (1)
{
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
{
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
{
cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
cp.m_lateralFrictionDir2.normalize();//??
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
cp.m_lateralFrictionInitialized = true;
} else
{
//re-calculate friction direction every frame, todo: check if this is really needed
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
cp.m_lateralFrictionInitialized = true;
}
} else
{
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
{
{
btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
if (rb0)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
if (rb1)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
} else
{
frictionConstraint1.m_appliedImpulse = 0.f;
}
}
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
if (rb0)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
if (rb1)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
} else
{
frictionConstraint2.m_appliedImpulse = 0.f;
}
}
} else
{
btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
frictionConstraint1.m_appliedImpulse = 0.f;
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
frictionConstraint2.m_appliedImpulse = 0.f;
}
}
}
}
}
}
}
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
{
BT_PROFILE("solveGroupCacheFriendlySetup");
@@ -443,7 +683,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
currentConstraintRow[j].m_upperLimit = FLT_MAX;
currentConstraintRow[j].m_appliedImpulse = 0.f;
currentConstraintRow[j].m_penetration = 0.f;
currentConstraintRow[j].m_appliedPushImpulse = 0.f;
currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
@@ -510,9 +749,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
rel_vel = vel1Dotn+vel2Dotn;
btScalar restitution = 0.f;
btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
solverConstraint.m_restitution = 0.f;
btScalar velocityError = solverConstraint.m_restitution - rel_vel;// * damping;
btScalar velocityError = restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
@@ -533,258 +772,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
for (i=0;i<numManifolds;i++)
{
manifold = manifoldPtr[i];
colObj0 = (btCollisionObject*)manifold->getBody0();
colObj1 = (btCollisionObject*)manifold->getBody1();
int solverBodyIdA=-1;
int solverBodyIdB=-1;
if (manifold->getNumContacts())
{
solverBodyIdA = getOrInitSolverBody(*colObj0);
solverBodyIdB = getOrInitSolverBody(*colObj1);
}
if (solverBodyIdA == 0 && solverBodyIdB == 0)
continue;
btVector3 rel_pos1;
btVector3 rel_pos2;
btScalar relaxation;
for (int j=0;j<manifold->getNumContacts();j++)
{
btManifoldPoint& cp = manifold->getContactPoint(j);
///this is a bad test and results in jitter -> always solve for those zero-distanc contacts!
///-> if (cp.getDistance() <= btScalar(0.))
//if (cp.getDistance() <= manifold->getContactBreakingThreshold())
{
const btVector3& pos1 = cp.getPositionWorldOnA();
const btVector3& pos2 = cp.getPositionWorldOnB();
rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
relaxation = 1.f;
btScalar rel_vel;
btVector3 vel;
int frictionIndex = m_tmpSolverContactConstraintPool.size();
{
btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
btRigidBody* rb0 = btRigidBody::upcast(colObj0);
btRigidBody* rb1 = btRigidBody::upcast(colObj1);
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_originalContactPoint = &cp;
btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1 : btVector3(0,0,0);
{
#ifdef COMPUTE_IMPULSE_DENOM
btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
#else
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
if (rb0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
}
if (rb1)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
}
#endif //COMPUTE_IMPULSE_DENOM
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 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
vel = vel1 - vel2;
rel_vel = cp.m_normalWorldOnB.dot(vel);
solverConstraint.m_penetration = cp.getDistance()+infoGlobal.m_linearSlop;
//solverConstraint.m_penetration = cp.getDistance();
solverConstraint.m_friction = cp.m_combinedFriction;
if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
{
solverConstraint.m_restitution = 0.f;
} else
{
solverConstraint.m_restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (solverConstraint.m_restitution <= btScalar(0.))
{
solverConstraint.m_restitution = 0.f;
};
}
///warm starting (or zero if disabled)
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
if (rb0)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
if (rb1)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
} else
{
solverConstraint.m_appliedImpulse = 0.f;
}
// solverConstraint.m_appliedPushImpulse = 0.f;
{
btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0))
+ solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
rel_vel = vel1Dotn+vel2Dotn;
btScalar positionalError = 0.f;
positionalError = -solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
btScalar velocityError = solverConstraint.m_restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_cfm = 0.f;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
#ifdef _USE_JACOBIAN
solverConstraint.m_jac = btJacobianEntry (
rel_pos1,rel_pos2,cp.m_normalWorldOnB,
rb0->getInvInertiaDiagLocal(),
rb0->getInvMass(),
rb1->getInvInertiaDiagLocal(),
rb1->getInvMass());
#endif //_USE_JACOBIAN
/////setup the friction constraints
if (1)
{
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
{
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
{
cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
cp.m_lateralFrictionDir2.normalize();//??
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
cp.m_lateralFrictionInitialized = true;
} else
{
//re-calculate friction direction every frame, todo: check if this is really needed
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
cp.m_lateralFrictionInitialized = true;
}
} else
{
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
{
{
btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
if (rb0)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
if (rb1)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
} else
{
frictionConstraint1.m_appliedImpulse = 0.f;
}
}
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
{
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
if (rb0)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
if (rb1)
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
} else
{
frictionConstraint2.m_appliedImpulse = 0.f;
}
}
} else
{
btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
frictionConstraint1.m_appliedImpulse = 0.f;
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
frictionConstraint2.m_appliedImpulse = 0.f;
}
}
}
}
}
}
convertContact(manifold,infoGlobal);
}
}
}

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

@@ -30,6 +30,7 @@ class btIDebugDraw;
///Applies impulses for combined restitution and penetration recovery and to simulate friction
class btSequentialImpulseConstraintSolver : public btConstraintSolver
{
protected:
btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
btConstraintArray m_tmpSolverContactConstraintPool;
@@ -38,7 +39,6 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver
btAlignedObjectArray<int> m_orderTmpConstraintPool;
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
protected:
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
@@ -47,6 +47,8 @@ protected:
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
void resolveSplitPenetrationImpulseCacheFriendly(
btSolverBody& body1,
btSolverBody& body2,

8
src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
View File

@@ -44,10 +44,12 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
btScalar m_friction;
btScalar m_restitution;
btScalar m_jacDiagABInv;
btScalar m_penetration;
union
{
int m_numConsecutiveRowsPerKernel;
btScalar m_unusedPadding0;
};
union
{

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

@@ -665,8 +665,11 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
{
if (islandId<0)
{
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
if (numManifolds + m_numConstraints)
{
///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
}
} else
{
//also add all non-contact constraints/joints for this island

16
src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp
View File

@@ -1238,7 +1238,7 @@ void processSolverTask(void* userPtr, void* lsMemory)
vel = vel1 - vel2;
rel_vel = cp.m_normalWorldOnB.dot(vel);
constraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations);
btScalar penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations);
constraint.m_friction = cp.m_combinedFriction;
float rest = - rel_vel * cp.m_combinedRestitution;
if (rest <= btScalar(0.))
@@ -1251,7 +1251,7 @@ void processSolverTask(void* userPtr, void* lsMemory)
btScalar erp = taskDesc.m_commandData.m_manifoldSetup.m_solverInfo.m_erp;
btScalar timeStep = taskDesc.m_commandData.m_manifoldSetup.m_solverInfo.m_timeStep;
constraint.m_restitution = rest;
btScalar restitution = rest;
constraint.m_appliedImpulse = cp.m_appliedImpulse*taskDesc.m_commandData.m_manifoldSetup.m_solverInfo.m_warmstartingFactor;
if (constraint.m_appliedImpulse!= 0.f)
{
@@ -1271,8 +1271,8 @@ void processSolverTask(void* userPtr, void* lsMemory)
rel_vel = vel1Dotn-vel2Dotn;
btScalar positionalError = 0.f;
positionalError = -constraint.m_penetration * erp/timeStep;
btScalar velocityError = constraint.m_restitution - rel_vel;// * damping;
positionalError = -penetration * erp/timeStep;
btScalar velocityError = restitution - rel_vel;// * damping;
btScalar penetrationImpulse = positionalError*constraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *constraint.m_jacDiagABInv;
constraint.m_rhs = penetrationImpulse+velocityImpulse;
@@ -1339,9 +1339,9 @@ void processSolverTask(void* userPtr, void* lsMemory)
btScalar positionalError = 0.f;
positionalError = 0;
constraint.m_restitution=0.f;
btScalar restitution=0.f;
btSimdScalar velocityError = constraint.m_restitution - rel_vel;
btSimdScalar velocityError = restitution - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(constraint.m_jacDiagABInv);
constraint.m_rhs = velocityImpulse;
constraint.m_cfm = 0.f;
@@ -1386,9 +1386,9 @@ void processSolverTask(void* userPtr, void* lsMemory)
btScalar positionalError = 0.f;
positionalError = 0;
constraint.m_restitution=0.f;
btScalar restitution=0.f;
btSimdScalar velocityError = constraint.m_restitution - rel_vel;
btSimdScalar velocityError = restitution - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(constraint.m_jacDiagABInv);
constraint.m_rhs = velocityImpulse;
constraint.m_cfm = 0.f;