// © 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_Ig2.hpp"

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

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

YADE_PLUGIN(/* self-contained in hpp: */ (Ig2_Polyhedra_Polyhedra_PolyhedraGeom)(Ig2_Wall_Polyhedra_PolyhedraGeom)(Ig2_Facet_Polyhedra_PolyhedraGeom)(
        Ig2_Sphere_Polyhedra_ScGeom)(Ig2_Polyhedra_Polyhedra_ScGeom)(Ig2_Polyhedra_Polyhedra_PolyhedraGeomOrScGeom));

CREATE_LOGGER(Ig2_Polyhedra_Polyhedra_PolyhedraGeom);
CREATE_LOGGER(Ig2_Wall_Polyhedra_PolyhedraGeom);
CREATE_LOGGER(Ig2_Facet_Polyhedra_PolyhedraGeom);
CREATE_LOGGER(Ig2_Sphere_Polyhedra_ScGeom);
CREATE_LOGGER(Ig2_Polyhedra_Polyhedra_ScGeom);
CREATE_LOGGER(Ig2_Polyhedra_Polyhedra_PolyhedraGeomOrScGeom);

//**********************************************************************************
/*! Create Polyhedra (collision geometry) from colliding Polyhedras. */

bool Ig2_Polyhedra_Polyhedra_PolyhedraGeom::go(
        const shared_ptr<Shape>& shape1,
        const shared_ptr<Shape>& shape2,
        const State&             state1,
        const State&             state2,
        const Vector3r&          shift2,
        const bool& /*force*/,
        const shared_ptr<Interaction>& interaction)
{
	//get polyhedras
	const Se3r& se31 = state1.se3;
	const Se3r& se32 = state2.se3;
	Polyhedra*  A    = static_cast<Polyhedra*>(shape1.get());
	Polyhedra*  B    = static_cast<Polyhedra*>(shape2.get());

	bool isNew = !interaction->geom;

	//move and rotate 1st the CGAL structure Polyhedron
	Matrix3r    rot_mat   = (se31.orientation).toRotationMatrix();
	Vector3r    trans_vec = se31.position;
	const Real& s         = interactionDetectionFactor;

	Transformation t_rot_trans(
	        s * rot_mat(0, 0),
	        s * rot_mat(0, 1),
	        s * rot_mat(0, 2),
	        trans_vec[0],
	        s * rot_mat(1, 0),
	        s * rot_mat(1, 1),
	        s * rot_mat(1, 2),
	        trans_vec[1],
	        s * rot_mat(2, 0),
	        s * rot_mat(2, 1),
	        s * rot_mat(2, 2),
	        trans_vec[2],
	        1.);

	Polyhedron PA = A->GetPolyhedron();
	std::transform(PA.points_begin(), PA.points_end(), PA.points_begin(), t_rot_trans);

	std::transform(PA.facets_begin(), PA.facets_end(), PA.planes_begin(), Plane_equation());

	//move and rotate 2nd the CGAL structure Polyhedron
	rot_mat     = (se32.orientation).toRotationMatrix();
	trans_vec   = se32.position + shift2;
	t_rot_trans = Transformation(
	        s * rot_mat(0, 0),
	        s * rot_mat(0, 1),
	        s * rot_mat(0, 2),
	        trans_vec[0],
	        s * rot_mat(1, 0),
	        s * rot_mat(1, 1),
	        s * rot_mat(1, 2),
	        trans_vec[1],
	        s * rot_mat(2, 0),
	        s * rot_mat(2, 1),
	        s * rot_mat(2, 2),
	        trans_vec[2],
	        1.);

	Polyhedron PB = B->GetPolyhedron();

	std::transform(PB.points_begin(), PB.points_end(), PB.points_begin(), t_rot_trans);
	std::transform(PB.facets_begin(), PB.facets_end(), PB.planes_begin(), Plane_equation());

	shared_ptr<PolyhedraGeom> bang;

	if (isNew) {
		// new interaction
		bang = shared_ptr<PolyhedraGeom>(new PolyhedraGeom());
		bang->sep_plane.assign(3, 0);
		bang->contactPoint = Vector3r(0, 0, 0);
		bang->isShearNew   = true;
	} else {
		// use data from old interaction
		bang             = YADE_PTR_CAST<PolyhedraGeom>(interaction->geom);
		bang->isShearNew = bang->equivalentPenetrationDepth <= 0;
	}

	//find intersection Polyhedra
	Polyhedron Int;
	Int = Polyhedron_Polyhedron_intersection(
	        PA, PB, ToCGALPoint(bang->contactPoint), ToCGALPoint(se31.position), ToCGALPoint(se32.position + shift2), bang->sep_plane);

	//volume and centroid of intersection
	Real     volume;
	Vector3r centroid;
	P_volume_centroid(Int, &volume, &centroid);

	if (math::isnan(volume) || volume <= 1E-25 || volume > min(A->GetVolume(), B->GetVolume())) {
		bang->equivalentPenetrationDepth = 0;
		bang->penetrationVolume          = min(A->GetVolume(), B->GetVolume());
		bang->normal                     = (A->GetVolume() > B->GetVolume() ? 1 : -1) * (se32.position + shift2 - se31.position);
		return !isNew;
	}

	if ((!Is_inside_Polyhedron(PA, ToCGALPoint(centroid))) or (!Is_inside_Polyhedron(PB, ToCGALPoint(centroid)))) {
		bang->equivalentPenetrationDepth = 0;
		return !isNew;
	}

	if (isNew) interaction->geom = bang;

	//find normal direction
	Vector3r normal = FindNormal(Int, PA, PB);

	if ((se32.position + shift2 - centroid).dot(normal) < 0) normal *= -1;

	Real area = math::pow(volume, 2. / 3.);
	// store calculated stuff in bang; some is redundant
	bang->equivalentCrossSection     = area;
	bang->contactPoint               = centroid;
	bang->penetrationVolume          = volume;
	bang->equivalentPenetrationDepth = volume / area;
	scene                            = Omega::instance().getScene().get();
	bang->precompute(state1, state2, scene, interaction, normal, bang->isShearNew, shift2);
	bang->normal = normal;

	/*
	FILE * fin = fopen("Interactions.dat","a");
	fprintf(fin,"************** IDS %d %d **************\n",interaction->id1, interaction->id2);
	fprintf(fin,"volume\t%e\n",volume);	
	fprintf(fin,"centroid\t%e\t%e\t%e\n",centroid[0],centroid[1],centroid[2]);
	PrintPolyhedron2File(PA,fin);
	PrintPolyhedron2File(PB,fin);
	PrintPolyhedron2File(Int,fin);
	fclose(fin);
	*/
	return true;
}

//**********************************************************************************

bool Ig2_Polyhedra_Polyhedra_PolyhedraGeom::goReverse(
        const shared_ptr<Shape>&       shape1,
        const shared_ptr<Shape>&       shape2,
        const State&                   state1,
        const State&                   state2,
        const Vector3r&                shift2,
        const bool&                    force,
        const shared_ptr<Interaction>& c)
{
	return go(shape1, shape2, state2, state1, -shift2, force, c);
}
//**********************************************************************************
/*! Create Polyhedra (collision geometry) from colliding Polyhedron and Wall. */

bool Ig2_Wall_Polyhedra_PolyhedraGeom::go(
        const shared_ptr<Shape>& shape1,
        const shared_ptr<Shape>& shape2,
        const State&             state1,
        const State&             state2,
        const Vector3r&          shift2,
        const bool& /*force*/,
        const shared_ptr<Interaction>& interaction)
{
	const int&  PA(shape1->cast<Wall>().axis);
	const int&  sense(shape1->cast<Wall>().sense);
	const Se3r& se31 = state1.se3;
	const Se3r& se32 = state2.se3;
	Polyhedra*  B    = static_cast<Polyhedra*>(shape2.get());

	bool isNew = !interaction->geom;

	//move and rotate also the CGAL structure Polyhedron
	Matrix3r rot_mat   = (se32.orientation).toRotationMatrix();
	Vector3r trans_vec = se32.position;

	Transformation t_rot_trans(
	        rot_mat(0, 0),
	        rot_mat(0, 1),
	        rot_mat(0, 2),
	        trans_vec[0],
	        rot_mat(1, 0),
	        rot_mat(1, 1),
	        rot_mat(1, 2),
	        trans_vec[1],
	        rot_mat(2, 0),
	        rot_mat(2, 1),
	        rot_mat(2, 2),
	        trans_vec[2],
	        1.);
	Polyhedron PB = B->GetPolyhedron();
	std::transform(PB.points_begin(), PB.points_end(), PB.points_begin(), t_rot_trans);
	std::transform(PB.facets_begin(), PB.facets_end(), PB.planes_begin(), Plane_equation());

	//move wall
	Vector3r normal = Vector3r(0, 0, 0);
	if (sense != 0) normal[PA] = sense;
	else
		normal[PA] = se32.position[PA] - se31.position[PA] > 0 ? 1 : -1;
	CGALvector CGALnormal = CGALvector(normal[0], normal[1], normal[2]);
	Plane      A          = Plane(CGALpoint(se31.position[0], se31.position[1], se31.position[2]), CGALvector(normal[0], normal[1], normal[2]));

	shared_ptr<PolyhedraGeom> bang;
	if (isNew) {
		// new interaction
		bang               = shared_ptr<PolyhedraGeom>(new PolyhedraGeom());
		bang->contactPoint = Vector3r(0, 0, 0);
		bang->isShearNew   = true;
		interaction->geom  = bang;
	} else {
		// use data from old interaction
		bang             = YADE_PTR_CAST<PolyhedraGeom>(interaction->geom);
		bang->isShearNew = bang->equivalentPenetrationDepth <= 0;
	}

	//find intersection Polyhedra
	Polyhedron Int;
	Int = Polyhedron_Plane_intersection(PB, A, ToCGALPoint(se32.position), ToCGALPoint(bang->contactPoint));

	//volume and centroid of intersection
	Real     volume;
	Vector3r centroid;
	P_volume_centroid(Int, &volume, &centroid);
	if (math::isnan(volume) || volume <= 1E-25 || volume > B->GetVolume()) {
		bang->equivalentPenetrationDepth = 0;
		return true;
	}
	if (!Is_inside_Polyhedron(PB, ToCGALPoint(centroid))) {
		bang->equivalentPenetrationDepth = 0;
		return true;
	}

	//calculate area of projection of Intersection into the normal plane
	Real area = volume / 1E-8;

	// store calculated stuff in bang; some is redundant
	bang->equivalentCrossSection     = area;
	bang->contactPoint               = centroid;
	bang->penetrationVolume          = volume;
	bang->equivalentPenetrationDepth = volume / area;
	bang->precompute(state1, state2, scene, interaction, normal, isNew, shift2);
	bang->normal = FromCGALVector(CGALnormal);

	return true;
}

//**********************************************************************************
/*! Create Polyhedra (collision geometry) from colliding Polyhedron and Facet. */

bool Ig2_Facet_Polyhedra_PolyhedraGeom::go(
        const shared_ptr<Shape>& shape1,
        const shared_ptr<Shape>& shape2,
        const State&             state1,
        const State&             state2,
        const Vector3r&          shift2,
        const bool& /*force*/,
        const shared_ptr<Interaction>& interaction)
{
	const Se3r& se31 = state1.se3;
	const Se3r& se32 = state2.se3;
	Facet*      A    = static_cast<Facet*>(shape1.get());
	Polyhedra*  B    = static_cast<Polyhedra*>(shape2.get());

	bool isNew = !interaction->geom;

	//move and rotate 1st the CGAL structure Polyhedron
	Matrix3r       rot_mat   = (se32.orientation).toRotationMatrix();
	Vector3r       trans_vec = se32.position;
	Transformation t_rot_trans(
	        rot_mat(0, 0),
	        rot_mat(0, 1),
	        rot_mat(0, 2),
	        trans_vec[0],
	        rot_mat(1, 0),
	        rot_mat(1, 1),
	        rot_mat(1, 2),
	        trans_vec[1],
	        rot_mat(2, 0),
	        rot_mat(2, 1),
	        rot_mat(2, 2),
	        trans_vec[2],
	        1.);
	Polyhedron PB = B->GetPolyhedron();
	std::transform(PB.points_begin(), PB.points_end(), PB.points_begin(), t_rot_trans);
	std::transform(PB.facets_begin(), PB.facets_end(), PB.planes_begin(), Plane_equation());

	//move and rotate facet
	vector<CGALpoint> v;
	v.resize(6);
	for (int i = 0; i < 3; i++)
		v[i] = ToCGALPoint(se31.orientation * A->vertices[i] + se31.position); // vertices in global coordinates

	//determine
	CGALpoint  f_center((v[0].x() + v[1].x() + v[2].x()) / 3., (v[0].y() + v[1].y() + v[2].y()) / 3., (v[0].z() + v[1].z() + v[2].z()) / 3.);
	CGALvector f_normal = CGAL::cross_product((v[1] - v[0]), (v[2] - v[0]));

	//chage normal + change order of vertices to get outwarding facet normal for initial tetrahedron
	if ((ToCGALPoint(se32.position) - f_center) * (f_normal) < 0) {
		f_normal = (-1) * f_normal;
		CGALpoint help;
		help = v[0];
		v[0] = v[1];
		v[1] = help;
	}

	Real f_area = sqrt(f_normal.squared_length());
	for (int i = 3; i < 6; i++)
		v[i] = v[i - 3] - f_normal / f_area * 0.05 * sqrt(f_area); // vertices in global coordinates

	Polyhedron PA;

	//construct polyhedron directly
	Polyhedron::Halfedge_iterator hei = PA.make_tetrahedron(v[4], v[1], v[0], v[2]);
	hei                               = PA.split_vertex(hei, hei->next()->opposite());
	hei->vertex()->point()            = v[3];
	hei                               = PA.split_facet(hei->next()->next()->next(), hei->next());
	hei                               = PA.split_vertex(hei, hei->next_on_vertex()->next_on_vertex());
	hei->vertex()->point()            = v[5];

	shared_ptr<PolyhedraGeom> bang;
	if (isNew) {
		// new interaction
		bang = shared_ptr<PolyhedraGeom>(new PolyhedraGeom());
		bang->sep_plane.assign(3, 0);
		bang->contactPoint = Vector3r(0, 0, 0);
		bang->isShearNew   = true;
		interaction->geom  = bang;
	} else {
		// use data from old interaction
		bang             = YADE_PTR_CAST<PolyhedraGeom>(interaction->geom);
		bang->isShearNew = bang->equivalentPenetrationDepth <= 0;
	}

	//find intersection Polyhedra
	Polyhedron Int;
	Int = Polyhedron_Polyhedron_intersection(
	        PA, PB, ToCGALPoint(bang->contactPoint), ToCGALPoint(se31.position), ToCGALPoint(se32.position), bang->sep_plane);

	//volume and centroid of intersection
	Real     volume;
	Vector3r centroid;
	P_volume_centroid(Int, &volume, &centroid);
	if (math::isnan(volume) || volume <= 1E-25 || volume > B->GetVolume()) {
		bang->equivalentPenetrationDepth = 0;
		return true;
	}
	if (!Is_inside_Polyhedron(PB, ToCGALPoint(centroid))) {
		bang->equivalentPenetrationDepth = 0;
		return true;
	}

	//find normal direction
	Vector3r normal = FindNormal(Int, PA, PB);
	if ((se32.position - centroid).dot(normal) < 0) normal *= -1;

	//calculate area of projection of Intersection into the normal plane
	Real area = volume / 1E-8;

	// store calculated stuff in bang; some is redundant
	bang->equivalentCrossSection     = area;
	bang->contactPoint               = centroid;
	bang->penetrationVolume          = volume;
	bang->equivalentPenetrationDepth = volume / area;
	bang->precompute(state1, state2, scene, interaction, normal, bang->isShearNew, shift2);
	bang->normal = normal;

	return true;
}

//**********************************************************************************
/*! Create Polyhedra (collision geometry) from colliding Polyhedron and Sphere. */

bool Ig2_Sphere_Polyhedra_ScGeom::go(
        const shared_ptr<Shape>& shape1,
        const shared_ptr<Shape>& shape2,
        const State&             state1,
        const State&             state2,
        const Vector3r&          shift2,
        const bool& /*force*/,
        const shared_ptr<Interaction>& interaction)
{
	const Se3r& se31 = state1.se3;
	const Se3r& se32 = state2.se3;

	Sphere*    A      = static_cast<Sphere*>(shape1.get());
	Real       radius = A->radius;
	Real       r2     = radius * radius;
	Polyhedra* B      = static_cast<Polyhedra*>(shape2.get());

	bool               isNew = !interaction->geom;
	shared_ptr<ScGeom> geom;

	// use polyhedron surface triangulation
	const std::vector<int> faceTri = B->GetSurfaceTriangulation();
	// get vertices in global coordinate system
	std::vector<Vector3r> pts(B->v);
	for (unsigned int i = 0; i < pts.size(); i++)
		pts[i] = se32.position + se32.orientation * pts[i];

	//******************************************************************************
	// find the closest point of polyhedron to the sphere center, see following paper for notation
	// http://automatica.dei.unipd.it/public/Schenato/PSC/2010_2011/gruppo4-Building_termo_identification/IdentificazioneTermodinamica20062007/eggpsc/Simulatore%20termico/Generazione%20Mesh/1995%20-%203D%20Distance%20from%20a%20Point%20to%20a%20Triangle.pdf
	bool            isInside = true; // if center is inside polyhedron
	const Vector3r& p0       = se31.position;
	const Vector3r& center   = p0;
	Vector3r        closest  = Vector3r::Zero(); // closest point to be found
	Real            dst2min  = DBL_MAX;          // minimal squared distance (large number initially)
	// auxiliary value
	Vector3r p1, p2, p3, e1, e2, e3, e, n, p0a, p10, p20, p30, v1, v2, v3, pa, pb, r, p0aa, pp, ppp;
	pp = Vector3r::Zero();
	Real                  dst, t, en, o1, o2, o3, dst2, edst2, edst2min;
	PointTriangleRelation rel = none, relTemp = none, relTemp2;
	for (unsigned int i = 0; i < faceTri.size() / 3; i++) { // iterate over all triangles...
		// triangle vertices
		p1 = pts[faceTri[3 * i + 0]];
		p2 = pts[faceTri[3 * i + 1]];
		p3 = pts[faceTri[3 * i + 2]];
		// triangle edges vectors
		e1  = p2 - p1;
		e2  = p3 - p2;
		e3  = p1 - p3;
		n   = (e1.cross(-e3)).normalized(); // tirangle outer normal
		dst = (p0 - p1).dot(n);             // oriented distance of p0 to triangle plane
		if (dst > 0) isInside = false;      // p0 lies in positive halfspace of triangle, cannot be inside
		p0a = p0 - dst * n;                 // p0 projected to triangle plane
		p10 = p0a - p1;
		p20 = p0a - p2;
		p30 = p0a - p3;
		// inside/outside with respect to individual edges
		o1 = (p10.cross(p20)).dot(n);
		o2 = (p20.cross(p30)).dot(n);
		o3 = (p30.cross(p10)).dot(n);
		if (o1 > 0 && o2 > 0 && o3 > 0) { // p0a is inside triangle
			pp      = p0a;
			relTemp = inside;
		} else {
			edst2min = DBL_MAX;
			for (int j = 0; j < 3; j++) { // iterate over 3 edges end find the closest point of them
				pa   = j == 0 ? p1 : (j == 1 ? p2 : p3);
				pb   = j == 0 ? p2 : (j == 1 ? p3 : p1);
				r    = (((pb - p0a).cross(pa - p0a)).cross(pb - pa)).normalized();
				p0aa = p0a + (pa - p0a).dot(r) * r; // projection of p0a on the edge
				e    = pb - pa;
				en   = e.norm();
				e /= en;
				t = (p0aa - pa).dot(e) / en; // parameter of edge line
				if (t < 0.) {
					ppp      = pa;
					relTemp2 = vertex;
				} else if (t > 1.) {
					ppp      = pb;
					relTemp2 = vertex;
				} else {
					ppp      = p0aa;
					relTemp2 = edge;
				}
				edst2 = (ppp - p0).squaredNorm();
				if (edst2 < edst2min) {
					edst2min = edst2;
					pp       = ppp;
					relTemp  = relTemp2;
				}
			}
		}
		dst2 = (pp - p0).squaredNorm();
		if (dst2 < dst2min) { // dst2 is now the minimal squared distance and pp is now the closest found point so far
			dst2min = dst2;
			closest = pp;
			rel     = relTemp;
		}
	}
	//******************************************************************************

	if (isNew && !(isInside || dst2min <= r2)) return false;

	if (isNew) {
		geom          = shared_ptr<ScGeom>(new ScGeom());
		geom->radius1 = geom->radius2 = radius;
		interaction->geom             = geom;
	} else {
		geom = YADE_PTR_CAST<ScGeom>(interaction->geom);
	}

	Real     penetrationDepth;
	Vector3r normal, contactPoint;
	if (isInside) { // some artificial tricks for unlikely case
		normal           = se32.position - center;
		dst              = normal.norm();
		penetrationDepth = radius;
		normal /= dst;
		contactPoint = center + normal * (.5 * radius);
	} else {
		if (rel == none) { LOG_FATAL("TODO"); }
		Real coeff = rel == edge ? edgeCoeff : (rel == vertex ? vertexCoeff : 1.0);
		normal     = closest - center;
		dst        = normal.norm();
		normal /= dst;
		penetrationDepth = radius - dst;
		contactPoint     = center + normal * (radius - .5 * penetrationDepth);
		penetrationDepth *= coeff;
	}
	geom->normal           = normal;
	geom->penetrationDepth = penetrationDepth;
	geom->contactPoint     = contactPoint;

	geom->precompute(state1, state2, scene, interaction, normal, isNew, shift2, false /*avoidGranularRatcheting only for sphere-sphere*/);

	return true;
}


//**********************************************************************************
/*! Plyhedra -> ScGeom. */
bool Ig2_Polyhedra_Polyhedra_ScGeom::go(
        const shared_ptr<Shape>&       shape1,
        const shared_ptr<Shape>&       shape2,
        const State&                   state1,
        const State&                   state2,
        const Vector3r&                shift2,
        const bool&                    force,
        const shared_ptr<Interaction>& interaction)
{
	const Se3r&        se31 = state1.se3;
	const Se3r&        se32 = state2.se3;
	shared_ptr<ScGeom> geom;
	bool               isNew = !interaction->geom;
	if (isNew) {
		Ig2_Polyhedra_Polyhedra_PolyhedraGeom ppGeom = Ig2_Polyhedra_Polyhedra_PolyhedraGeom();
		ppGeom.interactionDetectionFactor            = interactionDetectionFactor;
		bool pp                                      = ppGeom.go(shape1, shape2, state2, state1, shift2, force, interaction);
		if (!pp) { return false; }
		shared_ptr<PolyhedraGeom> pGeom = YADE_PTR_CAST<PolyhedraGeom>(interaction->geom);
		geom                            = shared_ptr<ScGeom>(new ScGeom());
		geom->radius1                   = (pGeom->contactPoint - se31.position).norm();
		geom->radius2                   = (pGeom->contactPoint - se32.position + shift2).norm();
		interaction->geom               = geom;
	} else {
		geom = YADE_PTR_CAST<ScGeom>(interaction->geom);
	}
	const Real& radius1 = geom->radius1;
	const Real& radius2 = geom->radius2;
	Vector3r    normal  = (se32.position + shift2) - se31.position;
	Real        norm    = normal.norm();
	normal /= norm; // normal is unit vector now
	Real penetrationDepth  = radius1 + radius2 - norm;
	geom->contactPoint     = se31.position + (radius1 - 0.5 * penetrationDepth) * normal; //0.5*(pt1+pt2);
	geom->penetrationDepth = penetrationDepth;
	scene                  = Omega::instance().getScene().get();
	geom->precompute(state1, state2, scene, interaction, normal, isNew, shift2, false);
	return true;
}

bool Ig2_Polyhedra_Polyhedra_ScGeom::goReverse(
        const shared_ptr<Shape>&       shape1,
        const shared_ptr<Shape>&       shape2,
        const State&                   state1,
        const State&                   state2,
        const Vector3r&                shift2,
        const bool&                    force,
        const shared_ptr<Interaction>& interaction)
{
	return go(shape1, shape2, state2, state1, -shift2, force, interaction);
}


bool Ig2_Polyhedra_Polyhedra_PolyhedraGeomOrScGeom::go(
        const shared_ptr<Shape>&       shape1,
        const shared_ptr<Shape>&       shape2,
        const State&                   state1,
        const State&                   state2,
        const Vector3r&                shift2,
        const bool&                    force,
        const shared_ptr<Interaction>& interaction)
{
	bool isNew = !interaction->geom;
	if (isNew) {
		if (createScGeom) {
			return ig2scGeom->go(shape1, shape2, state1, state2, shift2, force, interaction);
		} else {
			return ig2polyhedraGeom->go(shape1, shape2, state1, state2, shift2, force, interaction);
		}
	}
	//
	ScGeom* scGeom = dynamic_cast<ScGeom*>(interaction->geom.get());
	if (scGeom) { return ig2scGeom->go(shape1, shape2, state1, state2, shift2, force, interaction); }
	//
	PolyhedraGeom* pGeom = dynamic_cast<PolyhedraGeom*>(interaction->geom.get());
	if (pGeom) { return ig2polyhedraGeom->go(shape1, shape2, state1, state2, shift2, force, interaction); }
	//
	LOG_ERROR("TODO, should not happen");
	return false;
}

bool Ig2_Polyhedra_Polyhedra_PolyhedraGeomOrScGeom::goReverse(
        const shared_ptr<Shape>&       shape1,
        const shared_ptr<Shape>&       shape2,
        const State&                   state1,
        const State&                   state2,
        const Vector3r&                shift2,
        const bool&                    force,
        const shared_ptr<Interaction>& interaction)
{
	return go(shape1, shape2, state2, state1, -shift2, force, interaction);
}

} // namespace yade

#endif // YADE_CGAL
