mirror of
https://github.com/bulletphysics/bullet3.git
synced 2026-09-24 03:04:50 +00:00
prepare and added constraint solver optimizations, not activated yet.
This commit is contained in:
ejcoumans
@@ -25,6 +25,12 @@ subject to the following restrictions:
|
||||
#include "LinearMath/btMinMax.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
||||
#include <new>
|
||||
#include "LinearMath/btStackAlloc.h"
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
#include "btSolverBody.h"
|
||||
#include "btSolverConstraint.h"
|
||||
|
||||
#include "LinearMath/btAlignedObjectArray.h"
|
||||
|
||||
#ifdef USE_PROFILE
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
@@ -105,7 +111,7 @@ bool MyContactDestroyedCallback(void* userPersistentData)
|
||||
|
||||
|
||||
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
|
||||
:m_solverMode(SOLVER_RANDMIZE_ORDER) //not using SOLVER_USE_WARMSTARTING
|
||||
:m_solverMode(SOLVER_RANDMIZE_ORDER)//SOLVER_USE_WARMSTARTING)//SOLVER_RANDMIZE_ORDER) //not using SOLVER_USE_WARMSTARTING
|
||||
{
|
||||
gContactDestroyedCallback = &MyContactDestroyedCallback;
|
||||
|
||||
@@ -120,10 +126,579 @@ btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
|
||||
}
|
||||
}
|
||||
|
||||
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
|
||||
|
||||
void initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody)
|
||||
{
|
||||
int size = sizeof(btSolverBody);
|
||||
int sizeofrb = sizeof(btRigidBody);
|
||||
int sizemanifold = sizeof(btPersistentManifold);
|
||||
int sizeofmp = sizeof(btManifoldPoint);
|
||||
int sizeofPersistData = sizeof (btConstraintPersistentData);
|
||||
|
||||
solverBody->m_angularVelocity = rigidbody->getAngularVelocity();
|
||||
solverBody->m_centerOfMassPosition = rigidbody->getCenterOfMassPosition();
|
||||
solverBody->m_friction = rigidbody->getFriction();
|
||||
solverBody->m_invInertiaWorld = rigidbody->getInvInertiaTensorWorld();
|
||||
solverBody->m_invMass = rigidbody->getInvMass();
|
||||
solverBody->m_linearVelocity = rigidbody->getLinearVelocity();
|
||||
solverBody->m_originalBody = rigidbody;
|
||||
}
|
||||
|
||||
btScalar penetrationResolveFactor = btScalar(0.9);
|
||||
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
|
||||
{
|
||||
btScalar rest = restitution * -rel_vel;
|
||||
return rest;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//velocity + friction
|
||||
//response between two dynamic objects with friction
|
||||
SIMD_FORCE_INLINE btScalar resolveSingleCollisionCombinedCacheFriendly(
|
||||
btSolverBody& body1,
|
||||
btSolverBody& body2,
|
||||
btSolverConstraint& contactConstraint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
btScalar normalImpulse(0.f);
|
||||
{
|
||||
if (contactConstraint.m_penetration < 0.f)
|
||||
return 0.f;
|
||||
|
||||
// Optimized version of projected relative velocity, use precomputed cross products with normal
|
||||
// body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
|
||||
// body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
|
||||
// btVector3 vel = vel1 - vel2;
|
||||
// btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel);
|
||||
|
||||
btScalar rel_vel;
|
||||
btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
|
||||
+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
|
||||
btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
|
||||
+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
|
||||
|
||||
rel_vel = vel1Dotn-vel2Dotn;
|
||||
|
||||
|
||||
btScalar positionalError = contactConstraint.m_penetration;
|
||||
btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
|
||||
|
||||
btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
|
||||
btScalar normalImpulse = penetrationImpulse+velocityImpulse;
|
||||
|
||||
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
|
||||
btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
|
||||
btScalar sum = oldNormalImpulse + normalImpulse;
|
||||
contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
|
||||
|
||||
btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse;
|
||||
btScalar velocitySum = oldVelocityImpulse + velocityImpulse;
|
||||
contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum;
|
||||
|
||||
normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
|
||||
|
||||
if (body1.m_invMass)
|
||||
{
|
||||
body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
|
||||
contactConstraint.m_angularComponentA,normalImpulse);
|
||||
}
|
||||
if (body2.m_invMass)
|
||||
{
|
||||
body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
|
||||
contactConstraint.m_angularComponentB,-normalImpulse);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
return normalImpulse;
|
||||
}
|
||||
|
||||
|
||||
#ifndef NO_FRICTION_TANGENTIALS
|
||||
|
||||
SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly(
|
||||
btSolverBody& body1,
|
||||
btSolverBody& body2,
|
||||
btSolverConstraint& contactConstraint,
|
||||
const btContactSolverInfo& solverInfo,
|
||||
btScalar appliedNormalImpulse)
|
||||
{
|
||||
|
||||
|
||||
btScalar combinedFriction = contactConstraint.m_friction;
|
||||
|
||||
btScalar limit = appliedNormalImpulse * combinedFriction;
|
||||
|
||||
if (appliedNormalImpulse>btScalar(0.))
|
||||
//friction
|
||||
{
|
||||
|
||||
btScalar j1;
|
||||
{
|
||||
|
||||
btScalar rel_vel;
|
||||
btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
|
||||
+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
|
||||
btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
|
||||
+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
|
||||
rel_vel = vel1Dotn-vel2Dotn;
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
|
||||
btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
|
||||
contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
|
||||
GEN_set_min(contactConstraint.m_appliedImpulse, limit);
|
||||
GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
|
||||
j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
|
||||
|
||||
}
|
||||
|
||||
if (body1.m_invMass)
|
||||
{
|
||||
body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
|
||||
}
|
||||
if (body2.m_invMass)
|
||||
{
|
||||
body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
|
||||
}
|
||||
|
||||
}
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
|
||||
#else
|
||||
|
||||
//velocity + friction
|
||||
//response between two dynamic objects with friction
|
||||
btScalar resolveSingleFrictionCacheFriendly(
|
||||
btSolverBody& body1,
|
||||
btSolverBody& body2,
|
||||
btSolverConstraint& contactConstraint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
|
||||
btVector3 vel1;
|
||||
btVector3 vel2;
|
||||
btScalar normalImpulse(0.f);
|
||||
|
||||
{
|
||||
const btVector3& normal = contactConstraint.m_contactNormal;
|
||||
if (contactConstraint.m_penetration < 0.f)
|
||||
return 0.f;
|
||||
|
||||
|
||||
body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
|
||||
body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
|
||||
btVector3 vel = vel1 - vel2;
|
||||
btScalar rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
btVector3 lat_vel = vel - normal * rel_vel;
|
||||
btScalar lat_rel_vel = lat_vel.length2();
|
||||
|
||||
btScalar combinedFriction = contactConstraint.m_friction;
|
||||
const btVector3& rel_pos1 = contactConstraint.m_rel_posA;
|
||||
const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
|
||||
|
||||
|
||||
//if (contactConstraint.m_appliedVelocityImpulse > 0.f)
|
||||
if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
|
||||
{
|
||||
lat_rel_vel = btSqrt(lat_rel_vel);
|
||||
|
||||
lat_vel /= lat_rel_vel;
|
||||
btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel);
|
||||
btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
|
||||
btScalar friction_impulse = lat_rel_vel /
|
||||
(body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
|
||||
btScalar normal_impulse = contactConstraint.m_appliedVelocityImpulse * combinedFriction;
|
||||
|
||||
GEN_set_min(friction_impulse, normal_impulse);
|
||||
GEN_set_max(friction_impulse, -normal_impulse);
|
||||
body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
|
||||
body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
|
||||
}
|
||||
}
|
||||
|
||||
return normalImpulse;
|
||||
}
|
||||
|
||||
#endif //NO_FRICTION_TANGENTIALS
|
||||
|
||||
btAlignedObjectArray<btSolverBody> tmpSolverBodyPool;
|
||||
btAlignedObjectArray<btSolverConstraint> tmpSolverConstraintPool;
|
||||
btAlignedObjectArray<btSolverConstraint> tmpSolverFrictionConstraintPool;
|
||||
|
||||
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
|
||||
{
|
||||
|
||||
if (!(numConstraints + numManifolds))
|
||||
return 0.f;
|
||||
|
||||
BEGIN_PROFILE("gatherSolverData");
|
||||
|
||||
|
||||
|
||||
int sizeofSB = sizeof(btSolverBody);
|
||||
int sizeofSC = sizeof(btSolverConstraint);
|
||||
|
||||
|
||||
if (numManifolds)
|
||||
{
|
||||
//if m_stackAlloc, try to pack bodies/constraints to speed up solving
|
||||
// btBlock* sablock;
|
||||
// sablock = stackAlloc->beginBlock();
|
||||
|
||||
// int memsize = 16;
|
||||
// unsigned char* stackMemory = stackAlloc->allocate(memsize);
|
||||
|
||||
|
||||
tmpSolverBodyPool.reserve(numManifolds*2);
|
||||
|
||||
{
|
||||
for (int i=0;i<numBodies;i++)
|
||||
{
|
||||
btRigidBody* rb = btRigidBody::upcast(bodies[i]);
|
||||
if (rb && (rb->getIslandTag() >= 0))
|
||||
{
|
||||
btAssert(rb->getCompanionId() < 0);
|
||||
int solverBodyId = tmpSolverBodyPool.size();
|
||||
btSolverBody& solverBody = tmpSolverBodyPool.expand();
|
||||
initSolverBody(&solverBody,rb);
|
||||
rb->setCompanionId(solverBodyId);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
tmpSolverConstraintPool.reserve(numManifolds*4+numConstraints);
|
||||
tmpSolverFrictionConstraintPool.reserve(numManifolds*4);
|
||||
|
||||
{
|
||||
int i;
|
||||
for (i=0;i<numManifolds;i++)
|
||||
{
|
||||
btPersistentManifold* manifold = manifoldPtr[i];
|
||||
btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
|
||||
btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
|
||||
|
||||
manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
|
||||
|
||||
int solverBodyIdA;
|
||||
int solverBodyIdB;
|
||||
|
||||
if (manifold->getNumContacts())
|
||||
{
|
||||
|
||||
|
||||
|
||||
if (rb0->getIslandTag() >= 0)
|
||||
{
|
||||
solverBodyIdA = rb0->getCompanionId();
|
||||
} else
|
||||
{
|
||||
//create a static body
|
||||
solverBodyIdA = tmpSolverBodyPool.size();
|
||||
btSolverBody& solverBody = tmpSolverBodyPool.expand();
|
||||
initSolverBody(&solverBody,rb0);
|
||||
}
|
||||
|
||||
if (rb1->getIslandTag() >= 0)
|
||||
{
|
||||
solverBodyIdB = rb1->getCompanionId();
|
||||
} else
|
||||
{
|
||||
//create a static body
|
||||
solverBodyIdB = tmpSolverBodyPool.size();
|
||||
btSolverBody& solverBody = tmpSolverBodyPool.expand();
|
||||
initSolverBody(&solverBody,rb1);
|
||||
}
|
||||
}
|
||||
|
||||
for (int j=0;j<manifold->getNumContacts();j++)
|
||||
{
|
||||
|
||||
btManifoldPoint& cp = manifold->getContactPoint(j);
|
||||
|
||||
int frictionIndex = tmpSolverConstraintPool.size();
|
||||
|
||||
if (cp.getDistance() <= btScalar(0.))
|
||||
{
|
||||
|
||||
const btVector3& pos1 = cp.getPositionWorldOnA();
|
||||
const btVector3& pos2 = cp.getPositionWorldOnB();
|
||||
|
||||
btVector3 rel_pos1 = pos1 - rb0->getCenterOfMassPosition();
|
||||
btVector3 rel_pos2 = pos2 - rb1->getCenterOfMassPosition();
|
||||
|
||||
|
||||
btScalar relaxation = 1.f;
|
||||
|
||||
{
|
||||
btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand();
|
||||
|
||||
solverConstraint.m_solverBodyIdA = solverBodyIdA;
|
||||
solverConstraint.m_solverBodyIdB = solverBodyIdB;
|
||||
solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
|
||||
|
||||
|
||||
|
||||
{
|
||||
//can be optimized, the cross products are already calculated
|
||||
btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
|
||||
btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
|
||||
btScalar denom = relaxation/(denom0+denom1);
|
||||
solverConstraint.m_jacDiagABInv = denom;
|
||||
}
|
||||
|
||||
solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
|
||||
solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
|
||||
solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
|
||||
|
||||
|
||||
btVector3 vel1 = rb0->getVelocityInLocalPoint(rel_pos1);
|
||||
btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2);
|
||||
|
||||
btVector3 vel = vel1 - vel2;
|
||||
btScalar rel_vel;
|
||||
rel_vel = cp.m_normalWorldOnB.dot(vel);
|
||||
|
||||
|
||||
solverConstraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations);
|
||||
solverConstraint.m_friction = cp.m_combinedFriction;
|
||||
btScalar rest = restitutionCurve(rel_vel, cp.m_combinedRestitution);
|
||||
if (rest <= btScalar(0.))
|
||||
{
|
||||
rest = 0.f;
|
||||
};
|
||||
|
||||
btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
|
||||
if (rest > penVel)
|
||||
{
|
||||
rest = btScalar(0.);
|
||||
}
|
||||
solverConstraint.m_restitution = rest;
|
||||
|
||||
solverConstraint.m_penetration *= -(infoGlobal.m_erp/infoGlobal.m_timeStep);
|
||||
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
solverConstraint.m_appliedVelocityImpulse = 0.f;
|
||||
|
||||
|
||||
btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
|
||||
solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0;
|
||||
btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
|
||||
solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1;
|
||||
}
|
||||
|
||||
//create 2 '1d axis' constraints for 2 tangential friction directions
|
||||
|
||||
//re-calculate friction direction every frame, todo: check if this is really needed
|
||||
btVector3 frictionTangential0a, frictionTangential1b;
|
||||
|
||||
btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b);
|
||||
|
||||
{
|
||||
btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
|
||||
solverConstraint.m_contactNormal = frictionTangential0a;
|
||||
|
||||
solverConstraint.m_solverBodyIdA = solverBodyIdA;
|
||||
solverConstraint.m_solverBodyIdB = solverBodyIdB;
|
||||
solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
|
||||
solverConstraint.m_frictionIndex = frictionIndex;
|
||||
|
||||
solverConstraint.m_friction = cp.m_combinedFriction;
|
||||
|
||||
solverConstraint.m_appliedImpulse = btScalar(0.);
|
||||
solverConstraint.m_appliedVelocityImpulse = 0.f;
|
||||
|
||||
btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
|
||||
btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
|
||||
btScalar denom = relaxation/(denom0+denom1);
|
||||
solverConstraint.m_jacDiagABInv = denom;
|
||||
|
||||
{
|
||||
btVector3 ftorqueAxis0 = rel_pos1.cross(solverConstraint.m_contactNormal);
|
||||
solverConstraint.m_relpos1CrossNormal = ftorqueAxis0;
|
||||
solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis0;
|
||||
}
|
||||
{
|
||||
btVector3 ftorqueAxis0 = rel_pos2.cross(solverConstraint.m_contactNormal);
|
||||
solverConstraint.m_relpos2CrossNormal = ftorqueAxis0;
|
||||
solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
|
||||
btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
|
||||
solverConstraint.m_contactNormal = frictionTangential1b;
|
||||
|
||||
solverConstraint.m_solverBodyIdA = solverBodyIdA;
|
||||
solverConstraint.m_solverBodyIdB = solverBodyIdB;
|
||||
solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
|
||||
solverConstraint.m_frictionIndex = frictionIndex;
|
||||
|
||||
solverConstraint.m_friction = cp.m_combinedFriction;
|
||||
|
||||
solverConstraint.m_appliedImpulse = btScalar(0.);
|
||||
solverConstraint.m_appliedVelocityImpulse = 0.f;
|
||||
|
||||
btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
|
||||
btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
|
||||
btScalar denom = relaxation/(denom0+denom1);
|
||||
solverConstraint.m_jacDiagABInv = denom;
|
||||
{
|
||||
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
|
||||
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
|
||||
solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1;
|
||||
}
|
||||
{
|
||||
btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
|
||||
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
|
||||
solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
END_PROFILE("gatherSolverData");
|
||||
|
||||
BEGIN_PROFILE("prepareConstraints");
|
||||
|
||||
btContactSolverInfo info = infoGlobal;
|
||||
|
||||
{
|
||||
int j;
|
||||
for (j=0;j<numConstraints;j++)
|
||||
{
|
||||
btTypedConstraint* constraint = constraints[j];
|
||||
constraint->buildJacobian();
|
||||
}
|
||||
}
|
||||
|
||||
END_PROFILE("prepareConstraints");
|
||||
|
||||
|
||||
BEGIN_PROFILE("solveConstraints");
|
||||
|
||||
//should traverse the contacts random order...
|
||||
int iteration;
|
||||
int j;
|
||||
{
|
||||
for ( iteration = 0;iteration<info.m_numIterations;iteration++)
|
||||
{
|
||||
for (j=0;j<numConstraints;j++)
|
||||
{
|
||||
btTypedConstraint* constraint = constraints[j];
|
||||
constraint->solveConstraint(info.m_timeStep);
|
||||
}
|
||||
|
||||
{
|
||||
int numPoolConstraints = tmpSolverConstraintPool.size();
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
{
|
||||
btSolverConstraint& solveManifold = tmpSolverConstraintPool[j];
|
||||
switch (solveManifold.m_constraintType)
|
||||
{
|
||||
case btSolverConstraint::BT_SOLVER_CONTACT_1D:
|
||||
{
|
||||
resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
|
||||
tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info);
|
||||
break;
|
||||
}
|
||||
case btSolverConstraint::BT_SOLVER_FRICTION_1D:
|
||||
{
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
//undefined constraint type?
|
||||
btAssert(0);
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size();
|
||||
for (j=0;j<numFrictionPoolConstraints;j++)
|
||||
{
|
||||
btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[j];
|
||||
switch (solveManifold.m_constraintType)
|
||||
{
|
||||
case btSolverConstraint::BT_SOLVER_CONTACT_1D:
|
||||
{
|
||||
break;
|
||||
}
|
||||
case btSolverConstraint::BT_SOLVER_FRICTION_1D:
|
||||
{
|
||||
|
||||
btScalar appliedNormalImpulse = tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
|
||||
|
||||
resolveSingleFrictionCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
|
||||
tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info,appliedNormalImpulse);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
//undefined constraint type?
|
||||
btAssert(0);
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
for (int i=0;i<tmpSolverBodyPool.size();i++)
|
||||
{
|
||||
tmpSolverBodyPool[i].writebackVelocity();
|
||||
}
|
||||
|
||||
END_PROFILE("solveConstraints");
|
||||
|
||||
|
||||
tmpSolverBodyPool.resize(0);
|
||||
tmpSolverConstraintPool.resize(0);
|
||||
tmpSolverFrictionConstraintPool.resize(0);
|
||||
|
||||
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
|
||||
{
|
||||
|
||||
|
||||
|
||||
//return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
|
||||
|
||||
|
||||
|
||||
BEGIN_PROFILE("prepareConstraints");
|
||||
|
||||
|
||||
btContactSolverInfo info = infoGlobal;
|
||||
|
||||
@@ -160,11 +735,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold**
|
||||
}
|
||||
}
|
||||
|
||||
END_PROFILE("prepareConstraints");
|
||||
|
||||
|
||||
BEGIN_PROFILE("solveConstraints");
|
||||
|
||||
//should traverse the contacts random order...
|
||||
int iteration;
|
||||
|
||||
{
|
||||
for ( iteration = 0;iteration<numiter-1;iteration++)
|
||||
for ( iteration = 0;iteration<numiter;iteration++)
|
||||
{
|
||||
int j;
|
||||
if (m_solverMode & SOLVER_RANDMIZE_ORDER)
|
||||
@@ -202,22 +782,23 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold**
|
||||
}
|
||||
}
|
||||
|
||||
END_PROFILE("solveConstraints");
|
||||
|
||||
|
||||
#ifdef USE_PROFILE
|
||||
btProfiler::endBlock("solve");
|
||||
#endif //USE_PROFILE
|
||||
|
||||
|
||||
|
||||
|
||||
return btScalar(0.);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
btScalar penetrationResolveFactor = btScalar(0.9);
|
||||
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
|
||||
{
|
||||
btScalar rest = restitution * -rel_vel;
|
||||
return rest;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
|
||||
@@ -304,10 +885,10 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
|
||||
|
||||
btScalar combinedRestitution = cp.m_combinedRestitution;
|
||||
|
||||
cpd->m_penetration = cp.getDistance();
|
||||
cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
|
||||
cpd->m_friction = cp.m_combinedFriction;
|
||||
cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
|
||||
if (cpd->m_restitution <= 0.) //btScalar(0.))
|
||||
if (cpd->m_restitution <= btScalar(0.))
|
||||
{
|
||||
cpd->m_restitution = btScalar(0.0);
|
||||
|
||||
@@ -404,6 +985,44 @@ void btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
btScalar maxImpulse = btScalar(0.);
|
||||
|
||||
{
|
||||
|
||||
btVector3 color(0,1,0);
|
||||
{
|
||||
if (cp.getDistance() <= btScalar(0.))
|
||||
{
|
||||
|
||||
if (iter == 0)
|
||||
{
|
||||
if (debugDrawer)
|
||||
debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
|
||||
}
|
||||
|
||||
{
|
||||
|
||||
btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
|
||||
btScalar impulse = resolveSingleCollisionCombined(
|
||||
*body0,*body1,
|
||||
cp,
|
||||
info);
|
||||
|
||||
if (maxImpulse < impulse)
|
||||
maxImpulse = impulse;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return maxImpulse;
|
||||
}
|
||||
|
||||
|
||||
|
||||
btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
|
||||
{
|
||||
|
||||
|
||||
Reference in New Issue
Block a user