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");
}
Exemple #2
0
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");
}