Polyhedra.cpp 24.4 KB
Newer Older
1 2 3 4
// © 2013 Jan Elias, http://www.fce.vutbr.cz/STM/elias.j/, [email protected]
// https://www.vutbr.cz/www_base/gigadisk.php?i=95194aa9a

#ifdef YADE_CGAL
5 6
// NDEBUG causes crashes in CGAL sometimes. Anton
#ifdef NDEBUG
7
#undef NDEBUG
8
#endif
9
#include "Polyhedra.hpp"
10
#ifdef YADE_OPENGL
11
#include <lib/opengl/OpenGLWrapper.hpp>
12 13 14
#endif

namespace yade { // Cannot have #include directive inside.
15

16
using math::max;
17
using math::min; // using inside .cpp file is ok.
18

19 20 21 22 23 24 25
YADE_PLUGIN(/* self-contained in hpp: */ (Polyhedra)(PolyhedraGeom)(Bo1_Polyhedra_Aabb)(PolyhedraPhys)(PolyhedraMat)(
        Ip2_PolyhedraMat_PolyhedraMat_PolyhedraPhys)(Ip2_FrictMat_PolyhedraMat_FrictPhys)(Law2_PolyhedraGeom_PolyhedraPhys_Volumetric)
/* some code in cpp (this file): */
#ifdef YADE_OPENGL
                    (Gl1_Polyhedra)(Gl1_PolyhedraGeom)(Gl1_PolyhedraPhys)
#endif
);
26

27 28
CREATE_LOGGER(Law2_PolyhedraGeom_PolyhedraPhys_Volumetric);

29 30 31
//*********************************************************************************
/* Polyhedra Constructor */

32 33
Polyhedra::Polyhedra(const std::vector<Vector3r>&& V)
{
34 35 36 37 38
	createIndex();
	v = V;
	Initialize();
}

39 40
Polyhedra::Polyhedra(const Vector3r&& xsize, const int&& xseed)
{
41
	createIndex();
42 43
	seed = xseed;
	size = xsize;
44 45 46
	v.clear();
	Initialize();
}
47 48 49 50
void Polyhedra::Initialize()
{
	if (init)
		return;
51
	bool isRandom = false;
52

53
	//get vertices
54 55
	int N = (int)v.size();
	if (N == 0) {
56
		//generate randomly
57 58 59
		while ((int)v.size() < 4)
			GenerateRandomGeometry();
		N        = (int)v.size();
60 61 62
		isRandom = true;
	}

63
	//compute convex hull of vertices
64 65
	std::vector<CGALpoint> points;
	points.resize(v.size());
66 67
	for (int i = 0; i < N; i++) {
		points[i] = CGALpoint(v[i][0], v[i][1], v[i][2]);
68 69 70
	}

	CGAL::convex_hull_3(points.begin(), points.end(), P);
71

72
	//connect triagular facets if possible
73
	std::transform(P.facets_begin(), P.facets_end(), P.planes_begin(), Plane_equation());
74 75
	P = Simplify(P, 1E-9);

76
	//modify order of v according to CGAl polyhedron
77
	//int i = 0;
78
	v.clear();
79 80
	for (Polyhedron::Vertex_iterator vIter = P.vertices_begin(); vIter != P.vertices_end(); ++vIter /*, i++*/) {
		v.push_back(Vector3r(vIter->point().x(), vIter->point().y(), vIter->point().z()));
81
	}
82 83 84

	//list surface triangles for plotting
	faceTri.clear();
85 86
	std::transform(P.facets_begin(), P.facets_end(), P.planes_begin(), Plane_equation());
	for (Polyhedron::Facet_iterator fIter = P.facets_begin(); fIter != P.facets_end(); fIter++) {
87
		Polyhedron::Halfedge_around_facet_circulator hfc0;
88 89 90 91
		int                                          n = fIter->facet_degree();
		hfc0                                           = fIter->facet_begin();
		int a                                          = std::distance(P.vertices_begin(), hfc0->vertex());
		for (int i = 2; i < n; i++) {
92 93 94 95 96 97 98 99 100 101
			++hfc0;
			faceTri.push_back(a);
			faceTri.push_back(std::distance(P.vertices_begin(), hfc0->vertex()));
			faceTri.push_back(std::distance(P.vertices_begin(), hfc0->next()->vertex()));
		}
	}

	//compute centroid and volume
	P_volume_centroid(P, &volume, &centroid);
	//check vierd behavior of CGAL in tessalation
102
	if (isRandom && volume * 1.75 < 4. / 3. * 3.14 * size[0] / 2. * size[1] / 2. * size[2] / 2.) {
103 104 105 106
		v.clear();
		seed = rand();
		Initialize();
	}
107 108
	Vector3r translation((-1) * centroid);

109
	//set centroid to be [0,0,0]
110 111
	for (int i = 0; i < N; i++) {
		v[i] = v[i] - centroid;
112
	}
113 114
	if (isRandom)
		centroid = Vector3r::Zero();
115

116
	Vector3r origin(0, 0, 0);
117 118

	//move and rotate also the CGAL structure Polyhedron
119 120
	Transformation t_trans(1., 0., 0., translation[0], 0., 1., 0., translation[1], 0., 0., 1., translation[2], 1.);
	std::transform(P.points_begin(), P.points_end(), P.points_begin(), t_trans);
121

122
	//compute inertia
123
	Real     vtet;
124 125 126
	Vector3r ctet;
	Matrix3r Itet1, Itet2;
	Matrix3r inertia_tensor(Matrix3r::Zero());
127 128 129 130 131 132 133 134 135
	for (int i = 0; i < (int)faceTri.size(); i += 3) {
		vtet  = math::abs((origin - v[faceTri[i + 2]]).dot((v[faceTri[i]] - v[faceTri[i + 2]]).cross(v[faceTri[i + 1]] - v[faceTri[i + 2]])) / 6.);
		ctet  = (origin + v[faceTri[i]] + v[faceTri[i + 1]] + v[faceTri[i + 2]]) / 4.;
		Itet1 = TetraInertiaTensor(origin - ctet, v[faceTri[i]] - ctet, v[faceTri[i + 1]] - ctet, v[faceTri[i + 2]] - ctet);
		ctet  = ctet - origin;
		Itet2 << ctet[1] * ctet[1] + ctet[2] * ctet[2], -ctet[0] * ctet[1], -ctet[0] * ctet[2], -ctet[0] * ctet[1],
		        ctet[0] * ctet[0] + ctet[2] * ctet[2], -ctet[2] * ctet[1], -ctet[0] * ctet[2], -ctet[2] * ctet[1],
		        ctet[1] * ctet[1] + ctet[0] * ctet[0];
		inertia_tensor = inertia_tensor + Itet1 + Itet2 * vtet;
136
	}
137

138
	if (math::abs(inertia_tensor(0, 1)) + math::abs(inertia_tensor(0, 2)) + math::abs(inertia_tensor(1, 2)) < 1E-13) {
139 140
		// no need to rotate, inertia already diagonal
		orientation = Quaternionr::Identity();
141 142
		inertia     = Vector3r(inertia_tensor(0, 0), inertia_tensor(1, 1), inertia_tensor(2, 2));
	} else {
143 144
		// calculate eigenvectors of I
		Vector3r rot;
145 146
		Matrix3r I_rot(Matrix3r::Zero()), I_new(Matrix3r::Zero());
		matrixEigenDecomposition(inertia_tensor, I_rot, I_new);
147 148 149
		// I_rot = eigenvectors of inertia_tensors in columns
		// I_new = eigenvalues on diagonal
		// set positove direction of vectors - otherwise reloading does not work
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
		Matrix3r sign(Matrix3r::Zero());
		Real     max_v_signed = I_rot(0, 0);
		Real     max_v        = math::abs(I_rot(0, 0));
		if (max_v < math::abs(I_rot(1, 0))) {
			max_v_signed = I_rot(1, 0);
			max_v        = math::abs(I_rot(1, 0));
		}
		if (max_v < math::abs(I_rot(2, 0))) {
			max_v_signed = I_rot(2, 0);
			max_v        = math::abs(I_rot(2, 0));
		}
		sign(0, 0)   = max_v_signed / max_v;
		max_v_signed = I_rot(0, 1);
		max_v        = math::abs(I_rot(0, 1));
		if (max_v < math::abs(I_rot(1, 1))) {
			max_v_signed = I_rot(1, 1);
			max_v        = math::abs(I_rot(1, 1));
		}
		if (max_v < math::abs(I_rot(2, 1))) {
			max_v_signed = I_rot(2, 1);
			max_v        = math::abs(I_rot(2, 1));
		}
		sign(1, 1) = max_v_signed / max_v;
		sign(2, 2) = 1.;
		I_rot      = I_rot * sign;
175 176
		// force the eigenvectors to be right-hand oriented
		Vector3r third = (I_rot.col(0)).cross(I_rot.col(1));
177 178 179 180 181 182
		I_rot(0, 2)    = third[0];
		I_rot(1, 2)    = third[1];
		I_rot(2, 2)    = third[2];

		inertia     = Vector3r(I_new(0, 0), I_new(1, 1), I_new(2, 2));
		orientation = Quaternionr(I_rot);
183 184
		//rotate the voronoi cell so that x - is maximal inertia axis and z - is minimal inertia axis
		//orientation.normalize();  //not needed
185 186
		for (int i = 0; i < (int)v.size(); i++) {
			v[i] = orientation.conjugate() * v[i];
187
		}
188

189
		//rotate also the CGAL structure Polyhedron
190
		Matrix3r       rot_mat = (orientation.conjugate()).toRotationMatrix();
Anton Gladky's avatar
Anton Gladky committed
191
		Transformation t_rot(
192 193 194 195 196 197 198 199 200 201 202
		        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.);
		std::transform(P.points_begin(), P.points_end(), P.points_begin(), t_rot);
203 204 205 206 207
	}
	//initialization done
	init = 1;
}

208 209 210
void Polyhedra::setVertices(const std::vector<Vector3r>& v)
{
	init    = false;
211 212 213 214
	this->v = v;
	Initialize();
}

215 216
void Polyhedra::setVertices4(const Vector3r& v0, const Vector3r& v1, const Vector3r& v2, const Vector3r& v3)
{
217 218 219 220 221 222 223 224 225
	init = false;
	v.resize(4);
	v[0] = v0;
	v[1] = v1;
	v[2] = v2;
	v[3] = v3;
	Initialize();
}

226 227 228
//**************************************************************************
/* Generator of randomly shaped polyhedron based on Voronoi tessellation*/

229 230
void Polyhedra::GenerateRandomGeometry()
{
231
	srand(seed);
232 233

	vector<CGALpoint> nuclei;
234 235 236 237
	nuclei.push_back(CGALpoint(5., 5., 5.));
	CGALpoint    trial;
	unsigned int iter = 0;
	bool         isOK;
238
	//fill box 5x5x5 with randomly located nuclei with restricted minimal mutual distance 0.75 which gives approximate mean mutual distance 1;
239 240
	Real dist_min2 = 0.75 * 0.75;
	while (iter < 500) {
241 242
		isOK = true;
		iter++;
243 244 245 246 247
		trial = CGALpoint(Real(rand()) / RAND_MAX * 5. + 2.5, Real(rand()) / RAND_MAX * 5. + 2.5, Real(rand()) / RAND_MAX * 5. + 2.5);
		for (int i = 0; i < (int)nuclei.size(); i++) {
			isOK = pow(nuclei[i].x() - trial.x(), 2) + pow(nuclei[i].y() - trial.y(), 2) + pow(nuclei[i].z() - trial.z(), 2) > dist_min2;
			if (!isOK)
				break;
248
		}
249
		if (isOK) {
250 251
			iter = 0;
			nuclei.push_back(trial);
252 253
		}
	}
254

255 256
	//perform Voronoi tessellation
	nuclei.erase(nuclei.begin());
257 258
	Triangulation                dt(nuclei.begin(), nuclei.end());
	Triangulation::Vertex_handle zero_point = dt.insert(CGALpoint(5., 5., 5.));
259
	v.clear();
260 261 262 263
	std::vector<Triangulation::Cell_handle> ch_cells;
	dt.incident_cells(zero_point, std::back_inserter(ch_cells));
	for (auto ci = ch_cells.begin(); ci != ch_cells.end(); ++ci) {
		v.push_back(FromCGALPoint(dt.dual(*ci)) - Vector3r(5., 5., 5.));
264 265 266
	}

	//resize and rotate the voronoi cell
267
	Quaternionr Rot(Real(rand()) / RAND_MAX, Real(rand()) / RAND_MAX, Real(rand()) / RAND_MAX, Real(rand()) / RAND_MAX);
268
	Rot.normalize();
269 270
	for (int i = 0; i < (int)v.size(); i++) {
		v[i] = Rot * (Vector3r(v[i][0] * size[0], v[i][1] * size[1], v[i][2] * size[2]));
271
	}
272

273 274 275 276 277 278 279 280 281
	//to avoid patological cases (that should not be present, but CGAL works somehow unpredicable)
	if (v.size() < 8) {
		cout << "wrong " << v.size() << endl;
		v.clear();
		seed = rand();
		GenerateRandomGeometry();
	}
}

282 283
//**************************************************************************
/* Get polyhdral surfaces */
284 285
vector<vector<int>> Polyhedra::GetSurfaces() const
{
286
	vector<vector<int>> ret(P.size_of_facets());
287 288 289
	int                 i = 0;
	for (Polyhedron::Facet_const_iterator f = P.facets_begin(); f != P.facets_end(); f++, i++) {
		Polyhedron::Halfedge_around_facet_const_circulator h = f->facet_begin();
290 291 292 293 294 295 296
		do {
			ret[i].push_back(std::distance(P.vertices_begin(), h->vertex()));
		} while (++h != f->facet_begin());
	}
	return ret;
}

297 298
Vector3r Polyhedra::GetCentroid()
{
299 300 301
	Initialize();
	return centroid;
}
302

303 304
Vector3r Polyhedra::GetInertia()
{
305 306 307 308
	Initialize();
	return inertia;
}

309 310
vector<int> Polyhedra::GetSurfaceTriangulation()
{
311 312 313 314
	Initialize();
	return faceTri;
}

315 316
Real Polyhedra::GetVolume()
{
317 318 319 320
	Initialize();
	return volume;
}

321 322
Quaternionr Polyhedra::GetOri()
{
323 324 325 326
	Initialize();
	return orientation;
}

327 328
void Polyhedra::Clear()
{
329 330 331
	v.clear();
	P.clear();
	init = 0;
332
	size = Vector3r(1., 1., 1.);
333 334 335
	faceTri.clear();
}

336
bool Polyhedra::IsInitialized() const { return init; }
337

338
Polyhedron Polyhedra::GetPolyhedron() const { return P; }
339 340 341
//****************************************************************************************
/* Destructor */

342
Polyhedra::~Polyhedra() {}
343

344
//****************************************************************************************
345 346 347 348 349 350 351 352
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; };
353

354 355 356
//****************************************************************************************
/* Destructor */

357
PolyhedraGeom::~PolyhedraGeom() {}
358 359 360 361

//****************************************************************************************
/* AaBb overlap checker  */

362 363 364 365 366 367 368 369 370
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());
371
	//Quaternionr invRot=se3.orientation.conjugate();
372 373 374 375 376 377
	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]));
378
	}
379
	if (aabbEnlargeFactor > 0) {
Jan Stransky's avatar
Jan Stransky committed
380 381 382
		mincoords *= aabbEnlargeFactor;
		maxcoords *= aabbEnlargeFactor;
	}
383 384
	aabb->min = se3.position + mincoords;
	aabb->max = se3.position + maxcoords;
385 386 387 388 389 390
}

//**********************************************************************************
/* Plotting */

#ifdef YADE_OPENGL
391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407
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]);
408
			}
409 410 411 412 413 414 415 416 417 418 419 420 421
		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]);
422
				n.normalize();
423 424 425 426
				Vector3r faceCenter = (t->v[a] + t->v[b] + t->v[c]) / 3.;
				if ((faceCenter - centroid).dot(n) < 0)
					n = -n;

427 428 429 430 431
				glNormal3v(n);
				glVertex3v(t->v[a]);
				glVertex3v(t->v[b]);
				glVertex3v(t->v[c]);
			}
432
		glEnd();
433
	}
434
}
435

436
void Gl1_PolyhedraGeom::go(const shared_ptr<IGeom>& ig, const shared_ptr<Interaction>&, const shared_ptr<Body>&, const shared_ptr<Body>&, bool) { draw(ig); }
437

438
void Gl1_PolyhedraGeom::draw(const shared_ptr<IGeom>& /*ig*/) {};
439

440 441 442 443 444 445 446
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;
447

448 449 450 451 452 453 454
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?).");
455
	}
456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494
	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();
}
495 496 497 498 499
#endif

//**********************************************************************************
//!Precompute data needed for rotating tangent vectors attached to the interaction

500 501 502 503 504 505 506 507 508 509
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) {
510
		orthonormal_axis = normal.cross(currentNormal);
511 512 513 514
		Real angle       = scene->dt * 0.5 * normal.dot(rbp1.angVel + rbp2.angVel);
		twist_axis       = angle * normal;
	} else
		twist_axis = orthonormal_axis = Vector3r::Zero();
515
	//Update contact normal
516
	normal = currentNormal;
517
	//Precompute shear increment
518 519 520
	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));
521
	//keep the shear part only
522 523
	relativeVelocity = relativeVelocity - normal.dot(relativeVelocity) * normal;
	shearInc         = relativeVelocity * scene->dt;
524 525 526
}

//**********************************************************************************
527 528
Vector3r& PolyhedraGeom::rotate(Vector3r& shearForce) const
{
529 530 531 532
	// 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?...
533
	shearForce -= normal.dot(shearForce) * normal;
534 535 536 537 538 539
	return shearForce;
}

//**********************************************************************************
/* Material law, physics */

540 541 542 543 544 545 546
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());
547
	const shared_ptr<PolyhedraPhys>& contactPhysics = YADE_PTR_CAST<PolyhedraPhys>(interaction->phys);
548 549 550 551 552 553 554 555
	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);
556 557
};

558 559 560
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);
561
	const shared_ptr<PolyhedraMat>& mat2 = YADE_PTR_CAST<PolyhedraMat>(pp2);
562
	Ip2_FrictMat_FrictMat_FrictPhys().go(mat1, mat2, interaction);
563 564
}

565
//**************************************************************************************
566 567 568 569 570 571
Real Law2_PolyhedraGeom_PolyhedraPhys_Volumetric::getPlasticDissipation() { return (Real)plasticDissipation; }
void Law2_PolyhedraGeom_PolyhedraPhys_Volumetric::initPlasticDissipation(Real initVal)
{
	plasticDissipation.reset();
	plasticDissipation += initVal;
}
572
Real Law2_PolyhedraGeom_PolyhedraPhys_Volumetric::elasticEnergy()
573
{
574 575 576 577 578
	Real energy = 0;
	FOREACH(const shared_ptr<Interaction>& I, *scene->interactions)
	{
		if (!I->isReal())
			continue;
579
		FrictPhys* phys = dynamic_cast<FrictPhys*>(I->phys.get());
580 581 582
		if (phys) {
			energy += 0.5 * (phys->normalForce.squaredNorm() / phys->kn + phys->shearForce.squaredNorm() / phys->ks);
		}
583 584 585 586 587 588
	}
	return energy;
}

//**************************************************************************************
// Apply forces on polyhedrons in collision based on geometric configuration
589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640
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
	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;
641 642
		}

643 644 645 646
		//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;
647

648 649
		if (scene->trackEnergy && traceEnergy) {
			const Real dissip = ((1 / phys->ks) * (trialForce - shearForce)).dot(shearForce);
650

651 652 653 654 655 656 657 658
			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);
659
		}
660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675
	} 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));

	/*
676 677 678 679 680 681 682 683 684 685 686
		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]);
687 688
		fclose(fin);
		*/
689 690 691 692
	//needed to be able to acces interaction forces in other parts of yade
	phys->normalForce = normalForce;
	phys->shearForce  = shearForce;
	return true;
693 694
}

695 696
} // namespace yade

697
#endif // YADE_CGAL