Based on feedback from another professional game company, there are several improvements, including some API change...
Some dynamic memory allocations have been replace by pool allocation or stack allocations. quantized aabb versus quantized aabb overlap check is made branch-free (helps a lot on consoles PS3/XBox 360) Collision algorithms are now created through a new btDefaultCollisionConfiguration, to decouple dependency (this is the API change): Example: btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); m_dispatcher = new btCollisionDispatcher(collisionConfiguration);
This commit is contained in:
ejcoumans
@@ -29,7 +29,7 @@ subject to the following restrictions:
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
#include "btSolverBody.h"
|
||||
#include "btSolverConstraint.h"
|
||||
|
||||
#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
#ifdef USE_PROFILE
|
||||
@@ -138,6 +138,7 @@ void initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody)
|
||||
solverBody->m_invMass = rigidbody->getInvMass();
|
||||
solverBody->m_linearVelocity = rigidbody->getLinearVelocity();
|
||||
solverBody->m_originalBody = rigidbody;
|
||||
btAssert(rigidbody);
|
||||
solverBody->m_angularFactor = rigidbody->getAngularFactor();
|
||||
}
|
||||
|
||||
@@ -333,12 +334,10 @@ btScalar resolveSingleFrictionCacheFriendly(
|
||||
|
||||
#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)
|
||||
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher)
|
||||
{
|
||||
(void)stackAlloc;
|
||||
(void)debugDrawer;
|
||||
@@ -351,15 +350,36 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
|
||||
|
||||
BEGIN_PROFILE("refreshManifolds");
|
||||
|
||||
int numActiveBodies = 0;
|
||||
|
||||
for (int i=0;i<numBodies;i++)
|
||||
{
|
||||
btRigidBody* rb = btRigidBody::upcast(bodies[i]);
|
||||
if (rb && (rb->getIslandTag() >= 0))
|
||||
{
|
||||
numActiveBodies++;
|
||||
}
|
||||
}
|
||||
|
||||
int numActiveManifolds = 0;
|
||||
int totalContacts = 0;
|
||||
|
||||
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());
|
||||
|
||||
if (((rb0) && rb0->getActivationState() != ISLAND_SLEEPING) ||
|
||||
((rb1) && rb1->getActivationState() != ISLAND_SLEEPING))
|
||||
{
|
||||
if (dispatcher->needsResponse(rb0,rb1))
|
||||
{
|
||||
manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
|
||||
totalContacts += manifold->getNumContacts();
|
||||
numActiveManifolds++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
END_PROFILE("refreshManifolds");
|
||||
@@ -367,24 +387,33 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
|
||||
|
||||
BEGIN_PROFILE("gatherSolverData");
|
||||
|
||||
btBlock* sablock;
|
||||
sablock = stackAlloc->beginBlock();
|
||||
|
||||
|
||||
int tmpSolverBodyPoolSize = 0;
|
||||
int mem1 = sizeof(btSolverBody) * numActiveBodies;
|
||||
btSolverBody* tmpSolverBodyPool = (btSolverBody*) stackAlloc->allocate(sizeof(btSolverBody) * numActiveBodies*2);
|
||||
int tmpSolverConstraintPoolSize = 0;
|
||||
int mem2 = sizeof(btSolverConstraint)*numActiveManifolds;
|
||||
|
||||
btSolverConstraint* tmpSolverConstraintPool = (btSolverConstraint*) stackAlloc->allocate(sizeof(btSolverConstraint)*totalContacts);
|
||||
|
||||
int tmpSolverFrictionConstraintPoolSize = 0;
|
||||
btSolverConstraint* tmpSolverFrictionConstraintPool = (btSolverConstraint*) stackAlloc->allocate(sizeof(btSolverConstraint)*totalContacts*2);
|
||||
|
||||
//int sizeofSB = sizeof(btSolverBody);
|
||||
//int sizeofSC = sizeof(btSolverConstraint);
|
||||
|
||||
|
||||
//if (1)
|
||||
{
|
||||
//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);
|
||||
|
||||
|
||||
//todo: use stack allocator for this temp memory
|
||||
int minReservation = numManifolds*2;
|
||||
|
||||
tmpSolverBodyPool.reserve(minReservation);
|
||||
|
||||
{
|
||||
for (int i=0;i<numBodies;i++)
|
||||
@@ -393,8 +422,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
|
||||
if (rb && (rb->getIslandTag() >= 0))
|
||||
{
|
||||
btAssert(rb->getCompanionId() < 0);
|
||||
int solverBodyId = tmpSolverBodyPool.size();
|
||||
btSolverBody& solverBody = tmpSolverBodyPool.expand();
|
||||
int solverBodyId = tmpSolverBodyPoolSize;
|
||||
btSolverBody& solverBody = tmpSolverBodyPool[tmpSolverBodyPoolSize++];
|
||||
initSolverBody(&solverBody,rb);
|
||||
rb->setCompanionId(solverBodyId);
|
||||
}
|
||||
@@ -402,8 +431,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
|
||||
}
|
||||
|
||||
|
||||
tmpSolverConstraintPool.reserve(minReservation);
|
||||
tmpSolverFrictionConstraintPool.reserve(minReservation);
|
||||
{
|
||||
int i;
|
||||
|
||||
@@ -413,187 +440,196 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
|
||||
btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
|
||||
btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
|
||||
|
||||
|
||||
int solverBodyIdA=-1;
|
||||
int solverBodyIdB=-1;
|
||||
|
||||
if (manifold->getNumContacts())
|
||||
if (((rb0) && rb0->getActivationState() != ISLAND_SLEEPING) ||
|
||||
((rb1) && rb1->getActivationState() != ISLAND_SLEEPING))
|
||||
{
|
||||
if (dispatcher->needsResponse(rb0,rb1))
|
||||
{
|
||||
|
||||
|
||||
|
||||
if (rb0->getIslandTag() >= 0)
|
||||
{
|
||||
solverBodyIdA = rb0->getCompanionId();
|
||||
} else
|
||||
{
|
||||
//create a static body
|
||||
solverBodyIdA = tmpSolverBodyPool.size();
|
||||
btSolverBody& solverBody = tmpSolverBodyPool.expand();
|
||||
initSolverBody(&solverBody,rb0);
|
||||
}
|
||||
int solverBodyIdA=-1;
|
||||
int solverBodyIdB=-1;
|
||||
|
||||
if (rb1->getIslandTag() >= 0)
|
||||
{
|
||||
solverBodyIdB = rb1->getCompanionId();
|
||||
} else
|
||||
{
|
||||
//create a static body
|
||||
solverBodyIdB = tmpSolverBodyPool.size();
|
||||
btSolverBody& solverBody = tmpSolverBodyPool.expand();
|
||||
initSolverBody(&solverBody,rb1);
|
||||
if (manifold->getNumContacts())
|
||||
{
|
||||
|
||||
|
||||
|
||||
if (rb0->getIslandTag() >= 0)
|
||||
{
|
||||
solverBodyIdA = rb0->getCompanionId();
|
||||
} else
|
||||
{
|
||||
//create a static body
|
||||
solverBodyIdA = tmpSolverBodyPoolSize;
|
||||
btSolverBody& solverBody = tmpSolverBodyPool[tmpSolverBodyPoolSize++];
|
||||
initSolverBody(&solverBody,rb0);
|
||||
}
|
||||
|
||||
if (rb1->getIslandTag() >= 0)
|
||||
{
|
||||
solverBodyIdB = rb1->getCompanionId();
|
||||
} else
|
||||
{
|
||||
//create a static body
|
||||
solverBodyIdB = tmpSolverBodyPoolSize;
|
||||
btSolverBody& solverBody = tmpSolverBodyPool[tmpSolverBodyPoolSize++];
|
||||
initSolverBody(&solverBody,rb1);
|
||||
}
|
||||
}
|
||||
|
||||
for (int j=0;j<manifold->getNumContacts();j++)
|
||||
{
|
||||
|
||||
btManifoldPoint& cp = manifold->getContactPoint(j);
|
||||
|
||||
int frictionIndex = tmpSolverConstraintPoolSize;
|
||||
|
||||
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[tmpSolverConstraintPoolSize++];
|
||||
|
||||
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[tmpSolverFrictionConstraintPoolSize++];
|
||||
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[tmpSolverFrictionConstraintPoolSize++];
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -612,15 +648,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
|
||||
}
|
||||
}
|
||||
|
||||
btAlignedObjectArray<int> gOrderTmpConstraintPool;
|
||||
btAlignedObjectArray<int> gOrderFrictionConstraintPool;
|
||||
int numConstraintPool = tmpSolverConstraintPoolSize;
|
||||
int numFrictionPool = tmpSolverFrictionConstraintPoolSize;
|
||||
|
||||
int numConstraintPool = tmpSolverConstraintPool.size();
|
||||
int numFrictionPool = tmpSolverFrictionConstraintPool.size();
|
||||
///use the stack allocator for temporarily memory
|
||||
|
||||
int gOrderTmpConstraintPoolSize = numConstraintPool;
|
||||
int* gOrderTmpConstraintPool = (int*) stackAlloc->allocate(sizeof(int)*numConstraintPool);
|
||||
int gOrderFrictionConstraintPoolSize = numFrictionPool;
|
||||
int* gOrderFrictionConstraintPool = (int*) stackAlloc->allocate(sizeof(int)*numFrictionPool);
|
||||
|
||||
///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
|
||||
gOrderTmpConstraintPool.resize(numConstraintPool);
|
||||
gOrderFrictionConstraintPool.resize(numFrictionPool);
|
||||
{
|
||||
int i;
|
||||
for (i=0;i<numConstraintPool;i++)
|
||||
@@ -695,7 +732,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
|
||||
}
|
||||
|
||||
{
|
||||
int numPoolConstraints = tmpSolverConstraintPool.size();
|
||||
int numPoolConstraints = tmpSolverConstraintPoolSize;
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
{
|
||||
btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
|
||||
@@ -705,7 +742,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
|
||||
}
|
||||
|
||||
{
|
||||
int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size();
|
||||
int numFrictionPoolConstraints = tmpSolverFrictionConstraintPoolSize;
|
||||
for (j=0;j<numFrictionPoolConstraints;j++)
|
||||
{
|
||||
btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
|
||||
@@ -721,36 +758,22 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
|
||||
}
|
||||
}
|
||||
|
||||
for ( i=0;i<tmpSolverBodyPool.size();i++)
|
||||
for ( i=0;i<tmpSolverBodyPoolSize;i++)
|
||||
{
|
||||
tmpSolverBodyPool[i].writebackVelocity();
|
||||
}
|
||||
|
||||
END_PROFILE("solveConstraints");
|
||||
|
||||
// printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size());
|
||||
|
||||
/*
|
||||
printf("tmpSolverBodyPool.size() = %i\n",tmpSolverBodyPool.size());
|
||||
printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size());
|
||||
printf("tmpSolverFrictionConstraintPool.size() = %i\n",tmpSolverFrictionConstraintPool.size());
|
||||
|
||||
|
||||
printf("tmpSolverBodyPool.capacity() = %i\n",tmpSolverBodyPool.capacity());
|
||||
printf("tmpSolverConstraintPool.capacity() = %i\n",tmpSolverConstraintPool.capacity());
|
||||
printf("tmpSolverFrictionConstraintPool.capacity() = %i\n",tmpSolverFrictionConstraintPool.capacity());
|
||||
*/
|
||||
|
||||
tmpSolverBodyPool.resize(0);
|
||||
tmpSolverConstraintPool.resize(0);
|
||||
tmpSolverFrictionConstraintPool.resize(0);
|
||||
///release stack memory
|
||||
|
||||
stackAlloc->endBlock(sablock);
|
||||
|
||||
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)
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher)
|
||||
{
|
||||
|
||||
if (getSolverMode() & SOLVER_CACHE_FRIENDLY)
|
||||
@@ -759,7 +782,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
|
||||
//btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
|
||||
btAssert(bodies);
|
||||
btAssert(numBodies);
|
||||
return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
|
||||
return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc,dispatcher);
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user