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

98
Demos/BasicDemo/BasicDemo.cpp
View File

@@ -13,17 +13,27 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#define LARGE_DEMO 0
#if LARGE_DEMO
///create 512 (8x8x8) dynamic object
#define ARRAY_SIZE_X 8
#define ARRAY_SIZE_Y 8
#define ARRAY_SIZE_Z 8
#else
///create 125 (5x5x5) dynamic object
#define ARRAY_SIZE_X 5
#define ARRAY_SIZE_Y 5
#define ARRAY_SIZE_Z 5
#endif
///create 125 (5x5x5) dynamic object
#define ARRAY_SIZE_X 5
#define ARRAY_SIZE_Y 5
#define ARRAY_SIZE_Z 5
//maximum number of objects (and allow user to shoot additional boxes)
#define MAX_PROXIES (ARRAY_SIZE_X*ARRAY_SIZE_Y*ARRAY_SIZE_Z + 1024)
///scaling of the objects (0.1 = 20 centimeter boxes )
#define SCALING 0.1
//#define SCALING 0.1
#define SCALING 1
#define START_POS_X -5
#define START_POS_Y -5
#define START_POS_Z -3
@@ -34,6 +44,9 @@ subject to the following restrictions:
#include "btBulletDynamicsCommon.h"
#include <stdio.h> //printf debugging
#include "BulletDynamics/ConstraintSolver/btParallelBatchConstraintSolver.h"
#include "BulletDynamics/ConstraintSolver/btParallelBatchConstraintSolver2.h"
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
void BasicDemo::clientMoveAndDisplay()
{
@@ -46,6 +59,10 @@ void BasicDemo::clientMoveAndDisplay()
if (m_dynamicsWorld)
{
m_dynamicsWorld->stepSimulation(ms / 1000000.f);
if (m_idle)
{
CProfileManager::dumpAll();
}
//optional but useful: debug drawing
m_dynamicsWorld->debugDrawWorld();
}
@@ -75,6 +92,10 @@ void BasicDemo::displayCallback(void) {
}
static btSequentialImpulseConstraintSolver* sDefSeqImpSolver = 0;
static btParallelBatchConstraintSolver* sParallelBatchSolver = 0;
static btParallelBatchConstraintSolver2* sParallelBatchSolver2 = 0;
@@ -94,22 +115,31 @@ void BasicDemo::initPhysics()
m_broadphase = new btDbvtBroadphase();
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
m_solver = sol;
sDefSeqImpSolver = new btSequentialImpulseConstraintSolver;
// create parallel batch solver for tests
sParallelBatchSolver = new btParallelBatchConstraintSolver();
sParallelBatchSolver2 = new btParallelBatchConstraintSolver2();
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
// start with parallel batch solver
m_solver = sParallelBatchSolver;
btDiscreteDynamicsWorld* ddw = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
m_dynamicsWorld = ddw;
ddw->getSimulationIslandManager()->setSplitIslands(false);
m_dynamicsWorld->setGravity(btVector3(0,-10,0));
///create a few basic rigid bodies
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(30.),btScalar(1.),btScalar(30.)));
// btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0,-50,0));
groundTransform.setOrigin(btVector3(0,-5,0));
// groundTransform.setOrigin(btVector3(0,-50,0));
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
{
@@ -215,7 +245,9 @@ void BasicDemo::exitPhysics()
delete m_dynamicsWorld;
delete m_solver;
m_solver = 0;
delete sDefSeqImpSolver;
delete sParallelBatchSolver;
delete m_broadphase;
@@ -228,4 +260,48 @@ void BasicDemo::exitPhysics()
void BasicDemo::keyboardCallback(unsigned char key, int x, int y)
{
(void)x;
(void)y;
switch (key)
{
case 'q' :
exitPhysics();
break;
case 'S' :
{
btConstraintSolver* curr_solver = m_dynamicsWorld->getConstraintSolver();
btDiscreteDynamicsWorld* pDdw = (btDiscreteDynamicsWorld*)m_dynamicsWorld;
if(curr_solver == sDefSeqImpSolver)
{
static bool toggle = true;
toggle=!toggle;
if (toggle)
{
pDdw->setConstraintSolver(sParallelBatchSolver);
pDdw->getSimulationIslandManager()->setSplitIslands(false);
printf("\nUsing ParallelBatch constraint solver\n");
} else
{
pDdw->setConstraintSolver(sParallelBatchSolver2);
pDdw->getSimulationIslandManager()->setSplitIslands(false);
printf("\nUsing ParallelBatch constraint solver2\n");
}
}
else
{
m_dynamicsWorld->setConstraintSolver(sDefSeqImpSolver);
pDdw->getSimulationIslandManager()->setSplitIslands(true);
printf("\nUsing default SequentialImpulse constraint solver\n");
}
}
break;
default :
break;
}
DemoApplication::keyboardCallback(key, x, y);
}

2
Demos/BasicDemo/BasicDemo.h
View File

@@ -58,6 +58,8 @@ class BasicDemo : public DemoApplication
virtual void displayCallback();
virtual void keyboardCallback(unsigned char key, int x, int y);
static DemoApplication* Create()
{
BasicDemo* demo = new BasicDemo;

6
Demos/OpenGL/DemoApplication.cpp
View File

@@ -1269,6 +1269,7 @@ void DemoApplication::clientResetScene()
numObjects = m_dynamicsWorld->getNumCollisionObjects();
}
///create a copy of the array, not a reference!
btCollisionObjectArray copyArray = m_dynamicsWorld->getCollisionObjectArray();
for (i=0;i<copyArray.size();i++)
@@ -1297,8 +1298,8 @@ void DemoApplication::clientResetScene()
colObj->setDeactivationTime(0);
//colObj->setActivationState(WANTS_DEACTIVATION);
}
//removed cached contact points
//m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
//removed cached contact points (this is not necessary if all objects have been removed from the dynamics world)
m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
btRigidBody* body = btRigidBody::upcast(colObj);
if (body && !body->isStaticObject())
@@ -1310,6 +1311,7 @@ void DemoApplication::clientResetScene()
}
///reset some internal cached data in the broadphase
m_dynamicsWorld->getBroadphase()->resetPool(getDynamicsWorld()->getDispatcher());
m_dynamicsWorld->getConstraintSolver()->reset();

154
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());
//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;
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;
}
// printf("Start Islands\n");
//find the accompanying contact manifold for this islandId
int numIslandManifolds = 0;
btPersistentManifold** startManifold = 0;
if (startManifoldIndex<numManifolds)
//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)
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)
{
startManifold = &m_islandmanifold[startManifoldIndex];
for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
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;
}
m_islandBodies.resize(0);
}
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;
}
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;