Index: cpp/frams/genetics/fS/part_distance_estimator.h
===================================================================
--- cpp/frams/genetics/fS/part_distance_estimator.h	(revision 1017)
+++ cpp/frams/genetics/fS/part_distance_estimator.h	(revision 1030)
@@ -8,120 +8,84 @@
 #include "frams/model/geometry/meshbuilder.h"
 
-class fS_Utils
+class PartDistanceEstimator
 {
 public:
-	static void rotateVector(Pt3D &vector, const Pt3D &rotation)
+
+	static Part *buildTemporaryPart(Part::Shape shape, const Pt3D &scale, const Pt3D &rotation)
 	{
-		Orient rotmatrix = Orient_1;
-		rotmatrix.rotate(rotation);
-		vector = rotmatrix.transform(vector);
+		Part *tmpPart1 = new Part(shape);
+		tmpPart1->scale = scale;
+		tmpPart1->setRot(rotation);
+		return tmpPart1;
 	}
 
-	static double avg(double a, double b)
-	{
-		return 0.5 * (a + b);
-	}
-
-	static double min3(const Pt3D &p)
-	{
-		double tmp = p.x;
-		if (p.y < tmp)
-			tmp = p.y;
-		if (p.z < tmp)
-			tmp = p.z;
-		return tmp;
-	}
-
-	static double max3(const Pt3D &p)
-	{
-		double tmp = p.x;
-		if (p.y > tmp)
-			tmp = p.y;
-		if (p.z > tmp)
-			tmp = p.z;
-		return tmp;
-	}
-};
-
-class PartDistanceEstimator
-{
-
-public:
-	static constexpr double PRECISION = 0.05;
-	static constexpr double RELATIVE_DENSITY = 5.0;
-
-
-	static Part *buildTemporaryPart(Part::Shape shape, const Pt3D &scale, const Pt3D &rotations)
-	{
-		Part *tmpPart = new Part(shape);
-		tmpPart->scale = scale;
-		tmpPart->setRot(rotations);
-		return tmpPart;
-	}
-
-	static vector <Pt3D> findSphereCenters(Part *part)
+	/// Get some of the points from the surface of the part
+	static vector <Pt3D> findSurfacePoints(Part *part, double  relativeDensity)
 	{
 		// Divide by maximal radius to avoid long computations
-		MeshBuilder::PartSurface surface(RELATIVE_DENSITY / fS_Utils::max3(part->scale));
+		MeshBuilder::PartSurface surface(relativeDensity / part->scale.maxComponentValue());
 		surface.initialize(part);
 
-		vector <Pt3D> centers;
+		vector <Pt3D> points;
 		Pt3D point;
 		while (surface.tryGetNext(point))
 		{
-			centers.push_back(point);
+			points.push_back(point);
 		}
-		return centers;
+		return points;
 	}
 
-	static bool isCollision(Part *parentPart, vector <Pt3D> &centers, Pt3D &vectorBetweenParts)
+	/// Check if there is a collision between the parts
+	static bool isCollision(Part *part, vector <Pt3D> &points, Pt3D &vectorBetweenParts)
 	{
 		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++)
+		double maxPartReachSq = pow(CBRT_3 * part->scale.maxComponentValue(), 2);
+		for (int i = 0; i < int(points.size()); i++)
 		{
-			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))
+			Pt3D shifted = points[i] + vectorBetweenParts;
+			double distanceToPointSq = shifted.x * shifted.x + shifted.y * shifted.y + shifted.z * shifted.z;
+			if (distanceToPointSq <= maxPartReachSq && GeometryUtils::isPointInsidePart(shifted, part))
 				return true;
 		}
 		return false;
 	}
+
+
+	static double calculateDistance(Part tmpPart1, Part tmpPart2, double distanceTolerance, double relativeDensity)
+	{
+		/// tmpPart1 and tmpPart2 are copied for purpose and should not be passed as reference
+		/// This function can change some of the properties of those parts
+		Pt3D directionVersor = tmpPart1.p - tmpPart2.p;
+		directionVersor.normalize();
+
+		tmpPart1.p = Pt3D(0);
+		tmpPart2.p = Pt3D(0);
+
+		static double CBRT_3 = std::cbrt(3);
+		vector <Pt3D> points = PartDistanceEstimator::findSurfacePoints(&tmpPart1, relativeDensity);
+
+		double minDistance = tmpPart2.scale.minComponentValue() + tmpPart1.scale.minComponentValue();
+		double maxDistance = CBRT_3 * (tmpPart2.scale.maxComponentValue() + tmpPart1.scale.maxComponentValue());
+		double currentDistance = 0.5 * (maxDistance + minDistance);
+		int collisionDetected = false;
+		while (maxDistance - minDistance > distanceTolerance)
+		{
+			Pt3D vectorBetweenParts = directionVersor * currentDistance;
+			collisionDetected = PartDistanceEstimator::isCollision(&tmpPart2, points, vectorBetweenParts);
+
+			if (collisionDetected)
+			{
+				minDistance = currentDistance;
+				currentDistance = 0.5 * (maxDistance + currentDistance);
+			} else
+			{
+				maxDistance = currentDistance;
+				currentDistance = 0.5 * (currentDistance + minDistance);
+			}
+		}
+		return currentDistance;
+	}
 };
 
-double Node::getDistance()
-{
-	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());
-
-	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 collisionDetected = false;
-	while (maxDistance - minDistance > PartDistanceEstimator::PRECISION)
-	{
-		Pt3D vectorBetweenParts = state->v * currentDistance;
-		collisionDetected = PartDistanceEstimator::isCollision(parentTmpPart, centers, vectorBetweenParts);
-
-		if (collisionDetected)
-		{
-			minDistance = currentDistance;
-			currentDistance = fS_Utils::avg(maxDistance, currentDistance);
-		} else
-		{
-			maxDistance = currentDistance;
-			currentDistance = fS_Utils::avg(currentDistance, minDistance);
-		}
-	}
-	delete tmpPart;
-	delete parentTmpPart;
-	return currentDistance;
-}
 
 #endif //_PART_DISTANCE_ESTIMATOR_H_
