1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
|
// © 2013 Jan Elias, http://www.fce.vutbr.cz/STM/elias.j/, elias.j@fce.vutbr.cz
// https://www.vutbr.cz/www_base/gigadisk.php?i=95194aa9a
#ifdef YADE_CGAL
// NDEBUG causes crashes in CGAL sometimes. Anton
#ifdef NDEBUG
#undef NDEBUG
#endif
#include "Polyhedra.hpp"
#ifdef YADE_OPENGL
#include <lib/opengl/OpenGLWrapper.hpp>
#include <preprocessing/dem/Shop.hpp>
#endif
namespace yade { // Cannot have #include directive inside.
using math::max;
using math::min; // using inside .cpp file is ok.
#ifdef YADE_OPENGL
YADE_PLUGIN((Gl1_Polyhedra)(Gl1_PolyhedraGeom)(Gl1_PolyhedraPhys));
#endif
//****************************************************************************************
/* Destructor */
Polyhedra::~Polyhedra() { }
//****************************************************************************************
Real PolyhedraMat::GetStrength() const { return strength; };
Real PolyhedraMat::GetStrengthTau() const { return strengthTau; };
Real PolyhedraMat::GetStrengthSigmaCZ() const { return sigmaCZ; };
Real PolyhedraMat::GetStrengthSigmaCD() const { return sigmaCD; };
int PolyhedraMat::GetWeiM() const { return Wei_m; };
Real PolyhedraMat::GetWeiS0() const { return Wei_S0; };
Real PolyhedraMat::GetWeiV0() const { return Wei_V0; };
Real PolyhedraMat::GetP() const { return Wei_P; };
//****************************************************************************************
/* Destructor */
PolyhedraGeom::~PolyhedraGeom() { }
//****************************************************************************************
/* AaBb overlap checker */
void Bo1_Polyhedra_Aabb::go(const shared_ptr<Shape>& ig, shared_ptr<Bound>& bv, const Se3r& se3, const Body*)
{
Polyhedra* t = static_cast<Polyhedra*>(ig.get());
if (!t->IsInitialized()) t->Initialize();
if (!bv) { bv = shared_ptr<Bound>(new Aabb); }
Aabb* aabb = static_cast<Aabb*>(bv.get());
//Quaternionr invRot=se3.orientation.conjugate();
int N = (int)t->v.size();
Vector3r v_g, mincoords(0., 0., 0.), maxcoords(0., 0., 0.);
for (int i = 0; i < N; i++) {
v_g = se3.orientation * t->v[i]; // vertices in global coordinates
mincoords = Vector3r(min(mincoords[0], v_g[0]), min(mincoords[1], v_g[1]), min(mincoords[2], v_g[2]));
maxcoords = Vector3r(max(maxcoords[0], v_g[0]), max(maxcoords[1], v_g[1]), max(maxcoords[2], v_g[2]));
}
if (aabbEnlargeFactor > 0) {
mincoords *= aabbEnlargeFactor;
maxcoords *= aabbEnlargeFactor;
}
aabb->min = se3.position + mincoords;
aabb->max = se3.position + maxcoords;
}
//**********************************************************************************
/* Plotting */
#ifdef YADE_OPENGL
bool Gl1_Polyhedra::wire;
void Gl1_Polyhedra::go(const shared_ptr<Shape>& cm, const shared_ptr<State>&, bool wire2, const GLViewInfo&)
{
glMaterialv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, Vector3r(cm->color[0], cm->color[1], cm->color[2]));
glColor3v(cm->color);
Polyhedra* t = static_cast<Polyhedra*>(cm.get());
vector<int> faceTri = t->GetSurfaceTriangulation();
if (wire || wire2) {
glDisable(GL_LIGHTING);
glBegin(GL_LINES)
;
for (int tri = 0; tri < (int)faceTri.size(); tri += 3) {
glOneWire(t, faceTri[tri], faceTri[tri + 1]);
glOneWire(t, faceTri[tri], faceTri[tri + 2]);
glOneWire(t, faceTri[tri + 1], faceTri[tri + 2]);
}
glEnd();
} else {
Vector3r centroid = t->GetCentroid();
glDisable(GL_CULL_FACE);
glEnable(GL_LIGHTING);
glBegin(GL_TRIANGLES)
;
for (int tri = 0; tri < (int)faceTri.size(); tri += 3) {
const auto a = faceTri[tri + 0];
const auto b = faceTri[tri + 1];
const auto c = faceTri[tri + 2];
Vector3r n = (t->v[b] - t->v[a]).cross(t->v[c] - t->v[a]);
n.normalize();
Vector3r 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]);
}
glEnd();
}
}
void Gl1_PolyhedraGeom::go(const shared_ptr<IGeom>& ig, const shared_ptr<Interaction>&, const shared_ptr<Body>&, const shared_ptr<Body>&, bool) { draw(ig); }
void Gl1_PolyhedraGeom::draw(const shared_ptr<IGeom>& /*ig*/) {};
GLUquadric* Gl1_PolyhedraPhys::gluQuadric = NULL;
Real Gl1_PolyhedraPhys::maxFn;
Real Gl1_PolyhedraPhys::refRadius;
Real Gl1_PolyhedraPhys::maxRadius;
int Gl1_PolyhedraPhys::signFilter;
int Gl1_PolyhedraPhys::slices;
int Gl1_PolyhedraPhys::stacks;
void Gl1_PolyhedraPhys::go(
const shared_ptr<IPhys>& ip, const shared_ptr<Interaction>& i, const shared_ptr<Body>& b1, const shared_ptr<Body>& b2, bool /*wireFrame*/)
{
if (!gluQuadric) {
gluQuadric = gluNewQuadric();
if (!gluQuadric) throw runtime_error("Gl1_PolyhedraPhys::go unable to allocate new GLUquadric object (out of memory?).");
}
PolyhedraPhys* np = static_cast<PolyhedraPhys*>(ip.get());
shared_ptr<IGeom> ig(i->geom);
if (!ig) return; // changed meanwhile?
PolyhedraGeom* geom = YADE_CAST<PolyhedraGeom*>(ig.get());
Real fnNorm = np->normalForce.dot(geom->normal);
if ((signFilter > 0 && fnNorm < 0) || (signFilter < 0 && fnNorm > 0)) return;
int fnSign = fnNorm > 0 ? 1 : -1;
fnNorm = math::abs(fnNorm);
Real radiusScale = 1.;
maxFn = max(fnNorm, maxFn);
Real realMaxRadius;
if (maxRadius < 0) {
refRadius = min(0.03, refRadius);
realMaxRadius = refRadius;
} else
realMaxRadius = maxRadius;
Real radius = radiusScale * realMaxRadius * (fnNorm / maxFn);
if (radius <= 0.) radius = 1E-8;
Vector3r color = Shop::scalarOnColorScale(fnNorm * fnSign, -maxFn, maxFn);
Vector3r p1 = b1->state->pos, p2 = b2->state->pos;
Vector3r relPos;
relPos = p2 - p1;
Real dist = relPos.norm();
glDisable(GL_CULL_FACE);
glPushMatrix();
glTranslate(p1[0], p1[1], p1[2]);
Quaternionr q(Quaternionr().setFromTwoVectors(Vector3r(0, 0, 1), relPos / dist /* normalized */));
// using Transform with OpenGL: http://eigen.tuxfamily.org/dox/TutorialGeometry.html
//glMultMatrixd(Eigen::Affine3d(q).data());
glMultMatrix(Eigen::Transform<Real, 3, Eigen::Affine>(q).data());
glColor3v(color);
gluCylinder(gluQuadric, radius, radius, dist, slices, stacks);
glPopMatrix();
}
#endif
//**********************************************************************************
//!Precompute data needed for rotating tangent vectors attached to the interaction
void PolyhedraGeom::precompute(
const State& rbp1,
const State& rbp2,
const Scene* scene,
const shared_ptr<Interaction>& /*c*/,
const Vector3r& currentNormal,
bool isNew,
const Vector3r& shift2)
{
if (!isNew) {
orthonormal_axis = normal.cross(currentNormal);
Real angle = scene->dt * 0.5 * normal.dot(rbp1.angVel + rbp2.angVel);
twist_axis = angle * normal;
} else
twist_axis = orthonormal_axis = Vector3r::Zero();
//Update contact normal
normal = currentNormal;
//Precompute shear increment
Vector3r c1x = (contactPoint - rbp1.pos);
Vector3r c2x = (contactPoint - (rbp2.pos + shift2));
Vector3r relativeVelocity = (rbp2.vel + rbp2.angVel.cross(c2x)) - (rbp1.vel + rbp1.angVel.cross(c1x));
//keep the shear part only
relativeVelocity = relativeVelocity - normal.dot(relativeVelocity) * normal;
shearInc = relativeVelocity * scene->dt;
}
//**********************************************************************************
Vector3r& PolyhedraGeom::rotate(Vector3r& shearForce) const
{
// approximated rotations
shearForce -= shearForce.cross(orthonormal_axis);
shearForce -= shearForce.cross(twist_axis);
//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?...
shearForce -= normal.dot(shearForce) * normal;
return shearForce;
}
//**********************************************************************************
/* Material law, physics */
void Ip2_PolyhedraMat_PolyhedraMat_PolyhedraPhys::go(const shared_ptr<Material>& b1, const shared_ptr<Material>& b2, const shared_ptr<Interaction>& interaction)
{
if (interaction->phys) return;
const shared_ptr<PolyhedraMat>& mat1 = YADE_PTR_CAST<PolyhedraMat>(b1);
const shared_ptr<PolyhedraMat>& mat2 = YADE_PTR_CAST<PolyhedraMat>(b2);
interaction->phys = shared_ptr<PolyhedraPhys>(new PolyhedraPhys());
const shared_ptr<PolyhedraPhys>& contactPhysics = YADE_PTR_CAST<PolyhedraPhys>(interaction->phys);
Real Kna = mat1->young;
Real Knb = mat2->young;
Real Ksa = mat1->young * mat1->poisson;
Real Ksb = mat2->young * mat2->poisson;
Real frictionAngle = math::min(mat1->frictionAngle, mat2->frictionAngle);
contactPhysics->tangensOfFrictionAngle = math::tan(frictionAngle);
contactPhysics->kn = Kna * Knb / (Kna + Knb);
contactPhysics->ks = Ksa * Ksb / (Ksa + Ksb);
};
void Ip2_FrictMat_PolyhedraMat_FrictPhys::go(const shared_ptr<Material>& pp1, const shared_ptr<Material>& pp2, const shared_ptr<Interaction>& interaction)
{
const shared_ptr<FrictMat>& mat1 = YADE_PTR_CAST<FrictMat>(pp1);
const shared_ptr<PolyhedraMat>& mat2 = YADE_PTR_CAST<PolyhedraMat>(pp2);
Ip2_FrictMat_FrictMat_FrictPhys().go(mat1, mat2, interaction);
}
//**************************************************************************************
Real Law2_PolyhedraGeom_PolyhedraPhys_Volumetric::getPlasticDissipation() const { return (Real)plasticDissipation; }
void Law2_PolyhedraGeom_PolyhedraPhys_Volumetric::initPlasticDissipation(Real initVal)
{
plasticDissipation.reset();
plasticDissipation += initVal;
}
Real Law2_PolyhedraGeom_PolyhedraPhys_Volumetric::elasticEnergy()
{
Real energy = 0;
FOREACH(const shared_ptr<Interaction>& I, *scene->interactions)
{
if (!I->isReal()) continue;
FrictPhys* phys = dynamic_cast<FrictPhys*>(I->phys.get());
if (phys) { energy += 0.5 * (phys->normalForce.squaredNorm() / phys->kn + phys->shearForce.squaredNorm() / phys->ks); }
}
return energy;
}
//**************************************************************************************
// Apply forces on polyhedrons in collision based on geometric configuration
bool Law2_PolyhedraGeom_PolyhedraPhys_Volumetric::go(shared_ptr<IGeom>& /*ig*/, shared_ptr<IPhys>& /*ip*/, Interaction* I)
{
if (!I->geom) { return true; }
const shared_ptr<PolyhedraGeom>& contactGeom(YADE_PTR_DYN_CAST<PolyhedraGeom>(I->geom));
if (!contactGeom) { return true; }
const Body::id_t idA = I->getId1(), idB = I->getId2();
const shared_ptr<Body>&A = Body::byId(idA), B = Body::byId(idB);
PolyhedraPhys* phys = dynamic_cast<PolyhedraPhys*>(I->phys.get());
//erase the interaction when aAbB shows separation, otherwise keep it to be able to store previous separating plane for fast detection of separation
Vector3r shift2 = scene->cell->hSize * I->cellDist.cast<Real>();
if (A->bound->min[0] >= B->bound->max[0] + shift2[0] || B->bound->min[0] + shift2[0] >= A->bound->max[0]
|| A->bound->min[1] >= B->bound->max[1] + shift2[1] || B->bound->min[1] + shift2[1] >= A->bound->max[1]
|| A->bound->min[2] >= B->bound->max[2] + shift2[2] || B->bound->min[2] + shift2[2] >= A->bound->max[2]) {
return false;
}
//zero penetration depth means no interaction force
if (!(contactGeom->equivalentPenetrationDepth > 1E-18) || !(contactGeom->penetrationVolume > 0)) {
phys->normalForce = Vector3r(0., 0., 0.);
phys->shearForce = Vector3r(0., 0., 0.);
return true;
}
Real prop = math::pow(contactGeom->penetrationVolume, volumePower);
Vector3r normalForce = contactGeom->normal * prop * phys->kn;
//shear force: in case the polyhdras are separated and come to contact again, one
//should not use the previous shear force
Vector3r shearForce = phys->shearForce;
if (contactGeom->isShearNew) shearForce = Vector3r::Zero();
else
shearForce = contactGeom->rotate(shearForce);
const Vector3r& shearDisp = contactGeom->shearInc;
shearForce -= phys->ks * shearDisp;
const Real maxFs = normalForce.squaredNorm() * math::pow(phys->tangensOfFrictionAngle, 2);
if (shearForce.squaredNorm() > maxFs && maxFs) {
//PFC3d SlipModel, is using friction angle. CoulombCriterion
Real ratio = sqrt(maxFs) / shearForce.norm();
if (math::isinf(ratio)) {
LOG_DEBUG(
"shearForce.squaredNorm() > maxFs && maxFs: "
<< (shearForce.squaredNorm() > maxFs && maxFs)); // the condition should be 1 (we are in this branch), but is actually 0
LOG_DEBUG("shearForce: " << shearForce); // should be (0,0,0)
ratio = 0;
}
//Store prev force for definition of plastic slip
//Define the plastic work input and increment the total plastic energy dissipated
const Vector3r trialForce = shearForce;
shearForce *= ratio;
if (scene->trackEnergy && traceEnergy) {
const Real dissip = ((1 / phys->ks) * (trialForce - shearForce)).dot(shearForce);
if (traceEnergy) plasticDissipation += dissip;
else if (dissip > 0)
scene->energy->add(dissip, "plastDissip", plastDissipIx, false);
// compute elastic energy as well
scene->energy->add(
0.5 * (normalForce.squaredNorm() / phys->kn + shearForce.squaredNorm() / phys->ks), "elastPotential", elastPotentialIx, true);
}
} else {
if (maxFs == 0) shearForce = Vector3r::Zero();
scene->energy->add(
0.5 * (normalForce.squaredNorm() / phys->kn + shearForce.squaredNorm() / phys->ks), "elastPotential", elastPotentialIx, true);
}
Vector3r F = -normalForce - shearForce;
if (contactGeom->equivalentPenetrationDepth != contactGeom->equivalentPenetrationDepth) exit(1);
scene->forces.addForce(idA, F);
scene->forces.addForce(idB, -F);
scene->forces.addTorque(idA, -(A->state->pos - contactGeom->contactPoint).cross(F));
scene->forces.addTorque(idB, (B->state->pos - contactGeom->contactPoint).cross(F));
/*
FILE * fin = fopen("Forces.dat","a");
fprintf(fin,"************** IDS %d %d **************\n",idA, idB);
Vector3r T = (B->state->pos-contactGeom->contactPoint).cross(F);
fprintf(fin,"volume\t%e\n",contactGeom->penetrationVolume);
fprintf(fin,"normal_force\t%e\t%e\t%e\n",normalForce[0],normalForce[1],normalForce[2]);
fprintf(fin,"shear_force\t%e\t%e\t%e\n",shearForce[0],shearForce[1],shearForce[2]);
fprintf(fin,"total_force\t%e\t%e\t%e\n",F[0],F[1],F[2]);
fprintf(fin,"torsion\t%e\t%e\t%e\n",T[0],T[1],T[2]);
fprintf(fin,"A\t%e\t%e\t%e\n",A->state->pos[0],A->state->pos[1],A->state->pos[2]);
fprintf(fin,"B\t%e\t%e\t%e\n",B->state->pos[0],B->state->pos[1],B->state->pos[2]);
fprintf(fin,"centroid\t%e\t%e\t%e\n",contactGeom->contactPoint[0],contactGeom->contactPoint[1],contactGeom->contactPoint[2]);
fclose(fin);
*/
//needed to be able to acces interaction forces in other parts of yade
phys->normalForce = normalForce;
phys->shearForce = shearForce;
return true;
}
} // namespace yade
#endif // YADE_CGAL
|
