Index: cpp/frams/genetics/fS/part_distance_estimator.h
===================================================================
--- cpp/frams/genetics/fS/part_distance_estimator.h	(revision 1006)
+++ cpp/frams/genetics/fS/part_distance_estimator.h	(revision 1017)
@@ -5,4 +5,6 @@
 #ifndef _PART_DISTANCE_ESTIMATOR_H_
 #define _PART_DISTANCE_ESTIMATOR_H_
+
+#include "frams/model/geometry/meshbuilder.h"
 
 class fS_Utils
@@ -44,146 +46,45 @@
 class PartDistanceEstimator
 {
-	/**
-	 * Used in finding the proper distance between the parts
-	 * distance between spheres / sphere radius
-	 * That default value can be changed in certain cases
-	 * */
-	static constexpr float SPHERE_RELATIVE_DISTANCE = 0.5;
-	/**
-	 * Used in finding the proper distance between the parts
-	 * The maximal allowed value for
-	 * maximal radius of the node / sphere radius
-	 */
-	static const int MAX_DIAMETER_QUOTIENT = 30;
-	/**
-	 * The tolerance of the value of distance between parts
-	 */
-	static constexpr double SPHERE_DISTANCE_TOLERANCE = 0.96;
 
-	static constexpr double SPHERE_SIZE_QUOTIENT = 0.25;
+public:
+	static constexpr double PRECISION = 0.05;
+	static constexpr double RELATIVE_DENSITY = 5.0;
 
 
-	static bool isInsidePart(Part::Shape shape, const Pt3D &partRadii, const Pt3D &center, double sphereRadius)
+	static Part *buildTemporaryPart(Part::Shape shape, const Pt3D &scale, const Pt3D &rotations)
 	{
-		if(sphereRadius >= fS_Utils::min3(partRadii))
-			return true; // TODO Special case
-		Pt3D mostRemote = Pt3D(fabs(center.x), fabs(center.y), fabs(center.z)) + Pt3D(sphereRadius);
-		bool isInEllipsis;
-
-		bool result = false;
-
-		switch (shape)
-		{
-			case Part::Shape::SHAPE_CUBOID:
-				if(mostRemote.x <= partRadii.x && mostRemote.y <= partRadii.y && mostRemote.z <= partRadii.z)
-					result = true;
-				break;
-			case Part::Shape::SHAPE_CYLINDER:
-				isInEllipsis = pow(center.y / (partRadii.y - sphereRadius), 2) + pow(center.z / (partRadii.z - sphereRadius), 2) <= 1.0;
-				if (mostRemote.x <= partRadii.x && isInEllipsis)
-					result = true;
-				break;
-			case Part::Shape::SHAPE_ELLIPSOID:
-				if(pow(center.x / (partRadii.x - sphereRadius), 2) + pow(center.y / (partRadii.y - sphereRadius), 2) + pow(center.z / (partRadii.z - sphereRadius), 2) <= 1.0)
-					result = true;
-				break;
-			default:
-				logMessage("fS", "calculateVolume", LOG_ERROR, "Invalid part type");
-		}
-		return result;
+		Part *tmpPart = new Part(shape);
+		tmpPart->scale = scale;
+		tmpPart->setRot(rotations);
+		return tmpPart;
 	}
 
-public:
-	static vector<Pt3D> findSphereCenters(Part::Shape shape, double &sphereRadius, const Pt3D &radii, const Pt3D &rotations)
+	static vector <Pt3D> findSphereCenters(Part *part)
 	{
-		double sphereRelativeDistance = SPHERE_RELATIVE_DISTANCE;
-		double minRadius = fS_Utils::min3(radii);
-		if(minRadius <= 0)
-			throw fS_Exception("Invalid part size in PartDistanceEstimator", 0);
-		double maxRadius = fS_Utils::max3(radii);
-		sphereRadius = SPHERE_SIZE_QUOTIENT * minRadius;
-		if (MAX_DIAMETER_QUOTIENT < maxRadius / sphereRadius)
+		// Divide by maximal radius to avoid long computations
+		MeshBuilder::PartSurface surface(RELATIVE_DENSITY / fS_Utils::max3(part->scale));
+		surface.initialize(part);
+
+		vector <Pt3D> centers;
+		Pt3D point;
+		while (surface.tryGetNext(point))
 		{
-			// When max radius is much bigger than sphereRadius and there are to many spheresZ
-			sphereRelativeDistance = 1.0;   // Make the spheres adjacent to speed up the computation
-			sphereRadius = maxRadius / MAX_DIAMETER_QUOTIENT;
-		}
-		else if(shape == Part::Shape::SHAPE_ELLIPSOID)
-			sphereRadius = minRadius;
-
-		double sphereDiameter = 2 * sphereRadius;
-
-		double radiiArr[3]{radii.x, radii.y, radii.z};
-		vector<double> coordinates[3];
-		for (int dim = 0; dim < 3; dim++)
-		{
-			double spaceForSphereCenters = 2 * radiiArr[dim] - sphereDiameter;
-			if (spaceForSphereCenters > 0)
-			{
-				int lastIndex = ceil(spaceForSphereCenters / (sphereDiameter * sphereRelativeDistance));
-				for (int i = 0; i <= lastIndex; i++)
-				{
-					coordinates[dim].push_back(spaceForSphereCenters * (double(i) / lastIndex - 0.5));
-				}
-			}
-			else
-				coordinates[dim].push_back(0.0);
-		}
-
-		vector<Pt3D> centers;
-		for (int xi = 0; xi < int(coordinates[0].size()); xi++)
-		{
-			for (int yi = 0; yi < int(coordinates[1].size()); yi++)
-			{
-				for (int zi = 0; zi < int(coordinates[2].size()); zi++)
-				{
-						Pt3D center = Pt3D(coordinates[0][xi], coordinates[1][yi], coordinates[2][zi]);
-						if (isInsidePart(shape, radii, center, sphereRadius))
-						{
-							fS_Utils::rotateVector(center, rotations);
-							centers.push_back(center);
-						}
-				}
-			}
+			centers.push_back(point);
 		}
 		return centers;
 	}
 
-	static int isCollision(vector<Pt3D> &centersParent, vector<Pt3D> &centers, Pt3D &vectorBetweenParts,
-					double distanceThreshold)
+	static bool isCollision(Part *parentPart, vector <Pt3D> &centers, Pt3D &vectorBetweenParts)
 	{
-		double upperThresholdSq = distanceThreshold * distanceThreshold;
-		double lowerThresholdSq = pow(SPHERE_DISTANCE_TOLERANCE * distanceThreshold, 2);
-		double distanceSq;
-		double dx, dy, dz;
-		bool existsAdjacent = false;
-		Pt3D *tmpPoint;
-		for (int sc = 0; sc < int(centers.size()); sc++)
+		static double CBRT_3 = std::cbrt(3);
+		double maxParentReachSq = pow(CBRT_3 * fS_Utils::max3(parentPart->scale), 2);
+		for (int i = 0; i < int(centers.size()); i++)
 		{
-			Pt3D shiftedSphere = Pt3D(centers[sc]);
-			shiftedSphere += vectorBetweenParts;
-			for (int psc = 0; psc < int(centersParent.size()); psc++)
-			{
-				tmpPoint = &centersParent[psc];
-				dx = shiftedSphere.x - tmpPoint->x;
-				dy = shiftedSphere.y - tmpPoint->y;
-				dz = shiftedSphere.z - tmpPoint->z;
-				distanceSq = dx * dx + dy * dy + dz * dz;
-
-				if (distanceSq <= upperThresholdSq)
-				{
-					if (distanceSq >= lowerThresholdSq)
-						existsAdjacent = true;
-					else
-					{
-						return COLLISION;
-					}
-				}
-			}
+			Pt3D shifted = centers[i] + vectorBetweenParts;
+			double distanceToCenterSq = shifted.x * shifted.x + shifted.y * shifted.y + shifted.z * shifted.z;
+			if (distanceToCenterSq <= maxParentReachSq && GeometryUtils::isPointInsidePart(shifted, parentPart))
+				return true;
 		}
-		if (existsAdjacent)
-			return ADJACENT;
-		else
-			return DISJOINT;
+		return false;
 	}
 };
@@ -191,43 +92,34 @@
 double Node::getDistance()
 {
-	Pt3D size = calculateSize();
-	Pt3D parentSize = parent->calculateSize();	// Here we are sure that parent is not nullptr
-	double parentSphereRadius, sphereRadius;
-	vector<Pt3D> centersParent = PartDistanceEstimator::findSphereCenters(parent->partType, parentSphereRadius, parentSize, parent->getRotation());
-	vector<Pt3D> centers = PartDistanceEstimator::findSphereCenters(partType, sphereRadius, size, getRotation());
+	Pt3D size;
+	calculateSize(size);
+	Pt3D parentSize;
+	parent->calculateSize(parentSize);    // Here we are sure that parent is not nullptr
+	Part *tmpPart = PartDistanceEstimator::buildTemporaryPart(partType, size, getRotation());
+	Part *parentTmpPart = PartDistanceEstimator::buildTemporaryPart(parent->partType, parentSize, parent->getRotation());
 
-	double distanceThreshold = sphereRadius + parentSphereRadius;
+	vector <Pt3D> centers = PartDistanceEstimator::findSphereCenters(tmpPart);
+
 	double minDistance = 0.0;
 	double maxDistance = 2 * (fS_Utils::max3(parentSize) + fS_Utils::max3(size));
 	double currentDistance = fS_Utils::avg(maxDistance, minDistance);
-	int result = -1;
-	int iterationNo = 0;
-	while (result != ADJACENT)
+	int collisionDetected = false;
+	while (maxDistance - minDistance > PartDistanceEstimator::PRECISION)
 	{
-		iterationNo++;
 		Pt3D vectorBetweenParts = state->v * currentDistance;
-		result = PartDistanceEstimator::isCollision(centersParent, centers, vectorBetweenParts, distanceThreshold);
+		collisionDetected = PartDistanceEstimator::isCollision(parentTmpPart, centers, vectorBetweenParts);
 
-		if (result == DISJOINT)
+		if (collisionDetected)
+		{
+			minDistance = currentDistance;
+			currentDistance = fS_Utils::avg(maxDistance, currentDistance);
+		} else
 		{
 			maxDistance = currentDistance;
 			currentDistance = fS_Utils::avg(currentDistance, minDistance);
-		} else if (result == COLLISION)
-		{
-			minDistance = currentDistance;
-			currentDistance = fS_Utils::avg(maxDistance, currentDistance);
 		}
-
-		if(maxDistance <= 0 || iterationNo > 1000)
-			throw fS_Exception("Computing of distances between parts failed", 0);
-		if (currentDistance > maxDistance)
-		{
-			throw fS_Exception("Internal error; the maximal distance between parts exceeded.", 0);
-		}
-		if (currentDistance < minDistance)
-			throw fS_Exception("Internal error; the minimal distance between parts exceeded.", 0);
-
 	}
-
+	delete tmpPart;
+	delete parentTmpPart;
 	return currentDistance;
 }
