File: Cylinder.cpp

package info (click to toggle)
yade 2026.1.0-2
links: PTS, VCS
area: main
in suites: forky, sid
size: 34,448 kB
sloc: cpp: 97,645; python: 52,173; sh: 677; makefile: 162
file content (990 lines) | stat: -rw-r--r-- 45,350 bytes
parent folder | download | duplicates (3)
// 2011 © Bruno Chareyre <bruno.chareyre@grenoble-inp.fr>
// 2012 © Kneib Francois <francois.kneib@irstea.fr>

#include "Cylinder.hpp"
#include <lib/high-precision/Constants.hpp>
#include <pkg/common/Sphere.hpp>
#ifdef YADE_OPENGL
#include <lib/opengl/OpenGLWrapper.hpp>
#endif
#include <core/Aabb.hpp>

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

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


Cylinder::~Cylinder() { }
ChainedCylinder::~ChainedCylinder() { }
ChainedState::~ChainedState() { }
// Ig2_Sphere_ChainedCylinder_CylScGeom::~Ig2_Sphere_ChainedCylinder_CylScGeom() {}
// Ig2_ChainedCylinder_ChainedCylinder_ScGeom6D::~Ig2_ChainedCylinder_ChainedCylinder_ScGeom6D() {}

YADE_PLUGIN((Cylinder)(ChainedCylinder)(ChainedState)(Ig2_Sphere_ChainedCylinder_CylScGeom)(Ig2_Sphere_ChainedCylinder_CylScGeom6D)(
        Ig2_ChainedCylinder_ChainedCylinder_ScGeom6D)(Law2_CylScGeom6D_CohFrictPhys_CohesionMoment)(Law2_ChCylGeom6D_CohFrictPhys_CohesionMoment)(
        Law2_CylScGeom_FrictPhys_CundallStrack)
#ifdef YADE_OPENGL
                    (Gl1_Cylinder)(Gl1_ChainedCylinder)
#endif
                            (Bo1_Cylinder_Aabb)(Bo1_ChainedCylinder_Aabb));

vector<vector<int>> ChainedState::chains;
unsigned int        ChainedState::currentChain = 0;

//!##################	IG FUNCTORS   #####################


//!Sphere-cylinder or cylinder-cylinder not implemented yet, see Ig2_ChainedCylinder_ChainedCylinder_ScGeom6D and test/chained-cylinder-spring.py
bool Ig2_Sphere_ChainedCylinder_CylScGeom::go(
        const shared_ptr<Shape>& cm1,
        const shared_ptr<Shape>& cm2,
        const State&             state1,
        const State&             state2,
        const Vector3r&          shift2,
        const bool& /*force*/,
        const shared_ptr<Interaction>& c)
{
	const State*        sphereSt   = YADE_CAST<const State*>(&state1);
	const ChainedState* cylinderSt = YADE_CAST<const ChainedState*>(&state2);
	ChainedCylinder*    cylinder   = YADE_CAST<ChainedCylinder*>(cm2.get());
	Sphere*             sphere     = YADE_CAST<Sphere*>(cm1.get());
	assert(sphereSt && cylinderSt && cylinder && sphere);
	bool                  isLast = (cylinderSt->chains[cylinderSt->chainNumber].size() == (cylinderSt->rank + 1));
	bool                  isNew  = !c->geom;
	shared_ptr<CylScGeom> scm;
	if (!isNew) { scm = YADE_PTR_CAST<CylScGeom>(c->geom); }

	bool     betweenTwoCylinders = false; //defines whether the sphere's center is moving between two cylinders who have an angle>180°
	Vector3r segment, branch, direction;  //informations about the current cylinder
	Real     length, dist;
	shared_ptr<const ChainedState> statePrev;
	Vector3r                       segmentPrev = Vector3r(0, 0, 0), directionPrev = Vector3r(0, 0, 0), branchPrev = Vector3r(0, 0, 0);
	Real                           lengthPrev = 0, distPrev = 0;
	if (cylinderSt->rank > 0) { //informations about the previous cylinder
		statePrev     = YADE_PTR_CAST<const ChainedState>(Body::byId(cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank - 1], scene)->state);
		segmentPrev   = cylinderSt->pos - statePrev->pos;
		lengthPrev    = segmentPrev.norm();
		directionPrev = segmentPrev / lengthPrev;
		branchPrev    = sphereSt->pos - statePrev->pos;
		distPrev      = directionPrev.dot(branchPrev);
	}
	//FIXME : definition of segment in next line breaks periodicity
	shared_ptr<Body> cylinderNext;
	branch = sphereSt->pos - cylinderSt->pos - shift2;
	if (isLast) { //handle the last node with length=0
		segment   = Vector3r(0, 0, 0);
		length    = 0;
		direction = Vector3r(0, 1, 0);
		dist      = directionPrev.dot(branch);
		if (dist < 0) {
			if (isNew) return false;
			else if (scm->isDuplicate) {
				scm->isDuplicate      = 2;
				scm->penetrationDepth = -1;
				return true;
			}
		}
	} else {
		cylinderNext = Body::byId(cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank + 1], scene);
		segment      = cylinderNext->state->pos - cylinderSt->pos;
		length       = segment.norm();
		direction    = segment / length;
		dist         = direction.dot(branch);
		if (cylinderSt->rank > 0) {
			if (distPrev > lengthPrev && dist < 0) { //the sphere is touching two cylinder who have an angle>180°
				betweenTwoCylinders = true;
			}
		}
		if (!betweenTwoCylinders && (cylinderSt->rank > 0 or dist > 0)) {
			if (segment.dot(branch) >= segment.dot(segment) or dist < 0) { //position after or before the cylinder
				//FIXME : scm->penetrationDepth=-1 is defined to workaround interactions never being erased when scm->isDuplicate=2 on the true interaction.
				if (isNew) return false;
				else if (scm->isDuplicate) {
					scm->isDuplicate      = 2;
					scm->penetrationDepth = -1;
					return true;
				}
			}
		}
	}

	//Check sphere-cylinder distance
	Vector3r projectedP = cylinderSt->pos + shift2 + direction * dist;
	branch              = projectedP - sphereSt->pos;
	if (isLast || (cylinderSt->rank == 0 && dist < 0)) { branch = cylinderSt->pos - sphereSt->pos; }
	if (branch.norm() > sphere->radius + cylinder->radius) {
		if (isNew) return false;
		else if (scm->isDuplicate) {
			scm->isDuplicate      = 2;
			scm->penetrationDepth = -1;
			return true;
		}
	}

	if (!isNew) scm->isDuplicate = false; //reset here at each step, and recompute below
	//if there is a contact with the previous element in the chain, consider this one a duplicate and push data to the new contact. Two interactions will share the same geometry and physics during a timestep.
	if (!betweenTwoCylinders && cylinderSt->rank > 0 && dist < 0) {
		if (!isNew) {
			const shared_ptr<Interaction> intr
			        = scene->interactions->find(c->id1, cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank - 1]);
			if (!intr) {
				cout << "Skipping contact because collider didn't found the previous cylinder" << endl;
				return false;
			}
			//we know there is a contact, so there should be at least a virtual interaction created by collider
			intr->geom       = c->geom;
			intr->phys       = c->phys;
			scm              = YADE_PTR_CAST<CylScGeom>(c->geom);
			scm->isDuplicate = 2;
			scm->trueInt     = cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank - 1];
			isNew            = false;
			return true;
		} else
			scm->isDuplicate = true;
		scm->trueInt = cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank - 1];
	}
	//similarly, make sure there is no contact with the next element in the chain
	//else if (!isLast && dist>length) {
	if (!betweenTwoCylinders && !isLast && dist >= length) {
		if (!isNew) {
			const shared_ptr<Interaction> intr
			        = scene->interactions->find(c->id1, cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank + 1]);
			if (!intr) {
				cout << "Skipping contact because collider didn't found the next cylinder." << endl;
				return false;
			}
			intr->geom       = c->geom;
			intr->phys       = c->phys;
			scm              = YADE_PTR_CAST<CylScGeom>(c->geom);
			scm->isDuplicate = 2;
			scm->trueInt     = cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank + 1];
			isNew            = false;
			return true;
		} else
			scm->isDuplicate = true;
		scm->trueInt = cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank + 1];
	}

	//We didn't find any special case, do normal geometry definition
	if (isNew) {
		scm     = shared_ptr<CylScGeom>(new CylScGeom());
		c->geom = scm;
	}

	scm->radius1 = sphere->radius;
	scm->radius2 = cylinder->radius;
	if (!isLast) scm->id3 = cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank + 1];
	scm->start = cylinderSt->pos + shift2;
	scm->end   = scm->start + segment;

	//FIXME : there should be other checks without distanceFactor?
	if (dist <= 0 || isLast) { //We have sphere-node contact
		Vector3r normal = (cylinderSt->pos + shift2) - sphereSt->pos;
		Real     norm   = normal.norm();
		normal /= norm;
		scm->relPos           = 0;
		scm->onNode           = true;
		scm->relPos           = 0;
		scm->penetrationDepth = sphere->radius + cylinder->radius - norm;
		scm->contactPoint     = sphereSt->pos + (sphere->radius - 0.5 * scm->penetrationDepth) * normal;
		scm->precompute(state1, state2, scene, c, normal, isNew, shift2, true); //use sphere-sphere precompute (a node is a sphere)
	} else {                                                                        //we have sphere-cylinder contact
		scm->onNode           = false;
		scm->relPos           = dist / length;
		Real     norm         = branch.norm();
		Vector3r normal       = branch / norm;
		scm->penetrationDepth = sphere->radius + cylinder->radius - norm;

		// define a virtual sphere at the projected center
		scm->fictiousState.pos    = projectedP;
		scm->fictiousState.vel    = (1 - scm->relPos) * cylinderSt->vel + scm->relPos * cylinderNext->state->vel;
		scm->fictiousState.angVel = ((1 - scm->relPos) * cylinderSt->angVel + scm->relPos * cylinderNext->state->angVel).dot(direction)
		                * direction                                          //twist part : interpolated
		        + segment.cross(cylinderNext->state->vel - cylinderSt->vel); // non-twist part : defined from nodes velocities

		if (dist > length) {
			scm->penetrationDepth = sphere->radius + cylinder->radius - (cylinderSt->pos + segment - sphereSt->pos).norm();
			//FIXME : handle contact jump on next element
		}
		scm->contactPoint = sphereSt->pos + normal * (sphere->radius - 0.5 * scm->penetrationDepth);
		scm->precompute(
		        state1, scm->fictiousState, scene, c, branch / norm, isNew, shift2, true); //use sphere-sphere precompute (with a virtual sphere)
	}
	return true;
}


bool Ig2_Sphere_ChainedCylinder_CylScGeom::goReverse(
        const shared_ptr<Shape>&       cm1,
        const shared_ptr<Shape>&       cm2,
        const State&                   state1,
        const State&                   state2,
        const Vector3r&                shift2,
        const bool&                    force,
        const shared_ptr<Interaction>& c)
{
	cerr << "Ig2_Sphere_ChainedCylinder_CylScGeom::goReverse" << endl;
	c->swapOrder();
	return go(cm2, cm1, state2, state1, -shift2, force, c);
}

bool Ig2_Sphere_ChainedCylinder_CylScGeom6D::go(
        const shared_ptr<Shape>& cm1,
        const shared_ptr<Shape>& cm2,
        const State&             state1,
        const State&             state2,
        const Vector3r&          shift2,
        const bool& /*force*/,
        const shared_ptr<Interaction>& c)
{
	const State*        sphereSt   = YADE_CAST<const State*>(&state1);
	const ChainedState* cylinderSt = YADE_CAST<const ChainedState*>(&state2);
	ChainedCylinder*    cylinder   = YADE_CAST<ChainedCylinder*>(cm2.get());
	Sphere*             sphere     = YADE_CAST<Sphere*>(cm1.get());
	assert(sphereSt && cylinderSt && cylinder && sphere);
	bool                    isLast = (cylinderSt->chains[cylinderSt->chainNumber].size() == (cylinderSt->rank + 1));
	bool                    isNew  = !c->geom;
	shared_ptr<CylScGeom6D> scm;
	if (!isNew) { scm = YADE_PTR_CAST<CylScGeom6D>(c->geom); }

	bool     betweenTwoCylinders = false; //defines whether the sphere's center is moving between two cylinders who have an angle>180°
	Vector3r segment, branch, direction;  //informations about the current cylinder
	Real     length, dist;
	shared_ptr<const ChainedState> statePrev;
	Vector3r                       segmentPrev = Vector3r(0, 0, 0), directionPrev = Vector3r(0, 0, 0), branchPrev = Vector3r(0, 0, 0);
	Real                           lengthPrev = 0, distPrev = 0;
	if (cylinderSt->rank > 0) { //informations about the previous cylinder
		statePrev     = YADE_PTR_CAST<const ChainedState>(Body::byId(cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank - 1], scene)->state);
		segmentPrev   = cylinderSt->pos - statePrev->pos;
		lengthPrev    = segmentPrev.norm();
		directionPrev = segmentPrev / lengthPrev;
		branchPrev    = sphereSt->pos - statePrev->pos;
		distPrev      = directionPrev.dot(branchPrev);
	}

	//FIXME : definition of segment in next line breaks periodicity
	shared_ptr<Body> cylinderNext;
	branch = sphereSt->pos - cylinderSt->pos - shift2;
	if (isLast) { //handle the last node with length=0
		segment   = Vector3r(0, 0, 0);
		length    = 0;
		direction = Vector3r(0, 1, 0);
		dist      = directionPrev.dot(branch);
		if (dist < 0) {
			if (isNew) return false;
			else if (scm->isDuplicate) {
				scm->isDuplicate = 2;
				return true;
			}
		}
	} else {
		cylinderNext = Body::byId(cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank + 1], scene);
		segment      = cylinderNext->state->pos - cylinderSt->pos;
		length       = segment.norm();
		direction    = segment / length;
		dist         = direction.dot(branch);
		if (cylinderSt->rank > 0) {
			if (distPrev > lengthPrev && dist < 0) { //the sphere is touching two cylinder who have an angle>180°
				betweenTwoCylinders = true;
			}
		}
		if (!betweenTwoCylinders && (cylinderSt->rank > 0 or dist > 0)) {
			if (segment.dot(branch) >= segment.dot(segment) or dist < 0) { //position after or before the cylinder
				//FIXME : scm->penetrationDepth=-1 is defined to workaround interactions never being erased when scm->isDuplicate=2 on the true interaction.
				if (isNew) return false;
				else if (scm->isDuplicate) {
					scm->isDuplicate = 2;
					return true;
				}
			}
		}
	}

	//Check sphere-cylinder distance
	Vector3r projectedP = cylinderSt->pos + shift2 + direction * dist;
	branch              = projectedP - sphereSt->pos;
	if (isLast || (cylinderSt->rank == 0 && dist < 0)) { branch = cylinderSt->pos - sphereSt->pos; }
	if (branch.norm() > sphere->radius + cylinder->radius) {
		if (isNew) return false;
		else if (scm->isDuplicate) {
			scm->isDuplicate = 2;
			return true;
		}
	}

	if (!isNew) scm->isDuplicate = false; //reset here at each step, and recompute below
	//if there is a contact with the previous element in the chain, consider this one a duplicate and push data to the new contact. Two interactions will share the same geometry and physics during a timestep.
	if (!betweenTwoCylinders && cylinderSt->rank > 0 && dist < 0) {
		if (!isNew) {
			const shared_ptr<Interaction> intr
			        = scene->interactions->find(c->id1, cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank - 1]);
			if (!intr) {
				cout << "Skipping contact because collider didn't found the previous cylinder" << endl;
				return false;
			}
			//we know there is a contact, so there should be at least a virtual interaction created by collider
			intr->geom       = c->geom;
			intr->phys       = c->phys;
			scm              = YADE_PTR_CAST<CylScGeom6D>(c->geom);
			scm->isDuplicate = 2;
			scm->trueInt     = cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank - 1];
			isNew            = false;
			return true;
		} else
			scm->isDuplicate = true;
		scm->trueInt = cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank - 1];
	}
	//similarly, make sure there is no contact with the next element in the chain
	//else if (!isLast && dist>length) {
	if (!betweenTwoCylinders && !isLast && dist >= length) {
		if (!isNew) {
			const shared_ptr<Interaction> intr
			        = scene->interactions->find(c->id1, cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank + 1]);
			if (!intr) {
				cout << "Skipping contact because collider didn't found the next cylinder." << endl;
				return false;
			}
			intr->geom       = c->geom;
			intr->phys       = c->phys;
			scm              = YADE_PTR_CAST<CylScGeom6D>(c->geom);
			scm->isDuplicate = 2;
			scm->trueInt     = cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank + 1];
			isNew            = false;
			return true;
		} else
			scm->isDuplicate = true;
		scm->trueInt = cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank + 1];
	}

	//We didn't find any special case, do normal geometry definition
	if (isNew) {
		scm     = shared_ptr<CylScGeom6D>(new CylScGeom6D());
		c->geom = scm;
	}

	scm->radius1 = sphere->radius;
	scm->radius2 = cylinder->radius;
	if (!isLast && !scm->id3) scm->id3 = cylinderSt->chains[cylinderSt->chainNumber][cylinderSt->rank + 1];
	scm->start = cylinderSt->pos + shift2;
	scm->end   = scm->start + segment;

	//FIXME : there should be other checks without distanceFactor?
	if (dist <= 0 || isLast) { //We have sphere-node contact
		Vector3r normal = (cylinderSt->pos + shift2) - sphereSt->pos;
		Real     norm   = normal.norm();
		normal /= norm;
		scm->relPos           = 0;
		scm->onNode           = true;
		scm->relPos           = 0;
		scm->penetrationDepth = sphere->radius + cylinder->radius - norm;
		scm->contactPoint     = sphereSt->pos + (sphere->radius - 0.5 * scm->penetrationDepth) * normal;
		scm->precompute(state1, state2, scene, c, normal, isNew, shift2, true); //use sphere-sphere precompute (a node is a sphere)
	} else {                                                                        //we have sphere-cylinder contact
		scm->onNode           = false;
		scm->relPos           = dist / length;
		Real     norm         = branch.norm();
		Vector3r normal       = branch / norm;
		scm->penetrationDepth = sphere->radius + cylinder->radius - norm;

		// define a virtual sphere at the projected center
		scm->fictiousState.pos    = projectedP;
		scm->fictiousState.vel    = (1 - scm->relPos) * cylinderSt->vel + scm->relPos * cylinderNext->state->vel;
		scm->fictiousState.angVel = ((1 - scm->relPos) * cylinderSt->angVel + scm->relPos * cylinderNext->state->angVel).dot(direction)
		                * direction                                          //twist part : interpolated
		        + segment.cross(cylinderNext->state->vel - cylinderSt->vel); // non-twist part : defined from nodes velocities

		if (dist > length) {
			scm->penetrationDepth = sphere->radius + cylinder->radius - (cylinderSt->pos + segment - sphereSt->pos).norm();
			//FIXME : handle contact jump on next element
		}
		scm->contactPoint = sphereSt->pos + normal * (sphere->radius - 0.5 * scm->penetrationDepth);
		scm->precompute(
		        state1, scm->fictiousState, scene, c, branch / norm, isNew, shift2, true); //use sphere-sphere precompute (with a virtual sphere)
	}
	return true;
}


bool Ig2_Sphere_ChainedCylinder_CylScGeom6D::goReverse(
        const shared_ptr<Shape>&       cm1,
        const shared_ptr<Shape>&       cm2,
        const State&                   state1,
        const State&                   state2,
        const Vector3r&                shift2,
        const bool&                    force,
        const shared_ptr<Interaction>& c)
{
	return go(cm1, cm2, state2, state1, -shift2, force, c);
}

bool Ig2_ChainedCylinder_ChainedCylinder_ScGeom6D::go(
        const shared_ptr<Shape>& cm1,
        const shared_ptr<Shape>& cm2,
        const State&             state1,
        const State&             state2,
        const Vector3r&          shift2,
        const bool& /*force*/,
        const shared_ptr<Interaction>& c)
{
	const ChainedState *pChain1, *pChain2;
	pChain1                 = YADE_CAST<const ChainedState*>(&state1);
	pChain2                 = YADE_CAST<const ChainedState*>(&state2);
	unsigned int sizeChain1 = pChain1->chains[pChain1->chainNumber].size();
	unsigned int sizeChain2 = pChain2->chains[pChain2->chainNumber].size();
	if (!pChain1 || !pChain2) { cerr << "cast failed8567" << endl; }
	const bool          revert  = ((int)pChain2->rank - (int)pChain1->rank == -1);
	const ChainedState& bchain1 = revert ? *pChain2 : *YADE_CAST<const ChainedState*>(&state1);
	const ChainedState& bchain2 = revert ? *pChain1 : *pChain2;
	ChainedCylinder*    bs1     = static_cast<ChainedCylinder*>(revert ? cm2.get() : cm1.get());
	bool isLast = bchain1.chains[bchain1.chainNumber].size() == (bchain1.rank + 1) || bchain2.chains[bchain2.chainNumber].size() == (bchain2.rank + 1);
	bool isNew  = !c->geom;
	if (pChain2->chainNumber != pChain1->chainNumber) {
		shared_ptr<ChCylGeom6D> scm;
		if (isLast) { return false; }
		shared_ptr<Body> cylinderNext1 = Body::byId(pChain1->chains[pChain1->chainNumber][pChain1->rank + 1], scene);
		shared_ptr<Body> cylinderNext2 = Body::byId(pChain2->chains[pChain2->chainNumber][pChain2->rank + 1], scene);
		//cout<<c->id1<<"  "<<c->id2<<endl;
		bool colinearVectors = 0, insideCyl1 = 1,
		     insideCyl2 = 1;               //insideCyl1&2 are used to determine whether the contact is inside each cylinder's segment
		Real     dist = NaN, k = 0, m = 0; //k and m are the parameters of the fictious states on the cylinders.
		Vector3r A = pChain1->pos, a = cylinderNext1->state->pos - A, B = pChain2->pos, b = cylinderNext2->state->pos - B;
		Vector3r N = a.cross(b);
		Vector3r normal;
		if (N.norm() > 1e-14) {
			dist = math::abs(N.dot(B - A) / (N.norm())); //distance between the two LINES.
			//But we need to check that the intersection point is inside the two SEGMENTS ...
			//Projection of B to have a common plan between the two segments.
			Vector3r projB1 = B + dist * (N / (N.norm())), projB2 = B - dist * (N / (N.norm()));
			Real     distB1A = (projB1 - A).norm(), distB2A = (projB2 - A).norm();
			Vector3r projB = (distB1A <= distB2A) * projB1 + (distB1A > distB2A) * projB2;
			int      b1 = 0, b2 = 1; //base vectors used to compute the segment intersection (if N is aligned with an axis, we can't use this axis)
			if (math::abs(N[1]) < 1e-14 && math::abs(N[2]) < 1e-14) {
				b1 = 1;
				b2 = 2;
			}
			if (math::abs(N[0]) < 1e-14 && math::abs(N[2]) < 1e-14) {
				b1 = 0;
				b2 = 2;
			}
			Real det = a[b1] * b[b2] - a[b2] * b[b1];
			if (math::abs(det) > 1e-14) { //Check if the two segments are intersected (using k and m)
				k = (b[b2] * (projB[b1] - A[b1]) + b[b1] * (A[b2] - projB[b2])) / det;
				m = (a[b1] * (-projB[b2] + A[b2]) + a[b2] * (projB[b1] - A[b1])) / det;
				if (k < 0.0 || k >= 1.0 || m < 0.0 || m >= 1.0) { //so they are not intersected
					dist = NaN;
					if (k >= 1) {
						k = 1;
						if (!(pChain1->rank >= sizeChain1 - 2)) insideCyl1 = 0;
					}
					if (k < 0) {
						k = 0;
						if (!(pChain1->rank == 0)) insideCyl1 = 0;
					}
					if (m >= 1) {
						m = 1;
						if (!(pChain2->rank >= sizeChain2 - 2)) insideCyl2 = 0;
					}
					if (m < 0) {
						m = 0;
						if (!(pChain2->rank == 0)) insideCyl2 = 0;
					}
				}
			} else
				cout << "Error in Ig2_ChainedCylinder_ChainedCylinder_ScGeom6D : det==0 !!!" << endl; //should not happen
		} else {
			//Special case for parallel cylinders.
			//FIXME : this is an approximation, but it seems very complicated to do something else.
			//FIXME : contact following have to be done for parallel cylinders.
			colinearVectors = 1;
			insideCyl1      = 1;
			insideCyl2      = 1;
			Real PA         = (A - B).dot(b) / (b.norm() * b.norm());
			PA              = min((Real)1.0, max((Real)0.0, PA));
			Real Pa         = (A + a - B).dot(b) / (b.norm() * b.norm());
			Pa              = min((Real)1.0, max((Real)0.0, Pa));
			Real PB         = (B - A).dot(a) / (a.norm() * a.norm());
			PB              = min((Real)1.0, max((Real)0.0, PB));
			Real Pb         = (B + b - A).dot(a) / (a.norm() * a.norm());
			Pb              = min((Real)1.0, max((Real)0.0, Pb));

			Real h1              = (A + 0.5 * a - B).dot(b) / (b.norm() * b.norm()); //Projection parameter of center of a on b
			Real h2              = (B + 0.5 * b - A).dot(a) / (a.norm() * a.norm()); //Projection parameter of center of b on a
			k                    = (PB + Pb) / 2.;
			m                    = (PA + Pa) / 2.;
			dist                 = (A + k * a - (B + m * b)).norm();
			bool edgeEdgeContact = (h1 > 1 && pChain2->rank >= sizeChain2 - 2) || (h1 < 0 && pChain2->rank == 0)
			        || (h2 > 1 && pChain1->rank >= sizeChain1 - 2) || (h2 < 0 && pChain1->rank == 0);
			if (edgeEdgeContact) colinearVectors = 0;
			if ((0 <= h1 and 1 > h1) or (0 <= h2 and 1 > h2) or edgeEdgeContact) {
				; //Do a perfectly flat contact
			} else
				return false; //Parallel lines but edge-edge contact
		}

		ChainedCylinder* cc1 = static_cast<ChainedCylinder*>(cm1.get());
		ChainedCylinder* cc2 = static_cast<ChainedCylinder*>(cm2.get());
		if (math::isnan(
		            dist)) { //now if we didn't found a suitable distance because the segments don't cross each other, we try to find a sphere-cylinder distance.
			Vector3r pointsToCheck[4] = { A, A + a, B, B + b };
			Real     resultDist = dist, resultProj = dist;
			int      whichCaseIsCloser = -1;
			for (int i = 0; i < 4; i++) { //loop on the 4 cylinder's extremities and look at the extremity-cylinder distance
				Vector3r S = pointsToCheck[i], C = (i < 2) ? B : A, vec = (i < 2) ? b : a;
				Vector3r CS = S - C;
				Real     d  = CS.dot(vec) / (vec.norm());
				if (d < 0.) resultDist = CS.norm();
				else if (d > vec.norm())
					resultDist = (C + vec - S).norm();
				else
					resultDist = (CS.cross(vec)).norm() / (vec.norm());
				if (dist > resultDist or math::isnan(dist)) {
					dist              = resultDist;
					whichCaseIsCloser = i;
					resultProj        = d;
				}
			}
			//we know which extremity may be in contact (i), so k and m are computed to generate the right fictiousStates.
			insideCyl1 = 1;
			insideCyl2 = 1;

			//FIXME:NATCHOS ! this should be reformulated
			if (whichCaseIsCloser == 0 || whichCaseIsCloser == 1) {
				k = whichCaseIsCloser == 0 ? 0 : 1;
				if (resultProj <= 0) {
					m = 0;
					if (!(pChain2->rank == 0)) insideCyl2 = 0;
				} else if (resultProj > b.norm()) {
					m = 1;
					if (!(pChain2->rank >= sizeChain2 - 2)) insideCyl2 = 0;
				} else
					m = resultProj / (b.norm());
				if (isNew && whichCaseIsCloser == 1 && !(pChain1->rank >= sizeChain1 - 2)) return false;
			} else {
				m = whichCaseIsCloser == 2 ? 0 : 1;
				if (resultProj <= 0) {
					k = 0;
					if (!(pChain1->rank == 0)) insideCyl1 = 0;
				} else if (resultProj > a.norm()) {
					k = 1;
					if (!(pChain1->rank >= sizeChain2 - 2)) insideCyl1 = 0;
				} else
					k = resultProj / (a.norm());
				if (isNew && whichCaseIsCloser == 3 && !(pChain2->rank >= sizeChain2 - 2)) return false;
			}
		}
		if (isNew && dist >= cc1->radius + cc2->radius) return false; //if the contact had not yet occured, return false.
		//FIXME:the next line sometimes causes an error in the terminal, because instead of returning false here the contact should be correctly erased.
		if (insideCyl1 == 0 || insideCyl2 == 0) return false; //the contact may be duplicated ...
		else {                                                //else create the geometry.
			if (!isNew) scm = YADE_PTR_CAST<ChCylGeom6D>(c->geom);
			else {
				scm     = shared_ptr<ChCylGeom6D>(new ChCylGeom6D());
				c->geom = scm;
			}
			scm->relPos1            = colinearVectors ? 0.5 : k;
			scm->relPos2            = colinearVectors ? 0.5 : m;
			scm->fictiousState1.pos = A + k * a;
			scm->fictiousState2.pos = B + m * b;
			scm->fictiousState1.vel = (1 - k) * pChain1->vel + k * cylinderNext1->state->vel;
			scm->fictiousState2.vel = (1 - m) * pChain2->vel + m * cylinderNext2->state->vel;
			Vector3r direction      = a / (a.norm());
			scm->fictiousState1.angVel
			        = ((1 - k) * pChain1->angVel + k * cylinderNext1->state->angVel).dot(direction) * direction //twist part : interpolated
			        + a.cross(cylinderNext1->state->vel - pChain1->vel); // non-twist part : defined from nodes velocities
			direction = b / (b.norm());
			scm->fictiousState2.angVel
			        = ((1 - m) * pChain2->angVel + m * cylinderNext2->state->angVel).dot(direction) * direction //twist part : interpolated
			        + b.cross(cylinderNext2->state->vel - pChain2->vel); // non-twist part : defined from nodes velocities
			scm->contactPoint     = 0.5 * (scm->fictiousState1.pos + scm->fictiousState2.pos);
			normal                = scm->fictiousState2.pos - scm->fictiousState1.pos;
			normal                = normal / (normal.norm());
			scm->penetrationDepth = cc1->radius + cc2->radius - dist;
			scm->radius1          = cc1->radius;
			scm->radius2          = cc2->radius;
			scm->precompute(scm->fictiousState1, scm->fictiousState2, scene, c, normal, isNew, shift2, true);
			return true;
		}
	} else if (bchain2.rank - bchain1.rank != 1) { /*cerr<<"Mutual contacts in same chain between not adjacent elements, not handled*/
		return false;
	} else { //contact between two Cylinders within the same chain.
		shared_ptr<ScGeom6D> scm;
		if (!isNew) scm = YADE_PTR_CAST<ScGeom6D>(c->geom);
		else {
			scm     = shared_ptr<ScGeom6D>(new ScGeom6D());
			c->geom = scm;
		}
		Real     length = (bchain2.pos - bchain1.pos).norm();
		Vector3r segt   = pChain2->pos - pChain1->pos;
		if (isNew) { /*scm->normal=scm->prevNormal=segt/length;*/
			bs1->initLength = length;
		}
		if (!halfLengthContacts) {
			scm->radius1      = revert ? 0 : bs1->initLength;
			scm->radius2      = revert ? bs1->initLength : 0;
			scm->contactPoint = bchain2.pos;
		} else {
			scm->radius1 = scm->radius2 = 0.5 * bs1->initLength;
			scm->contactPoint           = 0.5 * (bchain2.pos + bchain1.pos);
		}
		scm->penetrationDepth = bs1->initLength - length;
		//bs1->segment used for fast BBs and projections + display
		bs1->segment = bchain2.pos - bchain1.pos;
#ifdef YADE_OPENGL
		bs1->length = length;
		bs1->chainedOrientation.setFromTwoVectors(Vector3r::UnitZ(), bchain1.ori.conjugate() * segt);
#endif
		scm->precompute(state1, state2, scene, c, segt / length, isNew, shift2, true);
		scm->precomputeRotations(state1, state2, isNew, false);
		//Set values that will be considered in Ip2 functor, geometry (precomputed) is really defined with values above
		scm->radius1 = scm->radius2 = bs1->initLength * 0.5;
		return true;
	}
}

bool Ig2_ChainedCylinder_ChainedCylinder_ScGeom6D::goReverse(
        const shared_ptr<Shape>&       cm1,
        const shared_ptr<Shape>&       cm2,
        const State&                   state1,
        const State&                   state2,
        const Vector3r&                shift2,
        const bool&                    force,
        const shared_ptr<Interaction>& c)
{
	return go(cm2, cm1, state2, state1, -shift2, force, c);
}

#ifdef YADE_OPENGL
//!##################	RENDERING   #####################

bool Gl1_Cylinder::wire;
bool Gl1_Cylinder::glutNormalize;
int  Gl1_Cylinder::glutSlices;
int  Gl1_Cylinder::glutStacks;
int  Gl1_Cylinder::glCylinderList = -1;

void Gl1_Cylinder::out(Quaternionr q)
{
	AngleAxisr aa(q);
	std::cout << " axis: " << aa.axis()[0] << " " << aa.axis()[1] << " " << aa.axis()[2] << ", angle: " << aa.angle() << " | ";
}

void Gl1_Cylinder::go(const shared_ptr<Shape>& cm, const shared_ptr<State>&, bool wire2, const GLViewInfo&)
{
	Real r      = (static_cast<Cylinder*>(cm.get()))->radius;
	Real length = (static_cast<Cylinder*>(cm.get()))->length;
	//glMaterialv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, Vector3f(cm->color[0],cm->color[1],cm->color[2]));
	glColor3v(cm->color);
	if (glutNormalize) glPushAttrib(GL_NORMALIZE);
	// 	glPushMatrix();
	Quaternionr shift = (static_cast<ChainedCylinder*>(cm.get()))->chainedOrientation;
	if (wire || wire2) drawCylinder(true, r, length, shift);
	else
		drawCylinder(false, r, length, shift);
	if (glutNormalize) glPopAttrib();
	// 	glPopMatrix();
	return;
}

void Gl1_ChainedCylinder::go(const shared_ptr<Shape>& cm, const shared_ptr<State>& state, bool wire2, const GLViewInfo&)
{
	Real        r      = (static_cast<ChainedCylinder*>(cm.get()))->radius;
	Real        length = (static_cast<ChainedCylinder*>(cm.get()))->length;
	Quaternionr shift; // = (static_cast<ChainedCylinder*>(cm.get()))->chainedOrientation;
	shift.setFromTwoVectors(Vector3r::UnitZ(), state->ori.conjugate() * (static_cast<ChainedCylinder*>(cm.get()))->segment);
	glColor3v(cm->color);
	if (glutNormalize) glPushAttrib(GL_NORMALIZE);
	if (wire || wire2) drawCylinder(true, r, length, shift);
	else
		drawCylinder(false, r, length, shift);
	if (glutNormalize) glPopAttrib();
	return;
}

void Gl1_Cylinder::drawCylinder(bool wireNonMember, Real radius, Real length, const Quaternionr& shift) const
{
	glPushMatrix();
	GLUquadricObj* quadObj = gluNewQuadric();
	gluQuadricDrawStyle(quadObj, (GLenum)(wireNonMember ? GLU_SILHOUETTE : GLU_FILL));
	gluQuadricNormals(quadObj, (GLenum)GLU_SMOOTH);
	gluQuadricOrientation(quadObj, (GLenum)GLU_OUTSIDE);
	AngleAxisr aa(shift);
	glRotate(aa.angle() * 180.0 / Mathr::PI, aa.axis()[0], aa.axis()[1], aa.axis()[2]);
	gluCylinder(quadObj, radius, radius, length, glutSlices, glutStacks);
	gluQuadricOrientation(quadObj, (GLenum)GLU_INSIDE);
	glutSolidSphere(radius, glutSlices, glutStacks);
	glTranslate(0.0, 0.0, length);

	glutSolidSphere(radius, glutSlices, glutStacks);
	//    gluDisk(quadObj,0.0,radius,glutSlices,_loops);
	gluDeleteQuadric(quadObj);
	glPopMatrix();
}


//!##################	BOUNDS FUNCTOR   #####################

#endif

void Bo1_Cylinder_Aabb::go(const shared_ptr<Shape>& cm, shared_ptr<Bound>& bv, const Se3r& se3, const Body* /*b*/)
{
	Cylinder* cylinder = static_cast<Cylinder*>(cm.get());
	if (!bv) { bv = shared_ptr<Bound>(new Aabb); }
	Aabb* aabb = static_cast<Aabb*>(bv.get());
	if (!scene->isPeriodic) {
		const Vector3r& O  = se3.position;
		Vector3r        O2 = se3.position + se3.orientation * cylinder->segment;
		aabb->min = aabb->max = O;
		for (int k = 0; k < 3; k++) {
			aabb->min[k] = min(aabb->min[k], min(O[k], O2[k]) - cylinder->radius);
			aabb->max[k] = max(aabb->max[k], max(O[k], O2[k]) + cylinder->radius);
		}
		return;
	}
}

void Bo1_ChainedCylinder_Aabb::go(const shared_ptr<Shape>& cm, shared_ptr<Bound>& bv, const Se3r& se3, const Body* /*b*/)
{
	ChainedCylinder* cylinder = static_cast<ChainedCylinder*>(cm.get());
	if (!bv) { bv = shared_ptr<Bound>(new Aabb); }
	Aabb* aabb = static_cast<Aabb*>(bv.get());
	if (!scene->isPeriodic) {
		const Vector3r& O  = se3.position;
		Vector3r        O2 = O + cylinder->segment;
		//cout<<"O="<<O<<" O2="<<O2<<endl;
		for (int k = 0; k < 3; k++) {
			aabb->min[k] = min(O[k], O2[k]) - cylinder->radius;
			aabb->max[k] = max(O[k], O2[k]) + cylinder->radius;
		}
		return;
	}
}

bool Law2_CylScGeom_FrictPhys_CundallStrack::go(shared_ptr<IGeom>& ig, shared_ptr<IPhys>& ip, Interaction* contact)
{
	int id1 = contact->getId1(), id2 = contact->getId2();

	CylScGeom* geom = static_cast<CylScGeom*>(ig.get());
	FrictPhys* phys = static_cast<FrictPhys*>(ip.get());
	if (geom->penetrationDepth < 0) {
		if (neverErase) {
			phys->shearForce  = Vector3r::Zero();
			phys->normalForce = Vector3r::Zero();
		} else
			return false;
	}
	if (geom->isDuplicate) {
		if (id2 != geom->trueInt) {
			//cerr<<"skip duplicate "<<id1<<" "<<id2<<endl;
			if (geom->isDuplicate == 2) return false;
		}
	}
	Real& un          = geom->penetrationDepth;
	phys->normalForce = phys->kn * math::max(un, (Real)0) * geom->normal;

	Vector3r&       shearForce = geom->rotate(phys->shearForce);
	const Vector3r& shearDisp  = geom->shearIncrement();
	shearForce -= phys->ks * shearDisp;
	Real maxFs = phys->normalForce.squaredNorm() * math::pow(phys->tangensOfFrictionAngle, 2);

	if (!scene->trackEnergy) { //Update force but don't compute energy terms (see below))
		// PFC3d SlipModel, is using friction angle. CoulombCriterion
		if (shearForce.squaredNorm() > maxFs) {
			Real ratio = sqrt(maxFs) / shearForce.norm();
			shearForce *= ratio;
		}
	} else {
		//almost the same with additional Vector3r instanciated for energy tracing, duplicated block to make sure there is no cost for the instanciation of the vector when traceEnergy==false
		if (shearForce.squaredNorm() > maxFs) {
			Real     ratio      = sqrt(maxFs) / shearForce.norm();
			Vector3r trialForce = shearForce; //store prev force for definition of plastic slip
			//define the plastic work input and increment the total plastic energy dissipated
			shearForce *= ratio;
			Real dissip = ((1 / phys->ks) * (trialForce - shearForce)) /*plastic disp*/.dot(shearForce) /*active force*/;
			if (dissip > 0) scene->energy->add(dissip, "plastDissip", plastDissipIx, /*reset*/ false);
		}
		// compute elastic energy as well
		scene->energy->add(
		        0.5 * (phys->normalForce.squaredNorm() / phys->kn + phys->shearForce.squaredNorm() / phys->ks),
		        "elastPotential",
		        elastPotentialIx,
		        /*reset at every timestep*/ true);
	}
	if (!scene->isPeriodic) {
		Vector3r force = -phys->normalForce - shearForce;
		scene->forces.addForce(id1, force);
		scene->forces.addTorque(id1, (geom->radius1 - 0.5 * geom->penetrationDepth) * geom->normal.cross(force));
		//FIXME : include moment due to axis-contact distance in forces on node
		Vector3r twist = (geom->radius2 - 0.5 * geom->penetrationDepth) * geom->normal.cross(force);
		scene->forces.addForce(id2, (geom->relPos - 1) * force);
		scene->forces.addTorque(id2, (1 - geom->relPos) * twist);
		if (geom->relPos) { //else we are on node (or on last node - and id3 is junk)
			scene->forces.addForce(geom->id3, (-geom->relPos) * force);
			scene->forces.addTorque(geom->id3, geom->relPos * twist);
		}
	}
	// 		applyForceAtContactPoint(-phys->normalForce-shearForce, geom->contactPoint, id1, de1->se3.position, id2, de2->se3.position);
	else { //FIXME : periodicity not implemented here :
		Vector3r force = -phys->normalForce - shearForce;
		scene->forces.addForce(id1, force);
		scene->forces.addForce(id2, -force);
		scene->forces.addTorque(id1, (geom->radius1 - 0.5 * geom->penetrationDepth) * geom->normal.cross(force));
		scene->forces.addTorque(id2, (geom->radius2 - 0.5 * geom->penetrationDepth) * geom->normal.cross(force));
	}
	return true;
}


bool Law2_CylScGeom6D_CohFrictPhys_CohesionMoment::go(shared_ptr<IGeom>& ig, shared_ptr<IPhys>& ip, Interaction* contact)
{
	int id1 = contact->getId1(), id2 = contact->getId2();

	CylScGeom6D*  geom                  = YADE_CAST<CylScGeom6D*>(ig.get());
	CohFrictPhys* currentContactPhysics = YADE_CAST<CohFrictPhys*>(ip.get());

	Vector3r& shearForceFirst = currentContactPhysics->shearForce;           //force tangentielle
	if (contact->isFresh(scene)) shearForceFirst = Vector3r::Zero();         //contact nouveau => force tengentielle = 0,0,0
	Real un = geom->penetrationDepth;                                        //un : interpenetration
	Real Fn = currentContactPhysics->kn * (un - currentContactPhysics->unp); //Fn : force normale
	if (geom->isDuplicate) {
		if (id2 != geom->trueInt) {
			//cerr<<"skip duplicate "<<id1<<" "<<id2<<endl;
			if (geom->isDuplicate == 2) return false;
		}
	}

	if (currentContactPhysics->fragile && (-Fn) > currentContactPhysics->normalAdhesion) {
		// BREAK due to tension
		return false;
	} else {
		if ((-Fn) > currentContactPhysics->normalAdhesion) { //normal plasticity
			Fn                         = -currentContactPhysics->normalAdhesion;
			currentContactPhysics->unp = un + currentContactPhysics->normalAdhesion / currentContactPhysics->kn;
			if (currentContactPhysics->unpMax && currentContactPhysics->unp < currentContactPhysics->unpMax) return false;
		}
		currentContactPhysics->normalForce = Fn * geom->normal;
		Vector3r&       shearForce         = geom->rotate(currentContactPhysics->shearForce);
		const Vector3r& dus                = geom->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 = math::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;
			shearForce *= maxFs;
			if (Fn < 0) currentContactPhysics->normalForce = Vector3r::Zero(); //Vector3r::Zero()
		}
		Vector3r force = -currentContactPhysics->normalForce - shearForce;
		if (!scene->isPeriodic) {
			scene->forces.addForce(id1, force);
			scene->forces.addTorque(id1, (geom->radius1 - 0.5 * geom->penetrationDepth) * geom->normal.cross(force));
			//FIXME : include moment due to axis-contact distance in forces on node
			Vector3r twist = (geom->radius2 - 0.5 * geom->penetrationDepth) * geom->normal.cross(force);
			scene->forces.addForce(id2, (geom->relPos - 1) * force);
			scene->forces.addTorque(id2, (1 - geom->relPos) * twist);
			if (geom->relPos) { //else we are on node (or on last node - and id3 is junk)
				scene->forces.addForce(geom->id3, (-geom->relPos) * force);
				scene->forces.addTorque(geom->id3, geom->relPos * twist);
			}
		}
		// 		applyForceAtContactPoint(-phys->normalForce-shearForce, geom->contactPoint, id1, de1->se3.position, id2, de2->se3.position);
		else { //FIXME : periodicity not implemented here :
			scene->forces.addForce(id1, force);
			scene->forces.addForce(id2, -force);
			scene->forces.addTorque(id1, (geom->radius1 - 0.5 * geom->penetrationDepth) * geom->normal.cross(force));
			scene->forces.addTorque(id2, (geom->radius2 - 0.5 * geom->penetrationDepth) * geom->normal.cross(force));
		}
		//applyForceAtContactPoint(-currentContactPhysics->normalForce-shearForce, geom->contactPoint, id1, de1->se3.position, id2, de2->se3.position);
	}
	return true;
}


bool Law2_ChCylGeom6D_CohFrictPhys_CohesionMoment::go(shared_ptr<IGeom>& ig, shared_ptr<IPhys>& ip, Interaction* contact)
{
	int id1 = contact->getId1(), id2 = contact->getId2();

	ChCylGeom6D*  geom                  = YADE_CAST<ChCylGeom6D*>(ig.get());
	CohFrictPhys* currentContactPhysics = YADE_CAST<CohFrictPhys*>(ip.get());

	/*
    shared_ptr<const ChainedState> state1 = YADE_PTR_CAST<const ChainedState> (Body::byId(id1,scene)->state);
    const shared_ptr<Interaction> intr = scene->interactions->find(id1,id2+1);
    if(!intr) {cout<<"Skipping contact because collider didn't found the next cylinder."<<endl;return false;}
    intr->geom = c->geom;
    intr->phys = c->phys;
    */

	Vector3r& shearForceFirst = currentContactPhysics->shearForce;           //force tangentielle
	if (contact->isFresh(scene)) shearForceFirst = Vector3r::Zero();         //contact nouveau => force tengentielle = 0,0,0
	Real un = geom->penetrationDepth;                                        //un : interpenetration
	Real Fn = currentContactPhysics->kn * (un - currentContactPhysics->unp); //Fn : force normale

	if (currentContactPhysics->fragile && (-Fn) > currentContactPhysics->normalAdhesion) return false; // BREAK due to tension
	else {
		if ((-Fn) > currentContactPhysics->normalAdhesion) { //normal plasticity
			Fn                         = -currentContactPhysics->normalAdhesion;
			currentContactPhysics->unp = un + currentContactPhysics->normalAdhesion / currentContactPhysics->kn;
			if (currentContactPhysics->unpMax && currentContactPhysics->unp < currentContactPhysics->unpMax) return false;
		}


		currentContactPhysics->normalForce = Fn * geom->normal;
		Vector3r&       shearForce         = geom->rotate(currentContactPhysics->shearForce);
		const Vector3r& dus                = geom->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 = math::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;
			shearForce *= maxFs;
			if (Fn < 0) currentContactPhysics->normalForce = Vector3r::Zero(); //Vector3r::Zero()
		}


		Vector3r force = -currentContactPhysics->normalForce - shearForce;
		//cout<<"id1="<<contact->getId1()<<" id2="<<contact->getId2()<<" normalForce="<<currentContactPhysics->normalForce<<" shearForce="<<shearForce<<endl;
		if (!scene->isPeriodic) {
			Vector3r twist1 = (geom->radius1 - 0.5 * geom->penetrationDepth) * geom->normal.cross(force);
			Vector3r twist2 = (geom->radius2 - 0.5 * geom->penetrationDepth) * geom->normal.cross(force);
			scene->forces.addForce(id1, (1 - geom->relPos1) * force);
			scene->forces.addTorque(id1, (1 - geom->relPos1) * twist1);

			scene->forces.addForce(id2, -(1 - geom->relPos2) * force);
			scene->forces.addTorque(id2, (1 - geom->relPos2) * twist2);

			scene->forces.addForce(id1 + 1, geom->relPos1 * force);
			scene->forces.addTorque(id1 + 1, geom->relPos1 * twist1);

			scene->forces.addForce(id2 + 1, -geom->relPos2 * force);
			scene->forces.addTorque(id2 + 1, geom->relPos2 * twist2);
		}
		// 		applyForceAtContactPoint(-phys->normalForce-shearForce, geom->contactPoint, id1, de1->se3.position, id2, de2->se3.position);
		else { //FIXME : periodicity not implemented here :
			scene->forces.addForce(id1, force);
			scene->forces.addForce(id2, -force);
			scene->forces.addTorque(id1, (geom->radius1 - 0.5 * geom->penetrationDepth) * geom->normal.cross(force));
			scene->forces.addTorque(id2, (geom->radius2 - 0.5 * geom->penetrationDepth) * geom->normal.cross(force));
		}
	}
	return true;
}

} // namespace yade