void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
{
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
btCollisionObject* colObj0=0,*colObj1=0;
colObj0 = (btCollisionObject*)manifold->getBody0();
colObj1 = (btCollisionObject*)manifold->getBody1();
int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
// btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
// btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
///avoid collision response between two static objects
// if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
// return;
for (int j=0;j<manifold->getNumContacts();j++)
{
btManifoldPoint& cp = manifold->getContactPoint(j);
if (cp.getDistance() <= manifold->getContactProcessingThreshold())
{
btScalar relaxation;
int frictionIndex = m_multiBodyNormalContactConstraints.size();
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
// btRigidBody* rb0 = btRigidBody::upcast(colObj0);
// btRigidBody* rb1 = btRigidBody::upcast(colObj1);
solverConstraint.m_orgConstraint = 0;
solverConstraint.m_orgDofIndex = -1;
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_multiBodyA = mbA;
if (mbA)
solverConstraint.m_linkA = fcA->m_link;
solverConstraint.m_multiBodyB = mbB;
if (mbB)
solverConstraint.m_linkB = fcB->m_link;
solverConstraint.m_originalContactPoint = &cp;
bool isFriction = false;
setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction);
// const btVector3& pos1 = cp.getPositionWorldOnA();
// const btVector3& pos2 = cp.getPositionWorldOnB();
/////setup the friction constraints
#define ENABLE_FRICTION
#ifdef ENABLE_FRICTION
solverConstraint.m_frictionIndex = frictionIndex;
#if ROLLING_FRICTION
int rollingFriction=1;
btVector3 angVelA(0,0,0),angVelB(0,0,0);
if (rb0)
angVelA = rb0->getAngularVelocity();
if (rb1)
angVelB = rb1->getAngularVelocity();
btVector3 relAngVel = angVelB-angVelA;
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
{
//only a single rollingFriction per manifold
rollingFriction--;
if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
{
relAngVel.normalize();
applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
if (relAngVel.length()>0.001)
addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
} else
{
addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
btVector3 axis0,axis1;
btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
if (axis0.length()>0.001)
addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if (axis1.length()>0.001)
addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
}
#endif //ROLLING_FRICTION
///Bullet has several options to set the friction directions
///By default, each contact has only a single friction direction that is recomputed automatically very frame
///based on the relative linear velocity.
///If the relative velocity it zero, it will automatically compute a friction direction.
///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
///
///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
///
///The user can manually override the friction directions for certain contacts using a contact callback,
///and set the cp.m_lateralFrictionInitialized to true
///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
///this will give a conveyor belt effect
///
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 *= 1.f/btSqrt(lat_rel_vel);
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
cp.m_lateralFrictionDir2.normalize();//??
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
} else
*/
{
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
}
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
{
cp.m_lateralFrictionInitialized = true;
}
}
} else
{
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2);
//setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
//todo:
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
}
#endif //ENABLE_FRICTION
}
}
}
void kSetupContact(btParallelConstraintSolver* pSolver,
btParallelConstraintSolverSetupTaskParams* pParams,
btContactSolverInfo* pInfoGlobal, int threadId)
{
int numConstraints = pParams[threadId].m_numContactConstraints;
unsigned long int timeStamp;
int startIndex = pParams[threadId].m_startIndex;
btContactSolverInfo& infoGlobal = *pInfoGlobal;
for(int i = 0; i < numConstraints; i++)
{
timeStamp = sClock.getTimeMicroseconds();
btSolverConstraint& solverConstraint = pSolver->m_tmpSolverContactConstraintPool[startIndex + i];
solverConstraint.m_numConsecutiveRowsPerKernel = timeStamp;
btCollisionObject* colObj0 = (btCollisionObject*)solverConstraint.m_solverBodyA;
btCollisionObject* colObj1 = (btCollisionObject*)solverConstraint.m_solverBodyB;
btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
btManifoldPoint& cp = *((btManifoldPoint*)(solverConstraint.m_originalContactPoint));
btVector3 rel_pos1;
btVector3 rel_pos2;
btScalar relaxation;
btScalar rel_vel;
btVector3 vel;
pSolver->setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
int currFrictIndex = solverConstraint.m_frictionIndex;
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);
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
cp.m_lateralFrictionDir2.normalize();//??
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
currFrictIndex++;
pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
currFrictIndex++;
pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,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);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
currFrictIndex++;
pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
currFrictIndex++;
pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
cp.m_lateralFrictionInitialized = true;
}
}
else
{
btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
currFrictIndex++;
pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
currFrictIndex++;
pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
}
}
pSolver->setFrictionConstraintImpulse( solverConstraint, solverBodyA, solverBodyB, cp, infoGlobal);
}
}
