static void construct(PyObject* obj_ptr, py::converter::rvalue_from_python_stage1_data* data){ void* storage=((py::converter::rvalue_from_python_storage<Quaternionr>*)(data))->storage.bytes; py::object a(py::handle<>(PySequence_GetItem(obj_ptr,0))), b(py::handle<>(PySequence_GetItem(obj_ptr,1))); if(py::extract<Vector3r>(py::object(a)).check()) new (storage) Quaternionr(AngleAxisr(py::extract<Real>(b)(),py::extract<Vector3r>(a)().normalized())); else new (storage) Quaternionr(AngleAxisr(py::extract<Real>(a)(),py::extract<Vector3r>(b)().normalized())); data->convertible=storage; }
void Membrane::updateNode(){ assert(hasRefConf()); node->pos=this->getCentroid(); // temporary orientation, just to be planar // see http://www.colorado.edu/engineering/cas/courses.d/NFEM.d/NFEM.AppC.d/NFEM.AppC.pdf Quaternionr ori0; ori0.setFromTwoVectors(Vector3r::UnitZ(),this->getNormal()); Vector6r nxy0; for(int i:{0,1,2}){ Vector3r xy0=ori0.conjugate()*(nodes[i]->pos-node->pos); #ifdef MEMBRANE_DEBUG_ROT if(xy0[2]>1e-5*(max(abs(xy0[0]),abs(xy0[1])))){ LOG_ERROR("z-coordinate is not zero for node "<<i<<": ori0="<<AngleAxisr(ori0).axis()<<" !"<<AngleAxisr(ori0).angle()<<", xy0="<<xy0<<", position in global-oriented centroid-origin CS "<<(nodes[i]->pos-node->pos).transpose()); } #else assert(xy0[2]<1e-5*(max(abs(xy0[0]),abs(xy0[1])))); // z-coord should be zero #endif nxy0.segment<2>(2*i)=xy0.head<2>(); } // compute the best fit (C.8 of the paper) // split the fraction into numerator (y) and denominator (x) to get the correct quadrant Real tanTheta3_y=refPos[0]*nxy0[1]+refPos[2]*nxy0[3]+refPos[4]*nxy0[5]-refPos[1]*nxy0[0]-refPos[3]*nxy0[2]-refPos[5]*nxy0[4]; Real tanTheta3_x=refPos.dot(nxy0); // rotation to be planar, plus rotation around plane normal to the element CR frame (read backwards) node->ori=ori0*AngleAxisr(atan2(tanTheta3_y,tanTheta3_x),Vector3r::UnitZ()); };
void TensorGlRep::render(const shared_ptr<Node>& node, const GLViewInfo* viewInfo){ const int circDiv=20; static gleDouble circ[circDiv+2][3]={}; if(circ[0][0]==0){ gleSetNumSides(10); Real step=2*M_PI/circDiv; for(int i=-1; i<circDiv+1; i++){ circ[i+1][0]=cos(i*step); circ[i+1][1]=sin(i*step); circ[i+1][2]=0; } } if(range && !skewRange) skewRange=range; Vector3r pos=node->pos+(node->hasData<GlData>()?node->getData<GlData>().dGlPos:Vector3r::Zero()); for(int i:{0,1,2}){ Vector3r color=(range?range->color(eigVal[i]):CompUtils::scalarOnColorScale(eigVal[i],-1,1)); if(isnan(color.maxCoeff())) continue; Real mxNorm=(range?range->maxAbs(eigVal[i]):1); Real len=relSz*viewInfo->sceneRadius; len*=isnan(scaleExp)?abs(eigVal[i]/mxNorm):pow(abs(eigVal[i])/mxNorm,scaleExp); glColor3v(color); Vector3r dx(len*eigVec.col(i)); // arrows which go towards each other for negative eigenvalues, and away from each other for positive ones GLUtils::GLDrawArrow(pos+(eigVal[i]>0?Vector3r::Zero():dx),pos+(eigVal[i]>0?dx:Vector3r::Zero()),color); GLUtils::GLDrawArrow(pos-(eigVal[i]>0?Vector3r::Zero():dx),pos-(eigVal[i]>0?dx:Vector3r::Zero()),color); // draw circular arrow to show skew components, in the half-height // compute it in the xy plane, transform coords instead Real maxSkew=(skewRange?skewRange->maxAbs(skew[i]):1); // if(abs(skew[i])<.05*maxSkew) continue; int nPts=int((abs(skew[i])/maxSkew)*circDiv*.5/* show max skew as .5*2π rad */-.5/*round evenly, but exclude one segment for arrow head*/); if(nPts>circDiv-2) nPts=circDiv-2; if(nPts<=0) continue; Real torRad1=(skewRelSz>0?skewRelSz:relSz)*viewInfo->sceneRadius*(abs(skew[i])/maxSkew); Real torDist=0*.3*torRad1; // draw both arcs in-plane Real torRad2=torRad1*.1; glColor3v(skewRange?skewRange->color(skew[i]):CompUtils::scalarOnColorScale(skew[i],-1,1)); for(int j:{0,1,2}){ glPushMatrix(); Eigen::Affine3d T=Eigen::Affine3d::Identity(); T.translate(pos+(j==0?1:-1)*eigVec.col(i)*torDist).rotate(Quaternionr().setFromTwoVectors(Vector3r::UnitZ(),eigVec.col(i)*(skew[i]>0?-1:1))).scale(torRad1); if(j==1) T.rotate(AngleAxisr(M_PI,Vector3r::UnitZ())); //GLUtils::setLocalCoords(pos+(j==0?1:-1)*eigVec.col(i)*torDist, glMultMatrixd(T.data()); // since we scaled coords to transform unit circle coords to our radius, we will need to scale dimensions now by 1/torRad1 glePolyCylinder(nPts+2,circ,/* use current color*/NULL,torRad2*(1/torRad1)); gleDouble headRad[]={2*torRad2*(1/torRad1),2*torRad2*(1/torRad1),0,0,0}; glePolyCone(4,(gleDouble(*)[3])&(circ[nPts-1]),/*use current color*/NULL,headRad); glPopMatrix(); } } }
void Leapfrog::leapfrogSphericalRotate(const shared_ptr<Node>& node){ Vector3r axis=node->getData<DemData>().angVel; if (axis!=Vector3r::Zero()) {//If we have an angular velocity, we make a rotation Real angle=axis.norm(); axis/=angle; Quaternionr q(AngleAxisr(angle*dt,axis)); node->ori=q*node->ori; } node->ori.normalize(); }
void Disp2DPropLoadEngine::letDisturb() { const Real& dt = scene->dt; dgamma=cos(theta*Mathr::PI/180.0)*v*dt; dh=sin(theta*Mathr::PI/180.0)*v*dt; Real Ysup = topbox->state->pos.y(); Real Ylat = leftbox->state->pos.y(); // Changes in vertical and horizontal position : topbox->state->pos += Vector3r(dgamma,dh,0); leftbox->state->pos += Vector3r(dgamma/2.0,dh/2.0,0); rightbox->state->pos += Vector3r(dgamma/2.0,dh/2.0,0); Real Ysup_mod = topbox->state->pos.y(); Real Ylat_mod = leftbox->state->pos.y(); // with the corresponding velocities : topbox->state->vel=Vector3r(dgamma/dt,dh/dt,0); leftbox->state->vel = Vector3r((dgamma/dt)/2.0,dh/(2.0*dt),0); rightbox->state->vel = Vector3r((dgamma/dt)/2.0,dh/(2.0*dt),0); // Then computation of the angle of the rotation to be done : computeAlpha(); if (alpha == Mathr::PI/2.0) // Case of the very beginning { dalpha = - atan( dgamma / (Ysup_mod -Ylat_mod) ); } else { Real A = (Ysup_mod - Ylat_mod) * 2.0*tan(alpha) / (2.0*(Ysup - Ylat) + dgamma*tan(alpha) ); dalpha = atan( (A - tan(alpha))/(1.0 + A * tan(alpha))); } Quaternionr qcorr(AngleAxisr(dalpha,Vector3r::UnitZ())); if(LOG) cout << "Quaternion associe a la rotation incrementale : " << qcorr.w() << " " << qcorr.x() << " " << qcorr.y() << " " << qcorr.z() << endl; // On applique la rotation en changeant l'orientation des plaques, leurs vang et en affectant donc alpha leftbox->state->ori = qcorr*leftbox->state->ori; leftbox->state->angVel = Vector3r(0,0,1)*dalpha/dt; rightbox->state->ori = qcorr*leftbox->state->ori; rightbox->state->angVel = Vector3r(0,0,1)*dalpha/dt; }
void Membrane::computeNodalDisplacements(Real dt, bool rotIncr){ assert(hasRefConf()); // supposes node is updated already for(int i:{0,1,2}){ Vector3r xy=node->glob2loc(nodes[i]->pos); // relative tolerance of 1e-6 was too little, in some cases?! #ifdef MEMBRANE_DEBUG_ROT if(xy[2]>1e-5*(max(abs(xy[0]),abs(xy[1])))){ LOG_ERROR("local z-coordinate is not zero for node "<<i<<": node->ori="<<AngleAxisr(node->ori).axis()<<" !"<<AngleAxisr(node->ori).angle()<<", xy="<<xy<<", position in global-oriented centroid-origin CS "<<(nodes[i]->pos-node->pos).transpose()); } #else assert(xy[2]<1e-5*(max(abs(xy[0]),abs(xy[1])))); #endif // displacements uXy.segment<2>(2*i)=xy.head<2>()-refPos.segment<2>(2*i); // rotations if(rotIncr){ // incremental Vector3r angVelL=node->glob2loc(nodes[i]->getData<DemData>().angVel); // angular velocity in element coords phiXy.segment<2>(2*i)-=dt*angVelL.head<2>(); // throw std::runtime_error("Incremental rotation (In2_ElastMat_Membrane.rotIncr) is not yet implemented properly."); } else { // from total rotation difference AngleAxisr aa(refRot[i].conjugate()*(nodes[i]->ori.conjugate()*node->ori)); /* aa sometimes gives angle close to 2π for rotations which are actually small and negative; I posted that question at http://forum.kde.org/viewtopic.php?f=74&t=110854 . Such a result is fixed by conditionally subtracting 2π: */ if(aa.angle()>M_PI) aa.angle()-=2*M_PI; Vector3r rot=Vector3r(aa.angle()*aa.axis()); // rotation vector in local coords // if(aa.angle()>3) if(rot.head<2>().squaredNorm()>3.1*3.1) LOG_WARN("Membrane's in-plane rotation in a node is > 3.1 radians, expect unstability!"); phiXy.segment<2>(2*i)=rot.head<2>(); // drilling rotation discarded #ifdef MEMBRANE_DEBUG_ROT AngleAxisr rr(refRot[i]); AngleAxisr cr(nodes[i]->ori.conjugate()*node->ori); LOG_TRACE("node "<<i<<"\n refRot : "<<rr.axis()<<" !"<<rr.angle()<<"\n currRot: "<<cr.axis()<<" !"<<cr.angle()<<"\n diffRot: "<<aa.axis()<<" !"<<aa.angle()); drill[i]=rot[2]; currRot[i]=nodes[i]->ori.conjugate()*node->ori; #endif } } };
void Law2_ScGeom6D_CohFrictPhys_CohesionMoment::go(shared_ptr<IGeom>& ig, shared_ptr<IPhys>& ip, Interaction* contact) { const Real& dt = scene->dt; const int &id1 = contact->getId1(); const int &id2 = contact->getId2(); ScGeom6D* currentContactGeometry = YADE_CAST<ScGeom6D*> (ig.get()); CohFrictPhys* currentContactPhysics = YADE_CAST<CohFrictPhys*> (ip.get()); Vector3r& shearForce = currentContactPhysics->shearForce; if (contact->isFresh(scene)) shearForce = Vector3r::Zero(); Real un = currentContactGeometry->penetrationDepth; Real Fn = currentContactPhysics->kn*(un-currentContactPhysics->unp); if (currentContactPhysics->fragile && (-Fn)> currentContactPhysics->normalAdhesion) { // BREAK due to tension scene->interactions->requestErase(contact); return; } else { if ((-Fn)> currentContactPhysics->normalAdhesion) {//normal plasticity Fn=-currentContactPhysics->normalAdhesion; currentContactPhysics->unp = un+currentContactPhysics->normalAdhesion/currentContactPhysics->kn; if (currentContactPhysics->unpMax && currentContactPhysics->unp<currentContactPhysics->unpMax) scene->interactions->requestErase(contact); return; } currentContactPhysics->normalForce = Fn*currentContactGeometry->normal; State* de1 = Body::byId(id1,scene)->state.get(); State* de2 = Body::byId(id2,scene)->state.get(); ///////////////////////// CREEP START /////////// if (shear_creep) shearForce -= currentContactPhysics->ks*(shearForce*dt/creep_viscosity); ///////////////////////// CREEP END //////////// Vector3r& shearForce = currentContactGeometry->rotate(currentContactPhysics->shearForce); const Vector3r& dus = currentContactGeometry->shearIncrement(); //Linear elasticity giving "trial" shear force shearForce -= currentContactPhysics->ks*dus; Real Fs = currentContactPhysics->shearForce.norm(); Real maxFs = currentContactPhysics->shearAdhesion; if (!currentContactPhysics->cohesionDisablesFriction || maxFs==0) maxFs += Fn*currentContactPhysics->tangensOfFrictionAngle; maxFs = std::max((Real) 0, maxFs); if (Fs > maxFs) {//Plasticity condition on shear force if (currentContactPhysics->fragile && !currentContactPhysics->cohesionBroken) { currentContactPhysics->SetBreakingState(); maxFs = max((Real) 0, Fn*currentContactPhysics->tangensOfFrictionAngle); } maxFs = maxFs / Fs; Vector3r trialForce=shearForce; shearForce *= maxFs; Real dissip=((1/currentContactPhysics->ks)*(trialForce-shearForce))/*plastic disp*/ .dot(shearForce)/*active force*/; if(dissip>0) scene->energy->add(dissip,"plastDissip",plastDissipIx,/*reset*/false); if (Fn<0) currentContactPhysics->normalForce = Vector3r::Zero();//Vector3r::Zero() } applyForceAtContactPoint(-currentContactPhysics->normalForce-shearForce, currentContactGeometry->contactPoint, id1, de1->se3.position, id2, de2->se3.position); /// Moment law /// if (currentContactPhysics->momentRotationLaw && (!currentContactPhysics->cohesionBroken || always_use_moment_law)) { if (!useIncrementalForm){ if (twist_creep) { Real viscosity_twist = creep_viscosity * std::pow((2 * std::min(currentContactGeometry->radius1,currentContactGeometry->radius2)),2) / 16.0; Real angle_twist_creeped = currentContactGeometry->getTwist() * (1 - dt/viscosity_twist); Quaternionr q_twist(AngleAxisr(currentContactGeometry->getTwist(),currentContactGeometry->normal)); Quaternionr q_twist_creeped(AngleAxisr(angle_twist_creeped,currentContactGeometry->normal)); Quaternionr q_twist_delta(q_twist_creeped * q_twist.conjugate()); currentContactGeometry->twistCreep = currentContactGeometry->twistCreep * q_twist_delta; } currentContactPhysics->moment_twist = (currentContactGeometry->getTwist()*currentContactPhysics->ktw)*currentContactGeometry->normal; currentContactPhysics->moment_bending = currentContactGeometry->getBending() * currentContactPhysics->kr; } else{ // Use incremental formulation to compute moment_twis and moment_bending (no twist_creep is applied) if (twist_creep) throw std::invalid_argument("Law2_ScGeom6D_CohFrictPhys_CohesionMoment: no twis creep is included if the incremental form for the rotations is used."); Vector3r relAngVel = currentContactGeometry->getRelAngVel(de1,de2,dt); // *** Bending ***// Vector3r relAngVelBend = relAngVel - currentContactGeometry->normal.dot(relAngVel)*currentContactGeometry->normal; // keep only the bending part Vector3r relRotBend = relAngVelBend*dt; // relative rotation due to rolling behaviour // incremental formulation for the bending moment (as for the shear part) Vector3r& momentBend = currentContactPhysics->moment_bending; momentBend = currentContactGeometry->rotate(momentBend); // rotate moment vector (updated) momentBend = momentBend-currentContactPhysics->kr*relRotBend; // ---------------------------------------------------------------------------------------- // *** Torsion ***// Vector3r relAngVelTwist = currentContactGeometry->normal.dot(relAngVel)*currentContactGeometry->normal; Vector3r relRotTwist = relAngVelTwist*dt; // component of relative rotation along n FIXME: sign? // incremental formulation for the torsional moment Vector3r& momentTwist = currentContactPhysics->moment_twist; momentTwist = currentContactGeometry->rotate(momentTwist); // rotate moment vector (updated) momentTwist = momentTwist-currentContactPhysics->ktw*relRotTwist; // FIXME: sign? } /// Plasticity /// // limit rolling moment to the plastic value, if required Real RollMax = currentContactPhysics->maxRollPl*currentContactPhysics->normalForce.norm(); if (RollMax>0.){ // do we want to apply plasticity? LOG_WARN("If :yref:`CohesiveFrictionalContactLaw::useIncrementalForm` is false, then plasticity would not be applied correctly (the total formulation would not reproduce irreversibility)."); Real scalarRoll = currentContactPhysics->moment_bending.norm(); if (scalarRoll>RollMax){ // fix maximum rolling moment Real ratio = RollMax/scalarRoll; currentContactPhysics->moment_bending *= ratio; } } // limit twisting moment to the plastic value, if required Real TwistMax = currentContactPhysics->maxTwistMoment.norm(); if (TwistMax>0.){ // do we want to apply plasticity? LOG_WARN("If :yref:`CohesiveFrictionalContactLaw::useIncrementalForm` is false, then plasticity would not be applied correctly (the total formulation would not reproduce irreversibility)."); Real scalarTwist= currentContactPhysics->moment_twist.norm(); if (scalarTwist>TwistMax){ // fix maximum rolling moment Real ratio = TwistMax/scalarTwist; currentContactPhysics->moment_twist *= ratio; } } // Apply moments now Vector3r moment = currentContactPhysics->moment_twist + currentContactPhysics->moment_bending; scene->forces.addTorque(id1,-moment); scene->forces.addTorque(id2, moment); } /// Moment law END /// } }
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(); }
void setupFrameBlacksheep(Params& params) { /* Motor placement: x (2) | (0) | ------------ y | (1) | (3) | */ //set up arm lengths //dimensions are for Team Blacksheep Discovery (http://team-blacksheep.com/products/product:98) params.rotor_count = 4; std::vector<real_T> arm_lengths; Vector3r unit_z(0, 0, -1); //NED frame // relative to Forward vector in the order (0,3,1,2) required by quad X pattern // http://ardupilot.org/copter/_images/MOTORS_QuadX_QuadPlus.jpg arm_lengths.push_back(0.22f); arm_lengths.push_back(0.255f); arm_lengths.push_back(0.22f); arm_lengths.push_back(0.255f); // note: the Forward vector is actually the "x" axis, and the AngleAxisr rotation is pointing down and is left handed, so this means the rotation // is counter clockwise, so the vector (arm_lengths[i], 0) is the X-axis, so the CCW rotations to position each arm correctly are listed below: // See measurements here: http://diydrones.com/profiles/blogs/arducopter-tbs-discovery-style (angles reversed because we are doing CCW rotation) std::vector<real_T> arm_angles; arm_angles.push_back(-55.0f); arm_angles.push_back(125.0f); arm_angles.push_back(55.0f); arm_angles.push_back(-125.0f); // quad X pattern std::vector<RotorTurningDirection> rotor_directions; rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW); rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW); rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW); rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW); // data from // http://dronesvision.net/team-blacksheep-750kv-motor-esc-set-for-tbs-discovery-fpv-quadcopter/ //set up mass params.mass = 2.0f; //can be varied from 0.800 to 1.600 real_T motor_assembly_weight = 0.052f; // weight for TBS motors real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; // the props we are using a E-Prop, which I didn't find in UIUC database, but this one is close: // http://m-selig.ae.illinois.edu/props/volume-2/plots/ef_130x70_static_ctcp.png params.rotor_params.C_T = 0.11f; params.rotor_params.C_P = 0.047f; params.rotor_params.max_rpm = 9500; params.rotor_params.calculateMaxThrust(); //set up dimensions of core body box or abdomen (not including arms). params.body_box.x() = 0.20f; params.body_box.y() = 0.12f; params.body_box.z() = 0.04f; real_T rotor_z = 2.5f / 100; //computer rotor poses params.rotor_poses.clear(); for (uint i = 0; i < 4; i++) { Quaternionr angle(AngleAxisr(arm_angles[i] * M_PIf / 180, unit_z)); params.rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[i], 0, rotor_z), angle, true), unit_z, rotor_directions[i]); }; //compute inertia matrix computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); }
bool Law2_ScGeom6D_CohFrictPhys_CohesionMoment::go(shared_ptr<IGeom>& ig, shared_ptr<IPhys>& ip, Interaction* contact) { const Real& dt = scene->dt; const int &id1 = contact->getId1(); const int &id2 = contact->getId2(); ScGeom6D* geom = YADE_CAST<ScGeom6D*> (ig.get()); CohFrictPhys* phys = YADE_CAST<CohFrictPhys*> (ip.get()); Vector3r& shearForce = phys->shearForce; if (contact->isFresh(scene)) shearForce = Vector3r::Zero(); Real un = geom->penetrationDepth; Real Fn = phys->kn*(un-phys->unp); if (phys->fragile && (-Fn)> phys->normalAdhesion) { // BREAK due to tension return false; } else { if ((-Fn)> phys->normalAdhesion) {//normal plasticity Fn=-phys->normalAdhesion; phys->unp = un+phys->normalAdhesion/phys->kn; if (phys->unpMax>=0 && -phys->unp>phys->unpMax) // Actually unpMax should be defined as a function of the average particule sizes for instance return false; } phys->normalForce = Fn*geom->normal; State* de1 = Body::byId(id1,scene)->state.get(); State* de2 = Body::byId(id2,scene)->state.get(); ///////////////////////// CREEP START /////////// if (shear_creep) shearForce -= phys->ks*(shearForce*dt/creep_viscosity); ///////////////////////// CREEP END //////////// Vector3r& shearForce = geom->rotate(phys->shearForce); const Vector3r& dus = geom->shearIncrement(); //Linear elasticity giving "trial" shear force shearForce -= phys->ks*dus; Real Fs = phys->shearForce.norm(); Real maxFs = phys->shearAdhesion; if (!phys->cohesionDisablesFriction || maxFs==0) maxFs += Fn*phys->tangensOfFrictionAngle; maxFs = std::max((Real) 0, maxFs); if (Fs > maxFs) {//Plasticity condition on shear force if (phys->fragile && !phys->cohesionBroken) { phys->SetBreakingState(); maxFs = max((Real) 0, Fn*phys->tangensOfFrictionAngle); } maxFs = maxFs / Fs; Vector3r trialForce=shearForce; shearForce *= maxFs; if (scene->trackEnergy || traceEnergy){ Real sheardissip=((1/phys->ks)*(trialForce-shearForce))/*plastic disp*/ .dot(shearForce)/*active force*/; if(sheardissip>0) { plasticDissipation+=sheardissip; if (scene->trackEnergy) scene->energy->add(sheardissip,"shearDissip",shearDissipIx,/*reset*/false);} } if (Fn<0) phys->normalForce = Vector3r::Zero();//Vector3r::Zero() } //Apply the force applyForceAtContactPoint(-phys->normalForce-shearForce, geom->contactPoint, id1, de1->se3.position, id2, de2->se3.position + (scene->isPeriodic ? scene->cell->intrShiftPos(contact->cellDist): Vector3r::Zero())); /// Moment law /// if (phys->momentRotationLaw && (!phys->cohesionBroken || always_use_moment_law)) { if (!useIncrementalForm){ if (twist_creep) { Real viscosity_twist = creep_viscosity * std::pow((2 * std::min(geom->radius1,geom->radius2)),2) / 16.0; Real angle_twist_creeped = geom->getTwist() * (1 - dt/viscosity_twist); Quaternionr q_twist(AngleAxisr(geom->getTwist(),geom->normal)); Quaternionr q_twist_creeped(AngleAxisr(angle_twist_creeped,geom->normal)); Quaternionr q_twist_delta(q_twist_creeped * q_twist.conjugate()); geom->twistCreep = geom->twistCreep * q_twist_delta; } phys->moment_twist = (geom->getTwist()*phys->ktw)*geom->normal; phys->moment_bending = geom->getBending() * phys->kr; } else{ // Use incremental formulation to compute moment_twis and moment_bending (no twist_creep is applied) if (twist_creep) throw std::invalid_argument("Law2_ScGeom6D_CohFrictPhys_CohesionMoment: no twis creep is included if the incremental form for the rotations is used."); Vector3r relAngVel = geom->getRelAngVel(de1,de2,dt); // *** Bending ***// Vector3r relAngVelBend = relAngVel - geom->normal.dot(relAngVel)*geom->normal; // keep only the bending part Vector3r relRotBend = relAngVelBend*dt; // relative rotation due to rolling behaviour // incremental formulation for the bending moment (as for the shear part) Vector3r& momentBend = phys->moment_bending; momentBend = geom->rotate(momentBend); // rotate moment vector (updated) momentBend = momentBend-phys->kr*relRotBend; // ---------------------------------------------------------------------------------------- // *** Torsion ***// Vector3r relAngVelTwist = geom->normal.dot(relAngVel)*geom->normal; Vector3r relRotTwist = relAngVelTwist*dt; // component of relative rotation along n FIXME: sign? // incremental formulation for the torsional moment Vector3r& momentTwist = phys->moment_twist; momentTwist = geom->rotate(momentTwist); // rotate moment vector (updated) momentTwist = momentTwist-phys->ktw*relRotTwist; // FIXME: sign? } /// Plasticity /// // limit rolling moment to the plastic value, if required if (phys->maxRollPl>=0.){ // do we want to apply plasticity? Real RollMax = phys->maxRollPl*phys->normalForce.norm(); if (!useIncrementalForm) LOG_WARN("If :yref:`Law2_ScGeom6D_CohFrictPhys_CohesionMoment::useIncrementalForm` is false, then plasticity will not be applied correctly (the total formulation would not reproduce irreversibility)."); Real scalarRoll = phys->moment_bending.norm(); if (scalarRoll>RollMax){ // fix maximum rolling moment Real ratio = RollMax/scalarRoll; phys->moment_bending *= ratio; if (scene->trackEnergy){ Real bendingdissip=((1/phys->kr)*(scalarRoll-RollMax)*RollMax)/*active force*/; if(bendingdissip>0) scene->energy->add(bendingdissip,"bendingDissip",bendingDissipIx,/*reset*/false);} } } // limit twisting moment to the plastic value, if required if (phys->maxTwistPl>=0.){ // do we want to apply plasticity? Real TwistMax = phys->maxTwistPl*phys->normalForce.norm(); if (!useIncrementalForm) LOG_WARN("If :yref:`Law2_ScGeom6D_CohFrictPhys_CohesionMoment::useIncrementalForm` is false, then plasticity will not be applied correctly (the total formulation would not reproduce irreversibility)."); Real scalarTwist= phys->moment_twist.norm(); if (scalarTwist>TwistMax){ // fix maximum rolling moment Real ratio = TwistMax/scalarTwist; phys->moment_twist *= ratio; if (scene->trackEnergy){ Real twistdissip=((1/phys->ktw)*(scalarTwist-TwistMax)*TwistMax)/*active force*/; if(twistdissip>0) scene->energy->add(twistdissip,"twistDissip",twistDissipIx,/*reset*/false);} } } // Apply moments now Vector3r moment = phys->moment_twist + phys->moment_bending; scene->forces.addTorque(id1,-moment); scene->forces.addTorque(id2, moment); } /// Moment law END /// } return true; }