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 SargassoNode::updateSpace(MDataBlock& block, unsigned idx)
{
MStatus stat;
/* // jump to elm is slow
MArrayDataHandle hparentspaces = block.inputArrayValue(aconstraintParentInverseMatrix, &stat);
// if(!stat) MGlobal::displayInfo("cannot input array");
stat = hparentspaces.jumpToElement(idx);
//if(!stat) AHelper::Info<unsigned>("cannot jump to elm", iobject);
MDataHandle hspace = hparentspaces.inputValue(&stat);
// if(!stat) MGlobal::displayInfo("cannot input single");
const MMatrix parentSpace = hspace.asMatrix();
*/
const unsigned itri = objectTriangleInd()[idx];
Matrix33F q = m_diff->Q()[itri];
q.orthoNormalize();
const Vector3F t = m_mesh->triangleCenter(itri);
Matrix44F sp(q, t);
AHelper::ConvertToMMatrix(m_currentSpace, sp);
// m_currentSpace *= parentSpace;
// AHelper::PrintMatrix("parent inv", m_currentSpace);
const Vector3F objectP = localP()[idx];
m_solvedT = MPoint(objectP.x, objectP.y, objectP.z) * m_currentSpace;
MTransformationMatrix mtm(m_currentSpace);
MTransformationMatrix::RotationOrder rotorder = MTransformationMatrix::kXYZ;
mtm.getRotation(m_rot, rotorder);
}
Matrix33F BaseTransform::orientation() const
{
Matrix33F r;
Vector3F angs = rotationAngles();
r.rotateEuler(angs.x, angs.y, angs.z, m_rotationOrder);
angs = rotationBaseAngles();
Matrix33F b;
b.rotateEuler(angs.x, angs.y, angs.z, m_rotationOrder);
r.multiply(b);
return r;
}
void Matrix44F::setRotation(const Matrix33F & r)
{
*m(0, 0) = r.M(0, 0);
*m(0, 1) = r.M(0, 1);
*m(0, 2) = r.M(0, 2);
*m(1, 0) = r.M(1, 0);
*m(1, 1) = r.M(1, 1);
*m(1, 2) = r.M(1, 2);
*m(2, 0) = r.M(2, 0);
*m(2, 1) = r.M(2, 1);
*m(2, 2) = r.M(2, 2);
}
Matrix44F BaseTransform::space() const
{
Matrix44F s;
s.setTranslation(m_translation);
Matrix33F r = orientation();
s.translate(m_rotatePivotTranslate);
s.translate(m_rotatePivot);
s.translate(r.transform(m_rotatePivot.reversed()));
s.translate(r.transform(m_scalePivotTranslate));
s.translate(r.transform(m_scalePivot));
Vector3F displaceByScaling = m_scalePivot.reversed();
displaceByScaling = displaceByScaling * m_scale;
s.translate(r.transform(displaceByScaling));
Matrix33F scaleMatrix;
*scaleMatrix.m(0, 0) = m_scale.x;
*scaleMatrix.m(1, 1) = m_scale.y;
*scaleMatrix.m(2, 2) = m_scale.z;
r = scaleMatrix * r;
s.setRotation(r);
return s;
}
void BlockBccMeshBuilder::addAnchorByThreshold(ATetrahedronMesh * mesh,
unsigned istripe,
const Matrix33F & invspace,
const Vector3F & center,
float threshold,
bool isLower,
unsigned tri)
{
unsigned tetMax = mesh->numTetrahedrons() * 4;
if(istripe < indexDrifts.size() -1)
tetMax = indexDrifts[istripe + 1];
BoundingBox box;
Vector3F q;
unsigned i = indexDrifts[istripe];
unsigned j;
for(;i<tetMax;i+=4) {
unsigned * tet = mesh->tetrahedronIndices(i/4);
box.reset();
for(j=0; j< 4; j++)
box.expandBy(mesh->points()[tet[j]], 1e-3f);
q = invspace.transform(box.center() - center);
if(isLower) {
if(q.x > threshold) continue;
}
else {
if(q.x < threshold) continue;
}
for(j=0; j< 4; j++)
mesh->anchors()[tet[j]] = (1<<24 | tri);
}
}
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;
}
float MlRachis::pushToSurface(const Vector3F & wv, const Matrix33F & space)
{
Vector3F ov = space.transform(wv);
ov.normalize();
ov.y = 0.f;
ov.x += 0.05f;
ov.normalize();
float a = acos(Vector3F::ZAxis.dot(ov));
if(ov.x < 0.f) a = 0.f;
return a;
}
void BlockBccMeshBuilder::addAnchors(ATetrahedronMesh * mesh, unsigned n, KdIntersection * anchorMesh)
{
Vector3F endP[2];
float lowerDist;
unsigned anchorTri;
Vector3F anchorPnt;
float anchorX;
BoundingBox ab;
Matrix33F invspace;
bool isLowerEnd;
unsigned i;
Geometry::ClosestToPointTestResult cls;
for(i=0;i<n;i++) {
AOrientedBox & box = m_boxes[i];
invspace = box.orientation();
invspace.inverse();
// find which end of the box x-axis is closer to grow mesh
endP[0] = box.majorPoint(true);
cls.reset(endP[0], 1e8f);
anchorMesh->closestToPoint(&cls);
lowerDist = cls._distance;
anchorTri = cls._icomponent;
anchorPnt = endP[0] - box.majorVector(true) * EstimatedGroupSize * .5f;
anchorX = -box.extent().x + EstimatedGroupSize * .9f;
isLowerEnd = true;
endP[1] = box.majorPoint(false);
cls.reset(endP[1], 1e8f);
anchorMesh->closestToPoint(&cls);
if(cls._distance < lowerDist) {
anchorTri = cls._icomponent;
anchorPnt = endP[1] - box.majorVector(false) * EstimatedGroupSize * .5f;
anchorX = box.extent().x - EstimatedGroupSize * .9f;
isLowerEnd = false;
}
addAnchorByThreshold(mesh, i, invspace, box.center(),
anchorX, isLowerEnd, anchorTri);
}
// addAnchor(mesh, anchorMesh);
}
Vector3F BaseTransform::rotatePlane(RotateAxis a) const
{
Matrix33F base;
base.rotateEuler(rotationBaseAngles().x, rotationBaseAngles().y, rotationBaseAngles().z);
if(a == AZ) return base.transform(Vector3F::ZAxis);
Matrix33F r;
if(a == AY) {
r.rotateEuler(0.f, 0.f, rotationAngles().z);
r.multiply(base);
return r.transform(Vector3F::YAxis);
}
r = orientation();
return r.transform(Vector3F::XAxis);
}
void BccInterface::testBlockMesh()
{
BlockBccMeshBuilder builder;
AOrientedBox box1;
box1.setExtent(Vector3F(28.f, 14.f, 1.f));
box1.setCenter(Vector3F(0.1f, -2.f, 1.f));
AOrientedBox box2;
Matrix33F rot; rot.set(Quaternion(0.f, 1.f, 0.f, .5f));
box2.setOrientation(rot);
box2.setExtent(Vector3F(18.f, 4.f, 1.f));
box2.setCenter(Vector3F(2.1f, 13.f, -1.f));
GeometryArray boxes;
boxes.create(2);
boxes.setGeometry(&box1, 0);
boxes.setGeometry(&box2, 1);
unsigned nv, nt, ns;
builder.build(&boxes, nt, nv, ns);
std::cout<<"\n tet nt nv ns "<<nt<<" "<<nv<<" "<<ns;
m_tetMesh = new ATetrahedronMeshGroup;
m_tetMesh->create(nv, nt, ns);
builder.getResult(m_tetMesh);
}
Float3 MlRachis::matchNormal(const Vector3F & wv, const Matrix33F & space)
{
Vector3F ov = space.transform(wv);
Vector3F va = ov;
va.y = 0.f;
va.z -= 0.05f;
va.normalize();
float a = acos(Vector3F::XAxis.dot(va));
if(va.z > 0.f) a = -a;
Vector3F vb = ov;
vb.z = 0.f;
vb.normalize();
float b = acos(Vector3F::XAxis.dot(vb));
if(vb.y < 0.f) b = -b;
return Float3(a, b, 0.f);
}
Matrix33F Matrix44F::rotation() const
{
Matrix33F r;
*r.m(0, 0) = M(0, 0);
*r.m(0, 1) = M(0, 1);
*r.m(0, 2) = M(0, 2);
*r.m(1, 0) = M(1, 0);
*r.m(1, 1) = M(1, 1);
*r.m(1, 2) = M(1, 2);
*r.m(2, 0) = M(2, 0);
*r.m(2, 1) = M(2, 1);
*r.m(2, 2) = M(2, 2);
return r;
}
void MlRachis::rotateForward(const Matrix33F & space, Matrix33F & dst)
{
Matrix33F s = space;
s.multiply(dst);
dst = s;
}
void MlRachis::moveForward(const Matrix33F & space, float distance, Vector3F & dst)
{
Vector3F wv = space.transform(Vector3F::ZAxis);
wv.normalize();
dst += wv * distance;
}
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;
}
}
}
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
}
}
}
}
