1
// © 2013 Jan Elias, http://www.fce.vutbr.cz/STM/elias.j/, elias.j@fce.vutbr.cz
2
// https://www.vutbr.cz/www_base/gigadisk.php?i=95194aa9a
6
#include<yade/core/Interaction.hpp>
7
#include<yade/core/Omega.hpp>
8
#include<yade/core/Scene.hpp>
9
#include<yade/core/State.hpp>
10
#include<yade/pkg/common/ElastMat.hpp>
11
#include<yade/pkg/common/Aabb.hpp>
12
#include"Polyhedra.hpp"
14
#define _USE_MATH_DEFINES
16
YADE_PLUGIN(/* self-contained in hpp: */ (Polyhedra) (PolyhedraGeom) (Bo1_Polyhedra_Aabb) (PolyhedraPhys) (PolyhedraMat) (Ip2_PolyhedraMat_PolyhedraMat_PolyhedraPhys) (PolyhedraVolumetricLaw)
17
/* some code in cpp (this file): */
23
//*********************************************************************************
24
/* Polyhedra Constructor */
26
void Polyhedra::Initialize(){
30
bool isRandom = false;
33
int N = (int) v.size();
36
while ((int) v.size()<4) GenerateRandomGeometry();
41
//compute convex hull of vertices
42
std::vector<CGALpoint> points;
43
points.resize(v.size());
44
for(int i=0;i<N;i++) {
45
points[i] = CGALpoint(v[i][0],v[i][1],v[i][2]);
48
CGAL::convex_hull_3(points.begin(), points.end(), P);
50
//connect triagular facets if possible
51
std::transform(P.facets_begin(), P.facets_end(), P.planes_begin(),Plane_equation());
52
P = Simplify(P, 1E-9);
54
//modify order of v according to CGAl polyhedron
57
for (Polyhedron::Vertex_iterator vIter = P.vertices_begin(); vIter != P.vertices_end(); ++vIter, i++){
58
v.push_back(Vector3r(vIter->point().x(),vIter->point().y(),vIter->point().z()));
61
//list surface triangles for plotting
63
std::transform(P.facets_begin(), P.facets_end(), P.planes_begin(),Plane_equation());
64
for (Polyhedron::Facet_iterator fIter = P.facets_begin(); fIter != P.facets_end(); fIter++){
65
Polyhedron::Halfedge_around_facet_circulator hfc0;
66
int n = fIter->facet_degree();
67
hfc0 = fIter->facet_begin();
68
int a = std::distance(P.vertices_begin(), hfc0->vertex());
69
for (int i=2; i<n; i++){
72
faceTri.push_back(std::distance(P.vertices_begin(), hfc0->vertex()));
73
faceTri.push_back(std::distance(P.vertices_begin(), hfc0->next()->vertex()));
77
//compute centroid and volume
78
P_volume_centroid(P, &volume, ¢roid);
79
//check vierd behavior of CGAL in tessalation
80
if(isRandom && volume*1.75<4./3.*3.14*size[0]/2.*size[1]/2.*size[2]/2.) {
85
Vector3r translation((-1)*centroid);
87
//set centroid to be [0,0,0]
88
for(int i=0;i<N;i++) {
91
if(isRandom) centroid = Vector3r::Zero();
93
Vector3r origin(0,0,0);
95
//move and rotate also the CGAL structure Polyhedron
96
Transformation t_trans(1.,0.,0.,translation[0],0.,1.,0.,translation[1],0.,0.,1.,translation[2],1.);
97
std::transform( P.points_begin(), P.points_end(), P.points_begin(), t_trans);
102
Matrix3r Itet1, Itet2;
103
Matrix3r inertia_tensor(Matrix3r::Zero());
104
for(int i=0; i<(int) faceTri.size(); i+=3){
105
vtet = std::abs((origin-v[faceTri[i+2]]).dot((v[faceTri[i]]-v[faceTri[i+2]]).cross(v[faceTri[i+1]]-v[faceTri[i+2]]))/6.);
106
ctet = (origin+v[faceTri[i]]+v[faceTri[i+1]]+v[faceTri[i+2]]) / 4.;
107
Itet1 = TetraInertiaTensor(origin-ctet, v[faceTri[i]]-ctet, v[faceTri[i+1]]-ctet, v[faceTri[i+2]]-ctet);
110
ctet[1]*ctet[1]+ctet[2]*ctet[2], -ctet[0]*ctet[1], -ctet[0]*ctet[2],
111
-ctet[0]*ctet[1], ctet[0]*ctet[0]+ctet[2]*ctet[2], -ctet[2]*ctet[1],
112
-ctet[0]*ctet[2], -ctet[2]*ctet[1], ctet[1]*ctet[1]+ctet[0]*ctet[0];
113
inertia_tensor = inertia_tensor + Itet1 + Itet2*vtet;
116
if(abs(inertia_tensor(0,1))+abs(inertia_tensor(0,2))+abs(inertia_tensor(1,2)) < 1E-13){
117
// no need to rotate, inertia already diagonal
118
orientation = Quaternionr::Identity();
119
inertia = Vector3r(inertia_tensor(0,0),inertia_tensor(1,1),inertia_tensor(2,2));
121
// calculate eigenvectors of I
123
Matrix3r I_rot(Matrix3r::Zero()), I_new(Matrix3r::Zero());
124
matrixEigenDecomposition(inertia_tensor,I_rot,I_new);
125
// I_rot = eigenvectors of inertia_tensors in columns
126
// I_new = eigenvalues on diagonal
127
// set positove direction of vectors - otherwise reloading does not work
128
Matrix3r sign(Matrix3r::Zero());
129
double max_v_signed = I_rot(0,0);
130
double max_v = abs(I_rot(0,0));
131
if (max_v < abs(I_rot(1,0))) {max_v_signed = I_rot(1,0); max_v = abs(I_rot(1,0));}
132
if (max_v < abs(I_rot(2,0))) {max_v_signed = I_rot(2,0); max_v = abs(I_rot(2,0));}
133
sign(0,0) = max_v_signed/max_v;
134
max_v_signed = I_rot(0,1);
135
max_v = abs(I_rot(0,1));
136
if (max_v < abs(I_rot(1,1))) {max_v_signed = I_rot(1,1); max_v = abs(I_rot(1,1));}
137
if (max_v < abs(I_rot(2,1))) {max_v_signed = I_rot(2,1); max_v = abs(I_rot(2,1));}
138
sign(1,1) = max_v_signed/max_v;
141
// force the eigenvectors to be right-hand oriented
142
Vector3r third = (I_rot.col(0)).cross(I_rot.col(1));
143
I_rot(0,2) = third[0];
144
I_rot(1,2) = third[1];
145
I_rot(2,2) = third[2];
148
inertia = Vector3r(I_new(0,0),I_new(1,1),I_new(2,2));
149
orientation = Quaternionr(I_rot);
150
//rotate the voronoi cell so that x - is maximal inertia axis and z - is minimal inertia axis
151
//orientation.normalize(); //not needed
152
for(int i=0; i< (int) v.size();i++) {
153
v[i] = orientation.conjugate()*v[i];
156
//rotate also the CGAL structure Polyhedron
157
Matrix3r rot_mat = (orientation.conjugate()).toRotationMatrix();
158
Transformation t_rot(rot_mat(0,0),rot_mat(0,1),rot_mat(0,2),rot_mat(1,0),rot_mat(1,1),rot_mat(1,2),rot_mat(2,0),rot_mat(2,1),rot_mat(2,2),1.);
159
std::transform( P.points_begin(), P.points_end(), P.points_begin(), t_rot);
162
//initialization done
166
//**************************************************************************
167
/* Generator of randomly shaped polyhedron based on Voronoi tessellation*/
169
void Polyhedra::GenerateRandomGeometry(){
172
vector<CGALpoint> nuclei;
173
nuclei.push_back(CGALpoint(5.,5.,5.));
177
//fill box 5x5x5 with randomly located nuclei with restricted minimal mutual distance 0.75 which gives approximate mean mutual distance 1;
178
double dist_min2 = 0.75*0.75;
182
trial = CGALpoint(double(rand())/RAND_MAX*5.+2.5,double(rand())/RAND_MAX*5.+2.5,double(rand())/RAND_MAX*5.+2.5);
183
for(int i=0;i< (int) nuclei.size();i++) {
184
isOK = pow(nuclei[i].x()-trial.x(),2)+pow(nuclei[i].y()-trial.y(),2)+pow(nuclei[i].z()-trial.z(),2) > dist_min2;
190
nuclei.push_back(trial);
195
//perform Voronoi tessellation
196
nuclei.erase(nuclei.begin());
197
Triangulation dt(nuclei.begin(), nuclei.end());
198
Triangulation::Vertex_handle zero_point = dt.insert(CGALpoint(5.,5.,5.));
200
std::vector<Triangulation::Cell_handle> ch_cells;
201
dt.incident_cells(zero_point,std::back_inserter(ch_cells));
202
for(std::vector<Triangulation::Cell_handle>::iterator ci = ch_cells.begin(); ci !=ch_cells.end(); ++ci){
203
v.push_back(FromCGALPoint(dt.dual(*ci))-Vector3r(5.,5.,5.));
207
//resize and rotate the voronoi cell
208
Quaternionr Rot(double(rand())/RAND_MAX,double(rand())/RAND_MAX,double(rand())/RAND_MAX,double(rand())/RAND_MAX);
210
for(int i=0; i< (int) v.size();i++) {
211
v[i] = Rot*(Vector3r(v[i][0]*size[0],v[i][1]*size[1],v[i][2]*size[2]));
214
//to avoid patological cases (that should not be present, but CGAL works somehow unpredicable)
216
cout << "wrong " << v.size() << endl;
219
GenerateRandomGeometry();
223
//****************************************************************************************
226
Polyhedra::~Polyhedra(){}
228
//****************************************************************************************
231
PolyhedraGeom::~PolyhedraGeom(){}
233
//****************************************************************************************
234
/* AaBb overlap checker */
236
void Bo1_Polyhedra_Aabb::go(const shared_ptr<Shape>& ig, shared_ptr<Bound>& bv, const Se3r& se3, const Body*){
238
Polyhedra* t=static_cast<Polyhedra*>(ig.get());
239
if (!t->IsInitialized()) t->Initialize();
240
if(!bv){ bv=shared_ptr<Bound>(new Aabb); }
241
Aabb* aabb=static_cast<Aabb*>(bv.get());
242
//Quaternionr invRot=se3.orientation.conjugate();
243
int N = (int) t->v.size();
244
Vector3r v_g, mincoords(0.,0.,0.), maxcoords(0.,0.,0.);
245
for(int i=0; i<N; i++) {
246
v_g=se3.orientation*t->v[i]; // vertices in global coordinates
247
mincoords = Vector3r(min(mincoords[0],v_g[0]),min(mincoords[1],v_g[1]),min(mincoords[2],v_g[2]));
248
maxcoords = Vector3r(max(maxcoords[0],v_g[0]),max(maxcoords[1],v_g[1]),max(maxcoords[2],v_g[2]));
250
aabb->min=se3.position+mincoords;
251
aabb->max=se3.position+maxcoords;
254
//**********************************************************************************
258
#include<yade/lib/opengl/OpenGLWrapper.hpp>
259
void Gl1_Polyhedra::go(const shared_ptr<Shape>& cm, const shared_ptr<State>&,bool,const GLViewInfo&)
261
glMaterialv(GL_FRONT,GL_AMBIENT_AND_DIFFUSE,Vector3r(cm->color[0],cm->color[1],cm->color[2]));
262
glColor3v(cm->color);
263
Polyhedra* t=static_cast<Polyhedra*>(cm.get());
264
vector<int> faceTri = t->GetSurfaceTriangulation();
267
glDisable(GL_LIGHTING);
269
#define __ONEWIRE(a,b) glVertex3v(t->v[a]);glVertex3v(t->v[b])
270
for(int tri=0; tri < (int) faceTri.size(); tri+=3) {__ONEWIRE(faceTri[tri],faceTri[tri+1]); __ONEWIRE(faceTri[tri],faceTri[tri+2]); __ONEWIRE(faceTri[tri+1],faceTri[tri+2]);}
276
Vector3r centroid = t->GetCentroid();
277
Vector3r faceCenter, n;
278
glDisable(GL_CULL_FACE); glEnable(GL_LIGHTING);
279
glBegin(GL_TRIANGLES);
280
#define __ONEFACE(a,b,c) n=(t->v[b]-t->v[a]).cross(t->v[c]-t->v[a]); n.normalize(); faceCenter=(t->v[a]+t->v[b]+t->v[c])/3.; if((faceCenter-centroid).dot(n)<0)n=-n; glNormal3v(n); glVertex3v(t->v[a]); glVertex3v(t->v[b]); glVertex3v(t->v[c]);
281
for(int tri=0; tri < (int) faceTri.size(); tri+=3) {__ONEFACE(faceTri[tri],faceTri[tri+1],faceTri[tri+2]);}
288
//**********************************************************************************
289
//!Precompute data needed for rotating tangent vectors attached to the interaction
291
void PolyhedraGeom::precompute(const State& rbp1, const State& rbp2, const Scene* scene, const shared_ptr<Interaction>& c, const Vector3r&
292
currentNormal, bool isNew, const Vector3r& shift2){
295
orthonormal_axis = normal.cross(currentNormal);
296
Real angle = scene->dt*0.5*normal.dot(rbp1.angVel + rbp2.angVel);
297
twist_axis = angle*normal;}
298
else twist_axis=orthonormal_axis=Vector3r::Zero();
299
//Update contact normal
300
normal=currentNormal;
301
//Precompute shear increment
302
Vector3r c1x = (contactPoint - rbp1.pos);
303
Vector3r c2x = (contactPoint - rbp2.pos + shift2);
304
Vector3r relativeVelocity = (rbp2.vel+rbp2.angVel.cross(c2x)) - (rbp1.vel+rbp1.angVel.cross(c1x));
305
//keep the shear part only
306
relativeVelocity = relativeVelocity-normal.dot(relativeVelocity)*normal;
307
shearInc = relativeVelocity*scene->dt;
310
//**********************************************************************************
311
Vector3r& PolyhedraGeom::rotate(Vector3r& shearForce) const {
312
// approximated rotations
313
shearForce -= shearForce.cross(orthonormal_axis);
314
shearForce -= shearForce.cross(twist_axis);
315
//NOTE : make sure it is in the tangent plane? It's never been done before. Is it not adding rounding errors at the same time in fact?...
316
shearForce -= normal.dot(shearForce)*normal;
321
//**********************************************************************************
322
/* Material law, physics */
324
void Ip2_PolyhedraMat_PolyhedraMat_PolyhedraPhys::go( const shared_ptr<Material>& b1
325
, const shared_ptr<Material>& b2
326
, const shared_ptr<Interaction>& interaction)
328
if(interaction->phys) return;
329
const shared_ptr<PolyhedraMat>& mat1 = YADE_PTR_CAST<PolyhedraMat>(b1);
330
const shared_ptr<PolyhedraMat>& mat2 = YADE_PTR_CAST<PolyhedraMat>(b2);
331
interaction->phys = shared_ptr<PolyhedraPhys>(new PolyhedraPhys());
332
const shared_ptr<PolyhedraPhys>& contactPhysics = YADE_PTR_CAST<PolyhedraPhys>(interaction->phys);
337
Real frictionAngle = std::min(mat1->frictionAngle,mat2->frictionAngle);
338
contactPhysics->tangensOfFrictionAngle = std::tan(frictionAngle);
339
contactPhysics->kn = Kna*Knb/(Kna+Knb);
340
contactPhysics->ks = Ksa*Ksb/(Ksa+Ksb);
343
//**************************************************************************************
345
Real PolyhedraVolumetricLaw::getPlasticDissipation() {return (Real) plasticDissipation;}
346
void PolyhedraVolumetricLaw::initPlasticDissipation(Real initVal) {plasticDissipation.reset(); plasticDissipation+=initVal;}
347
Real PolyhedraVolumetricLaw::elasticEnergy()
350
FOREACH(const shared_ptr<Interaction>& I, *scene->interactions){
351
if(!I->isReal()) continue;
352
FrictPhys* phys = dynamic_cast<FrictPhys*>(I->phys.get());
354
energy += 0.5*(phys->normalForce.squaredNorm()/phys->kn + phys->shearForce.squaredNorm()/phys->ks);}
361
//**************************************************************************************
362
// Apply forces on polyhedrons in collision based on geometric configuration
363
void PolyhedraVolumetricLaw::go(shared_ptr<IGeom>& ig, shared_ptr<IPhys>& ip, Interaction* I){
365
if (!I->geom) {I->phys=shared_ptr<IPhys>(); return;}
366
const shared_ptr<PolyhedraGeom>& contactGeom(dynamic_pointer_cast<PolyhedraGeom>(I->geom));
367
if(!contactGeom) {I->phys=shared_ptr<IPhys>(); return;}
368
const Body::id_t idA=I->getId1(), idB=I->getId2();
369
const shared_ptr<Body>& A=Body::byId(idA), B=Body::byId(idB);
371
PolyhedraPhys* phys = dynamic_cast<PolyhedraPhys*>(I->phys.get());
373
//erase the interaction when aAbB shows separation, otherwise keep it to be able to store previous separating plane for fast detection of separation
374
if (A->bound->min[0] >= B->bound->max[0] || B->bound->min[0] >= A->bound->max[0] || A->bound->min[1] >= B->bound->max[1] || B->bound->min[1] >= A->bound->max[1] || A->bound->min[2] >= B->bound->max[2] || B->bound->min[2] >= A->bound->max[2]) {
375
scene->interactions->requestErase(I);
379
//zero penetration depth means no interaction force
380
if(!(contactGeom->penetrationVolume > 0) ) {I->phys=shared_ptr<IPhys>(); return;}
381
Vector3r normalForce=contactGeom->normal*contactGeom->penetrationVolume*phys->kn;
383
//shear force: in case the polyhdras are separated and come to contact again, one should not use the previous shear force
384
if (contactGeom->isShearNew)
385
shearForce = Vector3r::Zero();
387
shearForce = contactGeom->rotate(shearForce);
389
const Vector3r& shearDisp = contactGeom->shearInc;
390
shearForce -= phys->ks*shearDisp;
392
Real maxFs = normalForce.squaredNorm()*std::pow(phys->tangensOfFrictionAngle,2);
394
if (likely(!scene->trackEnergy && !traceEnergy)){//Update force but don't compute energy terms (see below))
395
// PFC3d SlipModel, is using friction angle. CoulombCriterion
396
if( shearForce.squaredNorm() > maxFs ){
397
Real ratio = sqrt(maxFs) / shearForce.norm();
398
shearForce *= ratio;}
400
//almost the same with additional Vector3r instatinated for energy tracing,
401
//duplicated block to make sure there is no cost for the instanciation of the vector when traceEnergy==false
402
if(shearForce.squaredNorm() > maxFs){
403
Real ratio = sqrt(maxFs) / shearForce.norm();
404
Vector3r trialForce=shearForce;//store prev force for definition of plastic slip
405
//define the plastic work input and increment the total plastic energy dissipated
407
Real dissip=((1/phys->ks)*(trialForce-shearForce)).dot(shearForce);
408
if (traceEnergy) plasticDissipation += dissip;
409
else if(dissip>0) scene->energy->add(dissip,"plastDissip",plastDissipIx,false);
411
// compute elastic energy as well
412
scene->energy->add(0.5*(normalForce.squaredNorm()/phys->kn+shearForce.squaredNorm()/phys->ks),"elastPotential",elastPotentialIx,true);
415
Vector3r F = -normalForce-shearForce;
416
if (contactGeom->equivalentPenetrationDepth != contactGeom->equivalentPenetrationDepth) exit(1);
417
scene->forces.addForce (idA,F);
418
scene->forces.addForce (idB, -F);
419
scene->forces.addTorque(idA, -(A->state->pos-contactGeom->contactPoint).cross(F));
420
scene->forces.addTorque(idB, (B->state->pos-contactGeom->contactPoint).cross(F));
422
//needed to be able to acces interaction forces in other parts of yade
423
phys->normalForce = normalForce;
424
phys->shearForce = shearForce;
added
removed
pkg/dem/Polyhedra.cpp
