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 } } } }