void SimpleContactSolver::solveContacts(unsigned numContacts,
CUDABuffer * contactBuf,
CUDABuffer * pairBuf,
void * objectData)
{
#if DISABLE_COLLISION_RESOLUTION
return;
#endif
if(numContacts < 1) return;
m_numContacts = numContacts;
const unsigned indBufLength = iRound1024(numContacts * 2);
m_sortedInd[0]->create(indBufLength * 8);
m_sortedInd[1]->create(indBufLength * 8);
void * bodyContactHash = m_sortedInd[0]->bufferOnDevice();
void * pairs = pairBuf->bufferOnDevice();
simpleContactSolverWriteContactIndex((KeyValuePair *)bodyContactHash, (uint *)pairs, numContacts * 2, indBufLength);
void * tmp = m_sortedInd[1]->bufferOnDevice();
RadixSort((KeyValuePair *)bodyContactHash, (KeyValuePair *)tmp, indBufLength, 30);
m_splitPair->create(numContacts * 8);
void * splits = m_splitPair->bufferOnDevice();
const unsigned splitBufLength = numContacts * 2;
simpleContactSolverComputeSplitBufLoc((uint2 *)splits,
(uint2 *)pairs,
(KeyValuePair *)bodyContactHash,
splitBufLength);
m_bodyCount->create(splitBufLength * 4);
void * bodyCount = m_bodyCount->bufferOnDevice();
simpleContactSolverCountBody((uint *)bodyCount,
(KeyValuePair *)bodyContactHash,
splitBufLength);
int mxcount = 0;
max<int>(mxcount, (int *)bodyCount, splitBufLength);
// if(mxcount>9) std::cout<<" max count per contact "<<mxcount;
int numiterations = mxcount + 3;
m_splitInverseMass->create(splitBufLength * 4);
void * splitMass = m_splitInverseMass->bufferOnDevice();
CudaNarrowphase::CombinedObjectBuffer * objectBuf = (CudaNarrowphase::CombinedObjectBuffer *)objectData;
void * pos = objectBuf->m_pos->bufferOnDevice();
void * vel = objectBuf->m_vel->bufferOnDevice();
void * mass = objectBuf->m_mass->bufferOnDevice();
void * ind = objectBuf->m_ind->bufferOnDevice();
void * perObjPointStart = objectBuf->m_pointCacheLoc->bufferOnDevice();
void * perObjectIndexStart = objectBuf->m_indexCacheLoc->bufferOnDevice();
simpleContactSolverComputeSplitInverseMass((float *)splitMass,
(uint2 *)splits,
(uint2 *)pairs,
(float *)mass,
(uint4 *)ind,
(uint * )perObjPointStart,
(uint * )perObjectIndexStart,
(uint *)bodyCount,
splitBufLength);
m_constraint->create(numContacts * 64);
void * constraint = m_constraint->bufferOnDevice();
void * contacts = contactBuf->bufferOnDevice();
simpleContactSolverSetContactConstraint((ContactConstraint *)constraint,
(uint2 *)splits,
(uint2 *)pairs,
(float3 *)pos,
(float3 *)vel,
(uint4 *)ind,
(uint * )perObjPointStart,
(uint * )perObjectIndexStart,
(float *)splitMass,
(ContactData *)contacts,
numContacts * 2);
CudaBase::CheckCudaError("jacobi solver set constraint");
m_deltaLinearVelocity->create(nextPow2(splitBufLength * 12));
m_deltaAngularVelocity->create(nextPow2(splitBufLength * 12));
void * deltaLinVel = m_deltaLinearVelocity->bufferOnDevice();
void * deltaAngVel = m_deltaAngularVelocity->bufferOnDevice();
simpleContactSolverClearDeltaVelocity((float3 *)deltaLinVel,
(float3 *)deltaAngVel,
splitBufLength);
/*
const unsigned scanBufLength = iRound1024(numContacts * 2);
m_bodyCount->create(scanBufLength * 4);
m_scanBodyCount[0]->create(scanBufLength * 4);
m_scanBodyCount[1]->create(scanBufLength * 4);
void * scanResult = m_scanBodyCount[0]->bufferOnDevice();
void * scanIntermediate = m_scanBodyCount[1]->bufferOnDevice();
scanExclusive((uint *)scanResult, (uint *)bodyCount, (uint *)scanIntermediate, scanBufLength / 1024, 1024);
const unsigned numSplitBodies = ScanUtil::getScanResult(m_bodyCount, m_scanBodyCount[0], scanBufLength);
*/
int i;
for(i=0; i< numiterations; i++) {
// compute impulse and velocity changes per contact
simpleContactSolverSolveContactWoJ((ContactConstraint *)constraint,
(float3 *)deltaLinVel,
(float3 *)deltaAngVel,
(uint2 *)pairs,
(uint2 *)splits,
(float *)splitMass,
(ContactData *)contacts,
(float3 *)pos,
(float3 *)vel,
(uint4 *)ind,
(uint * )perObjPointStart,
(uint * )perObjectIndexStart,
numContacts * 2);
CudaBase::CheckCudaError("jacobi solver solve impulse");
simpleContactSolverAverageVelocities((float3 *)deltaLinVel,
(float3 *)deltaAngVel,
(uint *)bodyCount,
(KeyValuePair *)bodyContactHash,
splitBufLength);
CudaBase::CheckCudaError("jacobi solver average velocity");
}
// 2 tet per contact, 4 pnt per tet, key is pnt index, value is tet index in split
const unsigned pntHashBufLength = iRound1024(numContacts * 2 * 4);
// std::cout<<"\n pntHashBufLength"<<pntHashBufLength
// <<" numContact"<<numContacts;
m_pntTetHash[0]->create(pntHashBufLength * 8);
m_pntTetHash[1]->create(pntHashBufLength * 8);
void * pntTetHash = m_pntTetHash[0]->bufferOnDevice();
simpleContactSolverWritePointTetHash((KeyValuePair *)pntTetHash,
(uint2 *)pairs,
(uint2 *)splits,
(uint *)bodyCount,
(uint4 *)ind,
(uint * )perObjPointStart,
(uint * )perObjectIndexStart,
numContacts * 2,
pntHashBufLength);
CudaBase::CheckCudaError(CudaBase::Synchronize(),
"jacobi solver point-tetra hash");
void * intermediate = m_pntTetHash[1]->bufferOnDevice();
RadixSort((KeyValuePair *)pntTetHash, (KeyValuePair *)intermediate, pntHashBufLength, 24);
#if 0
svlg.writeHash(m_pntTetHash[1], numContacts * 2,
"pnttet_hash", CudaDbgLog::FAlways);
#endif
simpleContactSolverUpdateVelocity((float3 *)vel,
(float3 *)deltaLinVel,
(float3 *)deltaAngVel,
(KeyValuePair *)pntTetHash,
(uint2 *)pairs,
(uint2 *)splits,
(ContactConstraint *)constraint,
(ContactData *)contacts,
(float3 *)pos,
(uint4 *)ind,
(uint * )perObjPointStart,
(uint * )perObjectIndexStart,
numContacts * 2 * 4);
CudaBase::CheckCudaError(CudaBase::Synchronize(),
"jacobi solver update velocity");
}
void SimpleContactSolver::solveContacts(unsigned numContacts,
CUDABuffer * contactBuf,
CUDABuffer * pairBuf,
void * objectData)
{
#if DISABLE_COLLISION_RESOLUTION
return;
#endif
if(numContacts < 1) return;
#if 0
svlg.writeInt2( pairBuf,
numContacts,
"pair", CudaDbgLog::FAlways);
#endif
const unsigned indBufLength = iRound1024(numContacts * 2);
m_sortedInd[0]->create(indBufLength * 8);
m_sortedInd[1]->create(indBufLength * 8);
void * bodyContactHash = m_sortedInd[0]->bufferOnDevice();
void * pairs = pairBuf->bufferOnDevice();
/*
* for either side of each contact pair, set
* key: body index
* velue: contact index
* n x 2 hash
* sort by body index to put the same body together
*/
simpleContactSolverWriteContactIndex((KeyValuePair *)bodyContactHash,
(uint *)pairs,
numContacts * 2,
indBufLength);
void * tmp = m_sortedInd[1]->bufferOnDevice();
RadixSort((KeyValuePair *)bodyContactHash, (KeyValuePair *)tmp, indBufLength, 30);
#if 0
svlg.writeHash( m_sortedInd[0],
numContacts * 2,
"body-contact", CudaDbgLog::FAlways);
#endif
/*
* for each hash, find the index of contact pair
* set the indirection from contact pair to hash index
*/
m_splitPair->create(numContacts * 8);
void * splits = m_splitPair->bufferOnDevice();
const unsigned splitBufLength = numContacts * 2;
simpleContactSolverComputeSplitBufLoc((uint2 *)splits,
(uint2 *)pairs,
(KeyValuePair *)bodyContactHash,
splitBufLength);
#if 0
svlg.writeInt2( m_splitPair,
numContacts,
"splitpair", CudaDbgLog::FAlways);
#endif
m_bodyCount->create(splitBufLength * 4);
void * bodyCount = m_bodyCount->bufferOnDevice();
simpleContactSolverCountBody((uint *)bodyCount,
(KeyValuePair *)bodyContactHash,
splitBufLength);
#if 0
// num iterattions by max contacts per object
// todo ignore static object count
int mxcount = 0;
max<int>(mxcount, (int *)bodyCount, splitBufLength);
int numiterations = mxcount + 3;
#else
int numiterations = 9;
#endif
m_splitInverseMass->create(splitBufLength * 4);
void * splitMass = m_splitInverseMass->bufferOnDevice();
CudaNarrowphase::CombinedObjectBuffer * objectBuf = (CudaNarrowphase::CombinedObjectBuffer *)objectData;
void * pos = objectBuf->m_pos->bufferOnDevice();
void * vel = objectBuf->m_vel->bufferOnDevice();
void * mass = objectBuf->m_mass->bufferOnDevice();
void * linearImpulse = objectBuf->m_linearImpulse->bufferOnDevice();
void * ind = objectBuf->m_ind->bufferOnDevice();
void * perObjPointStart = objectBuf->m_pointCacheLoc->bufferOnDevice();
void * perObjectIndexStart = objectBuf->m_indexCacheLoc->bufferOnDevice();
m_bodyTetInd->create(4* 4 * numContacts *2);
simpleContactSolverComputeSplitInverseMass((float *)splitMass,
(uint2 *)splits,
(uint2 *)pairs,
(float *)mass,
(uint4 *)ind,
(uint * )perObjPointStart,
(uint * )perObjectIndexStart,
(uint *)bodyCount,
(uint4 *)m_bodyTetInd->bufferOnDevice(),
numContacts * 2);
#if 0
// svlg.writeFlt( m_splitInverseMass,
// numContacts,
// "masstensor", CudaDbgLog::FAlways);
svlg.writeUInt( objectBuf->m_pointCacheLoc,
2,
"pstart", CudaDbgLog::FAlways);
svlg.writeUInt( objectBuf->m_indexCacheLoc,
2,
"istart", CudaDbgLog::FAlways);
#endif
m_constraint->create(numContacts * 64);
m_contactLinearVelocity->create(numContacts * 2 * 12);
void * constraint = m_constraint->bufferOnDevice();
void * contactLinearVel = m_contactLinearVelocity->bufferOnDevice();
void * contacts = contactBuf->bufferOnDevice();
contactconstraint::prepareNoPenetratingContact((ContactConstraint *)constraint,
(float3 *)contactLinearVel,
(uint2 *)splits,
(uint2 *)pairs,
(float3 *)pos,
(float3 *)vel,
(float3 *)linearImpulse,
(float *)splitMass,
(ContactData *)contacts,
(uint4 *)m_bodyTetInd->bufferOnDevice(),
numContacts * 2);
CudaBase::CheckCudaError("jacobi solver prepare constraint");
#if 0
svlg.writeUInt( m_bodyTetInd,
numContacts * 8,
"tet", CudaDbgLog::FAlways);
#endif
#if 0
svlg.writeFlt( contactBuf,
numContacts * 12,
"contact", CudaDbgLog::FAlways);
#endif
#if 0
svlg.writeStruct(m_constraint, numContacts,
"constraint",
constraintDesc,
64,
CudaDbgLog::FAlways);
// svlg.writeVec3(m_contactLinearVelocity, numContacts * 2,
// "contact_vel", CudaDbgLog::FAlways);
#endif
m_deltaLinearVelocity->create(nextPow2(splitBufLength * 12));
m_deltaAngularVelocity->create(nextPow2(splitBufLength * 12));
void * deltaLinVel = m_deltaLinearVelocity->bufferOnDevice();
void * deltaAngVel = m_deltaAngularVelocity->bufferOnDevice();
simpleContactSolverClearDeltaVelocity((float3 *)deltaLinVel,
(float3 *)deltaAngVel,
splitBufLength);
int i;
for(i=0; i< numiterations; i++) {
// compute impulse and velocity changes per contact
collisionres::resolveCollision((ContactConstraint *)constraint,
(float3 *)contactLinearVel,
(float3 *)deltaLinVel,
(uint2 *)pairs,
(uint2 *)splits,
(float *)splitMass,
(ContactData *)contacts,
numContacts * 2);
CudaBase::CheckCudaError("jacobi solver resolve collision");
#if 0
unsigned ii = i;
svlg.write(ii);
#endif
#if 0
svlg.writeVec3(m_deltaLinearVelocity, numContacts * 2,
"deltaV_b4", CudaDbgLog::FAlways);
#endif
simpleContactSolverAverageVelocities((float3 *)deltaLinVel,
(float3 *)deltaAngVel,
(uint *)bodyCount,
(KeyValuePair *)bodyContactHash,
splitBufLength);
CudaBase::CheckCudaError("jacobi solver average velocity");
#if 0
svlg.writeVec3(m_deltaLinearVelocity, numContacts * 2,
"deltaV_avg", CudaDbgLog::FAlways);
#endif
collisionres::resolveFriction((ContactConstraint *)constraint,
(float3 *)contactLinearVel,
(float3 *)deltaLinVel,
(uint2 *)pairs,
(uint2 *)splits,
(float *)splitMass,
(ContactData *)contacts,
numContacts * 2);
CudaBase::CheckCudaError("jacobi solver resolve friction");
simpleContactSolverAverageVelocities((float3 *)deltaLinVel,
(float3 *)deltaAngVel,
(uint *)bodyCount,
(KeyValuePair *)bodyContactHash,
splitBufLength);
CudaBase::CheckCudaError("jacobi solver average velocity");
}
// 2 tet per contact, 4 pnt per tet, key is pnt index, value is tet index in split
const unsigned pntHashBufLength = iRound1024(numContacts * 2 * 4);
// std::cout<<"\n pntHashBufLength"<<pntHashBufLength
// <<" numContact"<<numContacts;
m_pntTetHash[0]->create(pntHashBufLength * 8);
m_pntTetHash[1]->create(pntHashBufLength * 8);
void * pntTetHash = m_pntTetHash[0]->bufferOnDevice();
simpleContactSolverWritePointTetHash((KeyValuePair *)pntTetHash,
(uint2 *)pairs,
(uint2 *)splits,
(uint *)bodyCount,
(uint4 *)m_bodyTetInd->bufferOnDevice(),
numContacts * 2,
pntHashBufLength);
CudaBase::CheckCudaError(// CudaBase::Synchronize(),
"jacobi solver point-tetra hash");
void * intermediate = m_pntTetHash[1]->bufferOnDevice();
RadixSort((KeyValuePair *)pntTetHash, (KeyValuePair *)intermediate, pntHashBufLength, 24);
#if 0
svlg.writeHash(m_pntTetHash[1], numContacts * 2,
"pnttet_hash", CudaDbgLog::FAlways);
#endif
contactsolver::updateImpulse((float3 *)linearImpulse,
(float3 *)deltaLinVel,
(float3 *)deltaAngVel,
(KeyValuePair *)pntTetHash,
(uint2 *)pairs,
(uint2 *)splits,
(ContactConstraint *)constraint,
(ContactData *)contacts,
(float3 *)pos,
(uint4 *)ind,
(uint * )perObjPointStart,
(uint * )perObjectIndexStart,
numContacts * 2 * 4);
CudaBase::CheckCudaError(// CudaBase::Synchronize(),
"jacobi solver update velocity");
}