// http://en.wikipedia.org/wiki/Inertia_tensor_of_triangle Matrix3r woo::Volumetric::triangleInertia(const Vector3r& v0, const Vector3r& v1, const Vector3r& v2){ Matrix3r V; V<<v0.transpose(),v1.transpose(),v2.transpose(); // rows! Real a=((v1-v0).cross(v2-v0)).norm(); // twice the triangle area Matrix3r S; S<<2,1,1, 1,2,1, 1,1,2; S*=(1/24.); Matrix3r C=a*V.transpose()*S*V; return Matrix3r::Identity()*C.trace()-C; };
Matrix3r MeasureCapStress::matBp_BodyGlob(Real alpha,Real wettAngle, Real radius,Vector3r vecN){ // matrix B prime (the surface tension coefficient excepted), defined at body scale (see (3.49) p.65), expressed in global framework Matrix3r Bp_BodyGlob ; Bp_BodyGlob << - pow(sin(alpha),2) * cos(alpha+wettAngle) , 0 , 0 , 0 , - pow(sin(alpha),2) * cos(alpha+wettAngle) , 0 , 0 , 0 , sin(2*alpha) * sin(alpha+wettAngle); Bp_BodyGlob *= Mathr::PI * pow(radius,2.0); Matrix3r globToLocRet = matGlobToLoc(vecN); return globToLocRet * Bp_BodyGlob * globToLocRet.transpose() ; }
Matrix3r MeasureCapStress::matLG_bridgeGlob(Real nn11,Real nn33, Vector3r vecN){ Matrix3r LG_bridgeGlob ; LG_bridgeGlob << nn11 + nn33 , 0 , 0 , // useless to write lgArea - nn11 = 2*nn11 + nn33 - nn11 0 , nn11 + nn33 , 0 , 0 , 0 , 2*nn11; // trace = 2*(2*nn11 + nn33) = 2*lgArea Matrix3r globToLocRet = matGlobToLoc(vecN); return globToLocRet * LG_bridgeGlob * globToLocRet.transpose() ; }
Matrix3r MeasureCapStress::matA_BodyGlob(Real alpha,Real radius,Vector3r vecN){ Matrix3r A_BodyGlob ; A_BodyGlob << pow( 1 - cos(alpha),2) * (2 + cos(alpha)) , 0 , 0 , 0 , pow( 1 - cos(alpha),2) * (2 + cos(alpha)) , 0 , 0 , 0 , 2 * ( 1-pow(cos(alpha),3) ); A_BodyGlob *= Mathr::PI * pow(radius,3.0)/3.0; Matrix3r globToLocRet = matGlobToLoc(vecN); return globToLocRet * A_BodyGlob * globToLocRet.transpose() ; }
// draw periodic cell, if active void Renderer::drawPeriodicCell(){ if(!scene->isPeriodic || !cell) return; glColor3v(Vector3r(1,1,0)); glPushMatrix(); const Matrix3r& hSize=scene->cell->hSize; if(scaleOn && dispScale!=Vector3r::Ones()){ const Matrix3r& refHSize(scene->cell->refHSize); Matrix3r scaledHSize; for(int i=0; i<3; i++) scaledHSize.col(i)=refHSize.col(i)+((dispScale-Vector3r::Ones()).array()*Vector3r(hSize.col(i)-refHSize.col(i)).array()).matrix(); GLUtils::Parallelepiped(scaledHSize.col(0),scaledHSize.col(1),scaledHSize.col(2)); } else { GLUtils::Parallelepiped(hSize.col(0),hSize.col(1),hSize.col(2)); } glPopMatrix(); }
// draw periodic cell, if active void OpenGLRenderer::drawPeriodicCell(){ if(!scene->isPeriodic) return; glColor3v(cellColor); glPushMatrix(); // Vector3r size=scene->cell->getSize(); const Matrix3r& hSize=scene->cell->hSize; if(dispScale!=Vector3r::Ones()){ const Matrix3r& refHSize(scene->cell->refHSize); Matrix3r scaledHSize; for(int i=0; i<3; i++) scaledHSize.col(i)=refHSize.col(i)+dispScale.cwiseProduct(Vector3r(hSize.col(i)-refHSize.col(i))); GLUtils::Parallelepiped(scaledHSize.col(0),scaledHSize.col(1),scaledHSize.col(2)); } else { GLUtils::Parallelepiped(hSize.col(0),hSize.col(1),hSize.col(2)); } glPopMatrix(); }
void woo::Sphere::lumpMassInertia(const shared_ptr<Node>& n, Real density, Real& mass, Matrix3r& I, bool& rotateOk){ if(n.get()!=nodes[0].get()) return; checkNodesHaveDemData(); rotateOk=true; Real m=(4/3.)*M_PI*pow(radius,3)*density; mass+=m; I.diagonal()+=Vector3r::Ones()*(2./5.)*m*pow(radius,2); };
void Cell::integrateAndUpdate(Real dt){ //incremental displacement gradient _trsfInc=dt*gradV; // total transformation; M = (Id+G).M = F.M trsf+=_trsfInc*trsf; _invTrsf=trsf.inverse(); if(trsfUpperTriangular) checkTrsfUpperTriangular(); // hSize contains colums with updated base vectors Matrix3r prevHsize=hSize; // remember old value if(homoDeform==HOMO_GRADV2) hSize=(Matrix3r::Identity()-gradV*dt/2.).inverse()*(Matrix3r::Identity()+gradV*dt/2.)*hSize; else hSize+=_trsfInc*hSize; pprevHsize=.5*(prevHsize+hSize); // average of previous and current value if(hSize.determinant()==0){ throw std::runtime_error("Cell is degenerate (zero volume)."); } // lengths of transformed cell vectors, skew cosines Matrix3r Hnorm; // normalized transformed base vectors for(int i=0; i<3; i++){ Vector3r base(hSize.col(i)); _size[i]=base.norm(); base/=_size[i]; //base is normalized now Hnorm(0,i)=base[0]; Hnorm(1,i)=base[1]; Hnorm(2,i)=base[2]; }; // skew cosines for(int i=0; i<3; i++){ int i1=(i+1)%3, i2=(i+2)%3; // sin between axes is cos of skew _cos[i]=(Hnorm.col(i1).cross(Hnorm.col(i2))).squaredNorm(); } // pure shear trsf: ones on diagonal _shearTrsf=Hnorm; // pure unshear transformation _unshearTrsf=_shearTrsf.inverse(); // some parts can branch depending on presence/absence of shear _hasShear=(hSize(0,1)!=0 || hSize(0,2)!=0 || hSize(1,0)!=0 || hSize(1,2)!=0 || hSize(2,0)!=0 || hSize(2,1)!=0); // OpenGL shear matrix (used frequently) fillGlShearTrsfMatrix(_glShearTrsfMatrix); if(!(homoDeform==HOMO_NONE || homoDeform==HOMO_POS || homoDeform==HOMO_VEL || homoDeform==HOMO_VEL_2ND || homoDeform==HOMO_GRADV2)) throw std::invalid_argument("Cell.homoDeform must be in {0,1,2,3,4}."); }
bool PositionBasedElasticRod::ComputeDarbouxVector(const Matrix3r& dA, const Matrix3r& dB, const float mid_edge_length, Vector3r& darboux_vector) { float factor = 1.0f + dA.col(0).dot(dB.col(0)) + dA.col(1).dot(dB.col(1)) + dA.col(2).dot(dB.col(2)); factor = 2.0f / (mid_edge_length * factor); for (int c = 0; c < 3; ++c) { const int i = permutation[c][0]; const int j = permutation[c][1]; const int k = permutation[c][2]; darboux_vector[i] = dA.col(j).dot(dB.col(k)) - dA.col(k).dot(dB.col(j)); } darboux_vector *= factor; return true; }
// ---------------------------------------------------------------------------------------------- bool PositionBasedElasticRod::ComputeMaterialFrame( const Vector3r& pA, const Vector3r& pB, const Vector3r& pG, Matrix3r& frame) { frame.col(2) = (pB - pA); frame.col(2).normalize(); frame.col(1) = (frame.col(2).cross(pG - pA)); frame.col(1).normalize(); frame.col(0) = frame.col(1).cross(frame.col(2)); // frame.col(0).normalize(); return true; }
void Membrane::setRefConf(){ if(!node) node=make_shared<Node>(); // set initial node position and orientation node->pos=this->getCentroid(); // for orientation, there is a few choices which one to pick enum {INI_CS_SIMPLE=0,INI_CS_NODE0_AT_X}; const int ini_cs=INI_CS_NODE0_AT_X; //const int ini_cs=INI_CS_SIMPLE; switch(ini_cs){ case INI_CS_SIMPLE: node->ori.setFromTwoVectors(Vector3r::UnitZ(),this->getNormal()); break; case INI_CS_NODE0_AT_X:{ Matrix3r T; T.col(0)=(nodes[0]->pos-node->pos).normalized(); // QQQ: row->col T.col(2)=this->getNormal(); T.col(1)=T.col(2).cross(T.col(0)); assert(T.col(0).dot(T.col(2))<1e-12); node->ori=Quaternionr(T); break; } }; // reference nodal positions for(int i:{0,1,2}){ // Vector3r nl=node->ori.conjugate()*(nodes[i]->pos-node->pos); // QQQ Vector3r nl=node->glob2loc(nodes[i]->pos); assert(nl[2]<1e-6*(max(abs(nl[0]),abs(nl[1])))); // z-coord should be zero refPos.segment<2>(2*i)=nl.head<2>(); } // reference nodal rotations refRot.resize(3); for(int i:{0,1,2}){ // facet node orientation minus vertex node orientation, in local frame (read backwards) /// QQQ: refRot[i]=node->ori*nodes[i]->ori.conjugate(); refRot[i]=nodes[i]->ori.conjugate()*node->ori; //LOG_WARN("refRot["<<i<<"]="<<AngleAxisr(refRot[i]).angle()<<"/"<<AngleAxisr(refRot[i]).axis().transpose()); }; // set displacements to zero uXy=phiXy=Vector6r::Zero(); #ifdef MEMBRANE_DEBUG_ROT drill=Vector3r::Zero(); currRot.resize(3); #endif // delete stiffness matrices to force their re-creating KKcst.resize(0,0); KKdkt.resize(0,0); };
void Gl1_Membrane::go(const shared_ptr<Shape>& sh, const Vector3r& shift, bool wire2, const GLViewInfo& viewInfo){ Gl1_Facet::go(sh,shift,/*don't force wire rendering*/ wire2,viewInfo); if(Renderer::fastDraw) return; Membrane& ff=sh->cast<Membrane>(); if(!ff.hasRefConf()) return; if(node){ Renderer::setNodeGlData(ff.node,false/*set refPos only for the first time*/); Renderer::renderRawNode(ff.node); if(ff.node->rep) ff.node->rep->render(ff.node,&viewInfo); #ifdef MEMBRANE_DEBUG_ROT // show what Membrane thinks the orientation of nodes is - render those midway if(ff.currRot.size()==3){ for(int i:{0,1,2}){ shared_ptr<Node> n=make_shared<Node>(); n->pos=ff.node->pos+.5*(ff.nodes[i]->pos-ff.node->pos); n->ori=(ff.currRot[i]*ff.node->ori.conjugate()).conjugate(); Renderer::renderRawNode(n); } } #endif } // draw everything in in local coords now glPushMatrix(); if(!Renderer::scaleOn){ // without displacement scaling, local orientation is easy GLUtils::setLocalCoords(ff.node->pos,ff.node->ori); } else { /* otherwise compute scaled orientation such that * +x points to the same point on the triangle (in terms of angle) * +z is normal to the facet in the GL space */ Real phi0=atan2(ff.refPos[1],ff.refPos[0]); Vector3r C=ff.getGlCentroid(); Matrix3r rot; rot.row(2)=ff.getGlNormal(); rot.row(0)=(ff.getGlVertex(0)-C).normalized(); rot.row(1)=rot.row(2).cross(rot.row(0)); Quaternionr ori=AngleAxisr(phi0,Vector3r::UnitZ())*Quaternionr(rot); GLUtils::setLocalCoords(C,ori.conjugate()); } if(refConf){ glColor3v(refColor); glLineWidth(refWd); glBegin(GL_LINE_LOOP); for(int i:{0,1,2}) glVertex3v(Vector3r(ff.refPos[2*i],ff.refPos[2*i+1],0)); glEnd(); } if(uScale!=0){ glLineWidth(uWd); for(int i:{0,1,2}) drawLocalDisplacement(ff.refPos.segment<2>(2*i),uScale*ff.uXy.segment<2>(2*i),uRange,uSplit,arrows?1:0,uWd); } if(relPhi!=0 && ff.hasBending()){ glLineWidth(phiWd); for(int i:{0,1,2}) drawLocalDisplacement(ff.refPos.segment<2>(2*i),relPhi*viewInfo.sceneRadius*ff.phiXy.segment<2>(2*i),phiRange,phiSplit,arrows?2:0,phiWd #ifdef MEMBRANE_DEBUG_ROT , relPhi*viewInfo.sceneRadius*ff.drill[i] /* show the out-of-plane component */ #endif ); } glPopMatrix(); }
Matrix3r inertiaRotate(const Matrix3r& I, const Matrix3r& T){ return T.transpose()*I*T; }
bool PositionBasedElasticRod::ComputeDarbouxGradient( const Vector3r& darboux_vector, const float length, const Matrix3r& da, const Matrix3r& db, //const Matrix3r(*dajpi)[3], const Matrix3r(*dbjpi)[3], const Matrix3r dajpi[3][3], const Matrix3r dbjpi[3][3], const Vector3r& bendAndTwistKs, Matrix3r& omega_pa, Matrix3r& omega_pb, Matrix3r& omega_pc, Matrix3r& omega_pd, Matrix3r& omega_pe ) { //float x = 1.0f + da[0] * db[0] + da[1] * db[1] + da[2] * db[2]; float x = 1.0f + da.col(0).dot(db.col(0)) + da.col(1).dot(db.col(1)) + da.col(2).dot(db.col(2)); x = 2.0f / (length * x); for (int c = 0; c < 3; ++c) { const int i = permutation[c][0]; const int j = permutation[c][1]; const int k = permutation[c][2]; // pa { Vector3r term1(0,0,0); Vector3r term2(0,0,0); Vector3r tmp(0,0,0); // first term //dj::MulVecMatrix3x3<real>(db[k](), (real(*)[3]) &dajpi[j][0], term1()); //dj::MulVecMatrix3x3<real>(db[j](), (real(*)[3]) &dajpi[k][0], tmp()); //DOUBLE CHECK !!! term1 = dajpi[j][0].transpose() * db.col(k); tmp = dajpi[k][0].transpose() * db.col(j); term1 = term1 - tmp; // second term for (int n = 0; n < 3; ++n) { //dj::MulVecMatrix3x3<real>(db[n](), (real(*)[3]) &dajpi[n][0], tmp()); //DOUBLE CHECK !!! tmp = dajpi[n][0].transpose() * db.col(n); term2 = term2 + tmp; } omega_pa.col(i) = (term1)-(0.5f * darboux_vector[i] * length) * (term2); omega_pa.col(i) *= (x * bendAndTwistKs[i]); } // pb { Vector3r term1(0, 0, 0); Vector3r term2(0, 0, 0); Vector3r tmp(0, 0, 0); // first term //dj::MulVecMatrix3x3<real>(db[k](), (real(*)[3]) &dajpi[j][1], term1()); //dj::MulVecMatrix3x3<real>(db[j](), (real(*)[3]) &dajpi[k][1], tmp()); term1 = dajpi[j][1].transpose() * db.col(k); tmp = dajpi[k][1].transpose() * db.col(j); term1 = term1 - tmp; // third term //dj::MulVecMatrix3x3<real>(da[k](), (real(*)[3]) &dbjpi[j][0], tmp()); tmp = dbjpi[j][0].transpose() * da.col(k); term1 = term1 - tmp; //dj::MulVecMatrix3x3<real>(da[j](), (real(*)[3]) &dbjpi[k][0], tmp()); tmp = dbjpi[k][0].transpose() * da.col(j); term1 = term1 + tmp; // second term for (int n = 0; n < 3; ++n) { //dj::MulVecMatrix3x3<real>(db[n](), (real(*)[3]) &dajpi[n][1], tmp()); tmp = dajpi[n][1].transpose() * db.col(n); term2 = term2 + tmp; //dj::MulVecMatrix3x3<real>(da[n](), (real(*)[3]) &dbjpi[n][0], tmp()); tmp = dbjpi[n][0].transpose() * da.col(n); term2 = term2 + tmp; } omega_pb.col(i) = (term1)-(0.5f * darboux_vector[i] * length) * (term2); omega_pb.col(i) *= (x * bendAndTwistKs[i]); } // pc { Vector3r term1(0, 0, 0); Vector3r term2(0, 0, 0); Vector3r tmp(0, 0, 0); // first term //dj::MulVecMatrix3x3<real>(da[k](), (real(*)[3]) &dbjpi[j][1], term1()); //dj::MulVecMatrix3x3<real>(da[j](), (real(*)[3]) &dbjpi[k][1], tmp()); term1 = dbjpi[j][1].transpose() * da.col(k); tmp = dbjpi[k][1].transpose() * da.col(j); term1 = term1 - tmp; // second term for (int n = 0; n < 3; ++n) { //dj::MulVecMatrix3x3<real>(da[n](), (real(*)[3]) &dbjpi[n][1], tmp()); tmp = dbjpi[n][1].transpose() * da.col(n); term2 = term2 + tmp; } omega_pc.col(i) = (term1)+(0.5f * darboux_vector[i] * length) * (term2); omega_pc.col(i) *= (-x * bendAndTwistKs[i]); } // pd { Vector3r term1(0, 0, 0); Vector3r term2(0, 0, 0); Vector3r tmp(0, 0, 0); // first term //dj::MulVecMatrix3x3<real>(db[k](), (real(*)[3]) &dajpi[j][2], term1()); //dj::MulVecMatrix3x3<real>(db[j](), (real(*)[3]) &dajpi[k][2], tmp()); term1 = dajpi[j][2].transpose() * db.col(k); tmp = dajpi[k][2].transpose() * db.col(j); term1 = term1 - tmp; // second term for (int n = 0; n < 3; ++n) { //dj::MulVecMatrix3x3<real>(db[n](), (real(*)[3]) &dajpi[n][2], tmp()); tmp = dajpi[n][2].transpose() * db.col(n); term2 = term2 + tmp; } omega_pd.col(i) = (term1)-(0.5f * darboux_vector[i] * length) * (term2); omega_pd.col(i) *= (x * bendAndTwistKs[i]); } // pe { Vector3r term1(0, 0, 0); Vector3r term2(0, 0, 0); Vector3r tmp(0, 0, 0); // first term //dj::MulVecMatrix3x3<real>(da[k](), (real(*)[3]) &dbjpi[j][2], term1()); //dj::MulVecMatrix3x3<real>(da[j](), (real(*)[3]) &dbjpi[k][2], tmp()); term1 = dbjpi[j][2].transpose() * da.col(k); tmp = dbjpi[k][2].transpose() * da.col(j); term1 -= tmp; // second term for (int n = 0; n < 3; ++n) { //WARNING!!! &dbjpi[n][2][0][0] ??? //dj::MulVecMatrix3x3<real>(da[n](), (real(*)[3]) &dbjpi[n][2][0][0], tmp()); tmp = dbjpi[n][2].transpose() * da.col(n); term2 += tmp; } omega_pe.col(i) = (term1)+(0.5f * darboux_vector[i] * length) * (term2); omega_pe.col(i) *= (-x * bendAndTwistKs[i]); } } return true; }
bool PositionBasedElasticRod::ComputeMaterialFrameDerivative( const Vector3r& p0, const Vector3r& p1, const Vector3r& p2, const Matrix3r& d, Matrix3r& d1p0, Matrix3r& d1p1, Matrix3r& d1p2, Matrix3r& d2p0, Matrix3r& d2p1, Matrix3r& d2p2, Matrix3r& d3p0, Matrix3r& d3p1, Matrix3r& d3p2) { // d3pi Vector3r p01 = p1 - p0; float length_p01 = p01.norm(); d3p0.col(0) = d.col(2)[0] * d.col(2); d3p0.col(1) = d.col(2)[1] * d.col(2); d3p0.col(2) = d.col(2)[2] * d.col(2); d3p0.col(0)[0] -= 1.0f; d3p0.col(1)[1] -= 1.0f; d3p0.col(2)[2] -= 1.0f; d3p0.col(0) *= (1.0f / length_p01); d3p0.col(1) *= (1.0f / length_p01); d3p0.col(2) *= (1.0f / length_p01); d3p1.col(0) = -d3p0.col(0); d3p1.col(1) = -d3p0.col(1); d3p1.col(2) = -d3p0.col(2); d3p2.col(0).setZero(); d3p2.col(1).setZero(); d3p2.col(2).setZero(); ////>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> //// d2pi Vector3r p02 = p2 - p0; Vector3r p01_cross_p02 = p01.cross(p02); float length_cross = p01_cross_p02.norm(); Matrix3r mat; mat.col(0) = d.col(1)[0] * d.col(1); mat.col(1) = d.col(1)[1] * d.col(1); mat.col(2) = d.col(1)[2] * d.col(1); mat.col(0)[0] -= 1.0f; mat.col(1)[1] -= 1.0f; mat.col(2)[2] -= 1.0f; mat.col(0) *= (-1.0f / length_cross); mat.col(1) *= (-1.0f / length_cross); mat.col(2) *= (-1.0f / length_cross); Matrix3r product_matrix; MathFunctions::crossProductMatrix(p2 - p1, product_matrix); d2p0 = mat * product_matrix; MathFunctions::crossProductMatrix(p0 - p2, product_matrix); d2p1 = mat * product_matrix; MathFunctions::crossProductMatrix(p1 - p0, product_matrix); d2p2 = mat * product_matrix; ////>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> //// d1pi Matrix3r product_mat_d3; Matrix3r product_mat_d2; Matrix3r m1, m2; MathFunctions::crossProductMatrix(d.col(2), product_mat_d3); MathFunctions::crossProductMatrix(d.col(1), product_mat_d2); //dj::MulMatrix3x3<real>(&product_mat_d3[0][0], &d2p0[0][0], &m1[0][0]); //dj::MulMatrix3x3<real>(&product_mat_d2[0][0], &d3p0[0][0], &m2[0][0]); d1p0 = product_mat_d2 * d3p0 - product_mat_d3 * d2p0; //dj::MulMatrix3x3<real>(&product_mat_d3[0][0], &d2p1[0][0], &m1[0][0]); //dj::MulMatrix3x3<real>(&product_mat_d2[0][0], &d3p1[0][0], &m2[0][0]); d1p1 = product_mat_d2 * d3p1 - product_mat_d3 * d2p1; /*dj::MulMatrix3x3<real>(&product_mat_d3[0][0], &d2p2[0][0], &d1p2[0][0]);*/ d1p2 = product_mat_d3 * d2p2; d1p2.col(0) *= -1.0f; d1p2.col(1) *= -1.0f; d1p2.col(2) *= -1.0f; return true; }
/*! @brief Recalculate body's inertia tensor in rotated coordinates. * * @param I inertia tensor in old coordinates * @param T rotation matrix from old to new coordinates * @return inertia tensor in new coordinates */ Matrix3r woo::Volumetric::inertiaTensorRotate(const Matrix3r& I,const Matrix3r& T){ /* [http://www.kwon3d.com/theory/moi/triten.html] */ return T.transpose()*I*T; }
/* Generic function to compute L3Geom (with colinear points), used for {sphere,facet,wall}+sphere contacts now */ void Ig2_Sphere_Sphere_L3Geom::handleSpheresLikeContact(const shared_ptr<Interaction>& I, const State& state1, const State& state2, const Vector3r& shift2, bool is6Dof, const Vector3r& normal, const Vector3r& contPt, Real uN, Real r1, Real r2){ // create geometry if(!I->geom){ if(is6Dof) I->geom=shared_ptr<L6Geom>(new L6Geom); else I->geom=shared_ptr<L3Geom>(new L3Geom); L3Geom& g(I->geom->cast<L3Geom>()); g.contactPoint=contPt; g.refR1=r1; g.refR2=r2; g.normal=normal; // g.trsf.setFromTwoVectors(Vector3r::UnitX(),g.normal); // quaternion just from the X-axis; does not seem to work for some reason?! const Vector3r& locX(g.normal); // initial local y-axis orientation, in the xz or xy plane, depending on which component is larger to avoid singularities Vector3r locY=normal.cross(abs(normal[1])<abs(normal[2])?Vector3r::UnitY():Vector3r::UnitZ()); locY-=locX*locY.dot(locX); locY.normalize(); Vector3r locZ=normal.cross(locY); #ifdef L3_TRSF_QUATERNION Matrix3r trsf; trsf.row(0)=locX; trsf.row(1)=locY; trsf.row(2)=locZ; g.trsf=Quaternionr(trsf); // from transformation matrix #else g.trsf.row(0)=locX; g.trsf.row(1)=locY; g.trsf.row(2)=locZ; #endif g.u=Vector3r(uN,0,0); // zero shear displacement if(distFactor<0) g.u0[0]=uN; // L6Geom::phi is initialized to Vector3r::Zero() automatically //cerr<<"Init trsf=\n"<<g.trsf<<endl<<"locX="<<locX<<", locY="<<locY<<", locZ="<<locZ<<endl; return; } // update geometry /* motion of the conctact consists in rigid motion (normRotVec, normTwistVec) and mutual motion (relShearDu); they are used to update trsf and u */ L3Geom& g(I->geom->cast<L3Geom>()); const Vector3r& currNormal(normal); const Vector3r& prevNormal(g.normal); // normal rotation vector, between last steps Vector3r normRotVec=prevNormal.cross(currNormal); // contrary to what ScGeom::precompute does now (r2486), we take average normal, i.e. .5*(prevNormal+currNormal), // so that all terms in the equation are in the previous mid-step // the re-normalization might not be necessary for very small increments, but better do it Vector3r avgNormal=(approxMask&APPROX_NO_MID_NORMAL) ? prevNormal : .5*(prevNormal+currNormal); if(!(approxMask&APPROX_NO_RENORM_MID_NORMAL) && !(approxMask&APPROX_NO_MID_NORMAL)) avgNormal.normalize(); // normalize only if used and if requested via approxMask // twist vector of the normal from the last step Vector3r normTwistVec=avgNormal*scene->dt*.5*avgNormal.dot(state1.angVel+state2.angVel); // compute relative velocity // noRatch: take radius or current distance as the branch vector; see discussion in ScGeom::precompute (avoidGranularRatcheting) Vector3r c1x=((noRatch && !r1>0) ? ( r1*normal).eval() : (contPt-state1.pos).eval()); // used only for sphere-sphere Vector3r c2x=(noRatch ? (-r2*normal).eval() : (contPt-state2.pos+shift2).eval()); //Vector3r state2velCorrected=state2.vel+(scene->isPeriodic?scene->cell->intrShiftVel(I->cellDist):Vector3r::Zero()); // velocity of the second particle, corrected with meanfield velocity if necessary //cerr<<"correction "<<(scene->isPeriodic?scene->cell->intrShiftVel(I->cellDist):Vector3r::Zero())<<endl; Vector3r relShearVel=(state2.vel+state2.angVel.cross(c2x))-(state1.vel+state1.angVel.cross(c1x)); // account for relative velocity of particles in different cell periods if(scene->isPeriodic) relShearVel+=scene->cell->intrShiftVel(I->cellDist); relShearVel-=avgNormal.dot(relShearVel)*avgNormal; Vector3r relShearDu=relShearVel*scene->dt; /* Update of quantities in global coords consists in adding 3 increments we have computed; in global coords (a is any vector) 1. +relShearVel*scene->dt; // mutual motion of the contact 2. -a.cross(normRotVec); // rigid rotation perpendicular to the normal 3. -a.cross(normTwistVec); // rigid rotation parallel to the normal */ // compute current transformation, by updating previous axes // the X axis can be prescribed directly (copy of normal) // the mutual motion on the contact does not change transformation #ifdef L3_TRSF_QUATERNION const Matrix3r prevTrsf(g.trsf.toRotationMatrix()); Quaternionr prevTrsfQ(g.trsf); #else const Matrix3r prevTrsf(g.trsf); // could be reference perhaps, but we need it to compute midTrsf (if applicable) #endif Matrix3r currTrsf; currTrsf.row(0)=currNormal; for(int i=1; i<3; i++){ currTrsf.row(i)=prevTrsf.row(i)-prevTrsf.row(i).cross(normRotVec)-prevTrsf.row(i).cross(normTwistVec); } #ifdef L3_TRSF_QUATERNION Quaternionr currTrsfQ(currTrsf); if((scene->iter % trsfRenorm)==0 && trsfRenorm>0) currTrsfQ.normalize(); #else if((scene->iter % trsfRenorm)==0 && trsfRenorm>0){ #if 1 currTrsf.row(0).normalize(); currTrsf.row(1)-=currTrsf.row(0)*currTrsf.row(1).dot(currTrsf.row(0)); // take away y projected on x, to stabilize numerically currTrsf.row(1).normalize(); currTrsf.row(2)=currTrsf.row(0).cross(currTrsf.row(1)); currTrsf.row(2).normalize(); #else currTrsf=Matrix3r(Quaternionr(currTrsf).normalized()); #endif #ifdef YADE_DEBUG if(abs(currTrsf.determinant()-1)>.05){ LOG_ERROR("##"<<I->getId1()<<"+"<<I->getId2()<<", |trsf|="<<currTrsf.determinant()); g.trsf=currTrsf; throw runtime_error("Transformation matrix far from orthonormal."); } #endif } #endif /* Previous local trsf u'⁻ must be updated to current u'⁰. We have transformation T⁻ and T⁰, δ(a) denotes increment of a as defined above. Two possibilities: 1. convert to global, update, convert back: T⁰(T⁻*(u'⁻)+δ(T⁻*(u'⁻))). Quite heavy. 2. update u'⁻ straight, using relShearVel in local coords; since relShearVel is computed at (t-Δt/2), we would have to find intermediary transformation (same axis, half angle; the same as slerp at t=.5 between the two). This could be perhaps simplified by using T⁰ or T⁻ since they will not differ much, but it would have to be verified somehow. */ // if requested via approxMask, just use prevTrsf #ifdef L3_TRSF_QUATERNION Quaternionr midTrsf=(approxMask&APPROX_NO_MID_TRSF) ? prevTrsfQ : prevTrsfQ.slerp(.5,currTrsfQ); #else Quaternionr midTrsf=(approxMask&APPROX_NO_MID_TRSF) ? Quaternionr(prevTrsf) : Quaternionr(prevTrsf).slerp(.5,Quaternionr(currTrsf)); #endif //cerr<<"prevTrsf=\n"<<prevTrsf<<", currTrsf=\n"<<currTrsf<<", midTrsf=\n"<<Matrix3r(midTrsf)<<endl; // updates of geom here // midTrsf*relShearVel should have the 0-th component (approximately) zero -- to be checked g.u+=midTrsf*relShearDu; //cerr<<"midTrsf=\n"<<midTrsf<<",relShearDu="<<relShearDu<<", transformed "<<midTrsf*relShearDu<<endl; g.u[0]=uN; // this does not have to be computed incrementally #ifdef L3_TRSF_QUATERNION g.trsf=currTrsfQ; #else g.trsf=currTrsf; #endif // GenericSpheresContact g.refR1=r1; g.refR2=r2; g.normal=currNormal; g.contactPoint=contPt; if(is6Dof){ // update phi, from the difference of angular velocities // the difference is transformed to local coord using the midTrsf transformation // perhaps not consistent when spheres have different radii (depends how bending moment is computed) I->geom->cast<L6Geom>().phi+=midTrsf*(scene->dt*(state2.angVel-state1.angVel)); } };