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