void SolverThread::stiffnessAssembly()
{
updateF0();
FEMTetrahedronMesh::Tetrahedron * tetrahedra = m_mesh->tetrahedra();
unsigned totalTetrahedra = m_mesh->numTetrahedra();
for(unsigned k=0;k<totalTetrahedra;k++) {
Matrix33F Re = tetrahedra[k].Re;
Matrix33F ReT = Re; ReT.transpose();
for (unsigned i = 0; i < 4; ++i) {
for (unsigned j = 0; j < 4; ++j) {
Matrix33F tmpKe = tetrahedra[k].Ke[i][j];
if (j >= i) {
//Based on pseudocode given in Fig. 10.12 on page 361
Matrix33F tmp = (Re*tmpKe)*ReT;
Matrix33F tmpT = tmp; tmpT.transpose();
m_K_row[ tetrahedra[k].indices[i] ][tetrahedra[k].indices[j]]+=(tmp);
if (j > i) {
m_K_row[ tetrahedra[k].indices[j] ][tetrahedra[k].indices[i]]+= tmpT;
}
}
}
}
}
}
void SolverThread::addPlasticityForce(float dt)
{
unsigned totalTetrahedra = m_mesh->numTetrahedra();
Vector3F * X = m_mesh->X();
Vector3F * Xi = m_mesh->Xi();
FEMTetrahedronMesh::Tetrahedron * tetrahedra = m_mesh->tetrahedra();
for(unsigned k=0;k<totalTetrahedra;k++) {
float e_total[6];
float e_elastic[6];
for(int i=0;i<6;++i)
e_elastic[i] = e_total[i] = 0;
//--- Compute total strain: e_total = Be (Re^{-1} x - x0)
for(unsigned int j=0;j<4;++j) {
Vector3F x_j = X[tetrahedra[k].indices[j]];
Vector3F x0_j = Xi[tetrahedra[k].indices[j]];
Matrix33F ReT = tetrahedra[k].Re; ReT.transpose();
Vector3F prod = ReT * x_j;
//Vector3F(ReT[0][0]*x_j.x+ ReT[0][1]*x_j.y+ReT[0][2]*x_j.z, //tmpKe*x0;
// ReT[1][0]*x_j.x+ ReT[1][1]*x_j.y+ReT[1][2]*x_j.z,
// ReT[2][0]*x_j.x+ ReT[2][1]*x_j.y+ReT[2][2]*x_j.z);
Vector3F tmp = prod - x0_j;
//B contains Jacobian of shape funcs. B=SN
float bj = tetrahedra[k].B[j].x;
float cj = tetrahedra[k].B[j].y;
float dj = tetrahedra[k].B[j].z;
e_total[0] += bj*tmp.x;
e_total[1] += cj*tmp.y;
e_total[2] += dj*tmp.z;
e_total[3] += cj*tmp.x + bj*tmp.y;
e_total[4] += dj*tmp.x + bj*tmp.z;
e_total[5] += dj*tmp.y + cj*tmp.z;
}
//--- Compute elastic strain
for(int i=0;i<6;++i)
e_elastic[i] = e_total[i] - tetrahedra[k].plastic[i];
//--- if elastic strain exceeds c_yield then it is added to plastic strain by c_creep
float norm_elastic = 0;
for(int i=0;i<6;++i)
norm_elastic += e_elastic[i]*e_elastic[i];
norm_elastic = sqrt(norm_elastic);
if(norm_elastic > yield) {
float creepdt = 1.f /dt;
if(creepdt > creep) creepdt = creep;
float amount = dt * creepdt; //--- make sure creep do not exceed 1/dt
for(int i=0;i<6;++i)
tetrahedra[k].plastic[i] += amount*e_elastic[i];
}
//--- if plastic strain exceeds c_max then it is clamped to maximum magnitude
float norm_plastic = 0;
for(int i=0;i<6;++i)
norm_plastic += tetrahedra[k].plastic[i]* tetrahedra[k].plastic[i];
norm_plastic = sqrt(norm_plastic);
if(norm_plastic > m_max) {
float scale = m_max/norm_plastic;
for(int i=0;i<6;++i)
tetrahedra[k].plastic[i] *= scale;
}
for(unsigned n=0;n<4;++n) {
float* e_plastic = tetrahedra[k].plastic;
//bn, cn and dn are the shape function derivative wrt. x,y and z axis
//These were calculated in CalculateK function
//Eq. 10.140(a) & (b) on page 365
float bn = tetrahedra[k].B[n].x;
float cn = tetrahedra[k].B[n].y;
float dn = tetrahedra[k].B[n].z;
float D0 = D.x;
float D1 = D.y;
float D2 = D.z;
Vector3F f = Vector3F::Zero;
float bnD0 = bn*D0;
float bnD1 = bn*D1;
float bnD2 = bn*D2;
float cnD0 = cn*D0;
float cnD1 = cn*D1;
float cnD2 = cn*D2;
float dnD0 = dn*D0;
float dnD1 = dn*D1;
float dnD2 = dn*D2;
//Eq. 10.141 on page 365
f.x = bnD0*e_plastic[0] + bnD1*e_plastic[1] + bnD1*e_plastic[2] + cnD2*e_plastic[3] + dnD2*e_plastic[4];
f.y = cnD1*e_plastic[0] + cnD0*e_plastic[1] + cnD1*e_plastic[2] + bnD2*e_plastic[3] + + dnD2*e_plastic[5];
f.z = dnD1*e_plastic[0] + dnD1*e_plastic[1] + dnD0*e_plastic[2] + bnD2*e_plastic[4] + cnD2*e_plastic[5];
f *= tetrahedra[k].volume;
int idx = tetrahedra[k].indices[n];
m_F[idx] += tetrahedra[k].Re*f;
}
}
}