Matrix33F Patch::tangentFrame() const
{
Matrix33F frm;
Vector3F du = (vertex(1) - vertex(0) + vertex(2) - vertex(3)) * .5f;
Vector3F dv = (vertex(3) - vertex(0) + vertex(2) - vertex(1)) * .5f;
du.normalize();
dv.normalize();
Vector3F side = du.cross(dv);
side.normalize();
Vector3F up = du.cross(side);
up.normalize();
frm.fill(side, up, du);
return frm;
}
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
}
}
}
}