예제 #1
0
void SolverThread::stepPhysics(float dt)
{
	computeForces();

	clearStiffnessAssembly();	

	if(bUseStiffnessWarping)
		updateOrientation();
	else
		resetOrientation();

	stiffnessAssembly();
	
	// addPlasticityForce(dt);
 
	dynamicsAssembly(dt);
 
#if SOLVEONGPU
	solveGpu(m_V, m_stiffnessMatrix);
#else
    solve(m_V);
#endif
 
	updatePosition(dt);
	
#if ENABLE_DBG	
	dbglg.write("Re");
	unsigned totalTetrahedra = m_mesh->numTetrahedra();
	FEMTetrahedronMesh::Tetrahedron * tetrahedra = m_mesh->tetrahedra();
	for(unsigned k=0;k<totalTetrahedra;k++) {
	    dbglg.write(k);
		dbglg.write(tetrahedra[k].Re.str());
	}

	dbglg.writeMat33(m_stiffnessMatrix->valueBuf(), 
	    m_stiffnessMatrix->numNonZero(),
	    "K ");

	dbglg.write("Rhs");
	unsigned totalPoints = m_mesh->numPoints();
	for(unsigned k=0;k<totalPoints;k++) {
	    dbglg.write(k);
		dbglg.write(rightHandSide()[k].str());
		dbglg.newLine();
	}
	dbglg.write("F0");
	for(unsigned k=0;k<totalPoints;k++) {
	    dbglg.write(k);
		dbglg.write(m_F0[k].str());
		dbglg.newLine();
	}
#endif
}
예제 #2
0
void SolverThread::calculateK()
{
#if ENABLE_DBG
    dbglg.write("Ke");
#endif
    unsigned totalTetrahedra = m_mesh->numTetrahedra();
    Vector3F * Xi = m_mesh->Xi();
    FEMTetrahedronMesh::Tetrahedron * tetrahedra = m_mesh->tetrahedra();
    
    for(unsigned k=0;k<totalTetrahedra;k++) {
		
		Vector3F x0 = Xi[tetrahedra[k].indices[0]];
		Vector3F x1 = Xi[tetrahedra[k].indices[1]];
		Vector3F x2 = Xi[tetrahedra[k].indices[2]];
		Vector3F x3 = Xi[tetrahedra[k].indices[3]];
		
		//For this check page no.: 344-346 of Kenny Erleben's book Physics based Animation
		//Eq. 10.30(a-c)
		Vector3F e10 = x1-x0;
		Vector3F e20 = x2-x0;
		Vector3F e30 = x3-x0;

		// tetrahedra[k].e1 = e10;
		// tetrahedra[k].e2 = e20;
		// tetrahedra[k].e3 = e30;

		tetrahedra[k].volume= FEMTetrahedronMesh::getTetraVolume(e10,e20,e30);
		
		//Eq. 10.32
		Matrix33F E; 
		E.fill(e10, e20, e30);
		
		float detE = E.determinant(); if(detE ==0.f) std::cout<<" zero det "<<E.str()<<"\n";
		float invDetE = 1.0f/detE;	
		
		//Eq. 10.40 (a) & Eq. 10.42 (a)
		//Shape function derivatives wrt x,y,z
		// d/dx N0
		float invE10 = (e20.z*e30.y - e20.y*e30.z)*invDetE;
		float invE20 = (e10.y*e30.z - e10.z*e30.y)*invDetE;
		float invE30 = (e10.z*e20.y - e10.y*e20.z)*invDetE;
		float invE00 = -invE10-invE20-invE30;

		//Eq. 10.40 (b) & Eq. 10.42 (b)
		// d/dy N0
		float invE11 = (e20.x*e30.z - e20.z*e30.x)*invDetE;
		float invE21 = (e10.z*e30.x - e10.x*e30.z)*invDetE;
		float invE31 = (e10.x*e20.z - e10.z*e20.x)*invDetE;
		float invE01 = -invE11-invE21-invE31;

		//Eq. 10.40 (c) & Eq. 10.42 (c)
		// d/dz N0
		float invE12 = (e20.y*e30.x - e20.x*e30.y)*invDetE;
		float invE22 = (e10.x*e30.y - e10.y*e30.x)*invDetE;
		float invE32 = (e10.y*e20.x - e10.x*e20.y)*invDetE;
		float invE02 = -invE12-invE22-invE32;

		//Eq. 10.43 
		//Bn ~ [bn cn dn]^T
		// bn = d/dx N0 = [ invE00 invE10 invE20 invE30 ]
		// cn = d/dy N0 = [ invE01 invE11 invE21 invE31 ]
		// dn = d/dz N0 = [ invE02 invE12 invE22 invE32 ]
		tetrahedra[k].B[0] = Vector3F(invE00, invE01, invE02);		
		tetrahedra[k].B[1] = Vector3F(invE10, invE11, invE12);		
		tetrahedra[k].B[2] = Vector3F(invE20, invE21, invE22);		
		tetrahedra[k].B[3] = Vector3F(invE30, invE31, invE32);
		
		// std::cout<<"B[0] "<<tetrahedra[k].B[0]<<"\n";
		// std::cout<<"B[1] "<<tetrahedra[k].B[1]<<"\n";
		// std::cout<<"B[2] "<<tetrahedra[k].B[2]<<"\n";
		// std::cout<<"B[3] "<<tetrahedra[k].B[3]<<"\n";
 
		for(unsigned i=0;i<4;i++) {
			for(unsigned j=0;j<4;j++) {
				Matrix33F & Ke = tetrahedra[k].Ke[i][j];
				float d19 = tetrahedra[k].B[i].x;
				float d20 = tetrahedra[k].B[i].y;
				float d21 = tetrahedra[k].B[i].z;
				float d22 = tetrahedra[k].B[j].x;
				float d23 = tetrahedra[k].B[j].y;
				float d24 = tetrahedra[k].B[j].z;
				*Ke.m(0, 0)= d16 * d19 * d22 + d18 * (d20 * d23 + d21 * d24);
				*Ke.m(0, 1)= d17 * d19 * d23 + d18 * (d20 * d22);
				*Ke.m(0, 2)= d17 * d19 * d24 + d18 * (d21 * d22);

				*Ke.m(1, 0)= d17 * d20 * d22 + d18 * (d19 * d23);
				*Ke.m(1, 1)= d16 * d20 * d23 + d18 * (d19 * d22 + d21 * d24);
				*Ke.m(1, 2)= d17 * d20 * d24 + d18 * (d21 * d23);

				*Ke.m(2, 0)= d17 * d21 * d22 + d18 * (d19 * d24);
				*Ke.m(2, 1)= d17 * d21 * d23 + d18 * (d20 * d24);
				*Ke.m(2, 2)= d16 * d21 * d24 + d18 * (d20 * d23 + d19 * d22);

				Ke *= tetrahedra[k].volume;
#if ENABLE_DBG				
				dbglg.write("kij");
				dbglg.write(k);
				dbglg.write(i);
				dbglg.write(j);
				dbglg.write(Ke.str());
#endif
			}
		}
 	}
}
예제 #3
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");
}