Index: cpp/frams/genetics/fS/fS_general.cpp
===================================================================
--- cpp/frams/genetics/fS/fS_general.cpp	(revision 973)
+++ cpp/frams/genetics/fS/fS_general.cpp	(revision 1000)
@@ -11,8 +11,25 @@
 #include "frams/neuro/neurolibrary.h"
 #include "../genooperators.h"
+#include "common/nonstd_math.h"
+#include "part_distance_estimator.h"
 
 int fS_Genotype::precision = 4;
 bool fS_Genotype::TURN_WITH_ROTATION = false;
 
+const std::map<string, double> defaultValues = {
+		{INGESTION,      0.5},//Model::getDefPart().ingest },
+		{FRICTION,       0.5},//Model::getDefPart().friction},
+		{STIFFNESS,	 	 0.5},//Model::getDefJoint().stif},
+		{ROT_X,          0.0},
+		{ROT_Y,          0.0},
+		{ROT_Z,          0.0},
+		{RX,             0.0},
+		{RY,             0.0},
+		{RZ,             0.0},
+		{SIZE,           1.0},
+		{SIZE_X,         0.5},//Model::getDefPart().scale.x},
+		{SIZE_Y,         0.5},//Model::getDefPart().scale.y},
+		{SIZE_Z,         0.5}//Model::getDefPart().scale.z}
+};
 
 double fS_stod(const string&  str, int start, size_t* size)
@@ -52,14 +69,7 @@
 }
 
-void rotateVector(Pt3D &vector, const Pt3D &rotation)
-{
-	Orient rotmatrix = Orient_1;
-	rotmatrix.rotate(rotation);
-	vector = rotmatrix.transform(vector);
-}
-
 void State::rotate(const Pt3D &rotation)
 {
-       rotateVector(v, rotation);
+       fS_Utils::rotateVector(v, rotation);
        v.normalize();
 }
@@ -111,8 +121,9 @@
 }
 
-Node::Node(Substring &restOfGeno, Node *_parent)
-{
+Node::Node(Substring &restOfGeno, Node *_parent, GenotypeParams _genotypeParams)
+{
+	partDescription = new Substring(restOfGeno);
+	genotypeParams = _genotypeParams;
 	parent = _parent;
-	partDescription = new Substring(restOfGeno);
 
 	try
@@ -289,169 +300,4 @@
 }
 
-double avg(double a, double b)
-{
-	return 0.5 * (a + b);
-}
-
-double min3(Pt3D p)
-{
-	double tmp = p.x;
-	if (p.y < tmp)
-		tmp = p.y;
-	if (p.z < tmp)
-		tmp = p.z;
-	return tmp;
-}
-
-double max3(Pt3D p)
-{
-	double tmp = p.x;
-	if (p.y > tmp)
-		tmp = p.y;
-	if (p.z > tmp)
-		tmp = p.z;
-	return tmp;
-}
-
-double getSphereCoordinate(double dimension, double sphereDiameter, double index, int count)
-{
-	if (count == 1)
-		return 0;
-	return (dimension - sphereDiameter) * (index / (count - 1) - 0.5);
-}
-
-Pt3D *findSphereCenters(int &sphereCount, double &sphereRadius, Pt3D radii, Pt3D rotations)
-{
-	double sphereRelativeDistance = SPHERE_RELATIVE_DISTANCE;
-	double minRadius = min3(radii);
-	if(minRadius <= 0)
-	    throw fS_Exception("Invalid part size", 0);
-	double maxRadius = max3(radii);
-	if (MAX_DIAMETER_QUOTIENT > maxRadius / minRadius)
-		sphereRadius = minRadius;
-	else
-	{
-		// When max radius is much bigger than min radius
-		sphereRelativeDistance = 1.0;   // Make the spheres adjacent to speed up the computation
-		sphereRadius = maxRadius / MAX_DIAMETER_QUOTIENT;
-	}
-	double sphereDiameter = 2 * sphereRadius;
-
-	double *diameters = new double[3] {2 * radii.x, 2 * radii.y, 2 * radii.z};
-	int counts[3];
-	for (int i = 0; i < 3; i++)
-	{
-		counts[i] = 1;
-		if (diameters[i] > sphereDiameter)
-			counts[i] += ceil((diameters[i] - sphereDiameter) / sphereDiameter / sphereRelativeDistance);
-	}
-
-	sphereCount = counts[0] * counts[1] * counts[2];
-	double x, y, z;
-	int totalCount = 0;
-	Pt3D *centers = new Pt3D[sphereCount];
-	for (double xi = 0; xi < counts[0]; xi++)
-	{
-		x = getSphereCoordinate(diameters[0], sphereDiameter, xi, counts[0]);
-		for (double yi = 0; yi < counts[1]; yi++)
-		{
-			y = getSphereCoordinate(diameters[1], sphereDiameter, yi, counts[1]);
-			for (double zi = 0; zi < counts[2]; zi++)
-			{
-				z = getSphereCoordinate(diameters[2], sphereDiameter, zi, counts[2]);
-				centers[totalCount] = Pt3D(x, y, z);
-				rotateVector(centers[totalCount], rotations);
-				totalCount++;
-			}
-		}
-	}
-	delete[] diameters;
-	return centers;
-}
-
-int isCollision(Pt3D *centersParent, Pt3D *centers, int parentSphereCount, int sphereCount, Pt3D &vector,
-				double distanceThreshold)
-{
-	double upperThreshold = distanceThreshold;
-	double lowerThreshold = SPHERE_DISTANCE_TOLERANCE * distanceThreshold;
-	double distance;
-	double dx, dy, dz;
-	bool existsAdjacent = false;
-	Pt3D *tmpPoint;
-	for (int sc = 0; sc < sphereCount; sc++)
-	{
-		Pt3D shiftedSphere = Pt3D(centers[sc]);
-		shiftedSphere += vector;
-		for (int psc = 0; psc < parentSphereCount; psc++)
-		{
-			tmpPoint = &centersParent[psc];
-			dx = shiftedSphere.x - tmpPoint->x;
-			dy = shiftedSphere.y - tmpPoint->y;
-			dz = shiftedSphere.z - tmpPoint->z;
-			distance = sqrt(dx * dx + dy * dy + dz * dz);
-
-			if (distance <= upperThreshold)
-			{
-				if (distance >= lowerThreshold)
-					existsAdjacent = true;
-				else
-				{
-					return COLLISION;
-				}
-			}
-		}
-	}
-	if (existsAdjacent)
-		return ADJACENT;
-	else
-		return DISJOINT;
-}
-
-double Node::getDistance()
-{
-	Pt3D size = calculateSize();
-	Pt3D parentSize = parent->calculateSize();	// Here we are sure that parent is not nullptr
-	int parentSphereCount, sphereCount;
-	double parentSphereRadius, sphereRadius;
-	Pt3D *centersParent = findSphereCenters(parentSphereCount, parentSphereRadius, parentSize, parent->getRotation());
-	Pt3D *centers = findSphereCenters(sphereCount, sphereRadius, size, getRotation());
-
-	double distanceThreshold = sphereRadius + parentSphereRadius;
-	double minDistance = 0.0;
-	double maxDistance = 2 * (max3(parentSize) + max3(size));
-	double currentDistance = avg(maxDistance, minDistance);
-	int result = -1;
-	int iterationNo = 0;
-	while (result != ADJACENT)
-	{
-		iterationNo++;
-		Pt3D currentVector = state->v * currentDistance;
-		result = isCollision(centersParent, centers, parentSphereCount, sphereCount, currentVector, distanceThreshold);
-
-		if (result == DISJOINT)
-		{
-			maxDistance = currentDistance;
-			currentDistance = avg(currentDistance, minDistance);
-		} else if (result == COLLISION)
-		{
-			minDistance = currentDistance;
-			currentDistance = 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; then maximal distance between parts exceeded.", 0);
-		}
-		if (currentDistance < minDistance)
-			throw fS_Exception("Internal error; the minimal distance between parts exceeded.", 0);
-
-	}
-
-	delete[] centersParent;
-	delete[] centers;
-	return currentDistance;
-}
 
 void Node::getState(State *_state)
@@ -469,5 +315,5 @@
 	{
 		char mod = it->first;
-		double multiplier = pow(MODIFIER_MULTIPLIER, it->second);
+		double multiplier = pow(genotypeParams.modifierMultiplier, it->second);
 		if (mod == MODIFIERS[0])
 			state->ing *= multiplier;
@@ -497,5 +343,5 @@
 	for (int i = 0; i < int(branches.size()); i++)
 	{
-		children.push_back(new Node(branches[i], this));
+		children.push_back(new Node(branches[i], this, genotypeParams));
 	}
 }
@@ -581,8 +427,8 @@
 		return false;
 
-	if (partType == Part::Shape::SHAPE_ELLIPSOID && max3(size) != min3(size))
+	if (partType == Part::Shape::SHAPE_ELLIPSOID && fS_Utils::max3(size) != fS_Utils::min3(size))
 		// When not all radii have different values
 		return false;
-	if (partType == Part::Shape::SHAPE_CYLINDER && size.x != size.y)
+	if (partType == Part::Shape::SHAPE_CYLINDER && size.y != size.z)
 		// If base radii have different values
 		return false;
@@ -619,5 +465,5 @@
 		Neuro *neuro = new Neuro(*neurons[i]);
 		model.addNeuro(neuro);
-		if (neuro->getClass()->preflocation == 2 && parent != nullptr)
+		if (neuro->getClass()->preflocation == NeuroClass::PREFER_JOINT && parent != nullptr)
 		{
 			neuro->attachToJoint(model.getJoint(model.getJointCount() - 1));
@@ -732,7 +578,6 @@
 			result += it->first.c_str();                    // Add parameter key to string
 			result += PARAM_KEY_VALUE_SEPARATOR;
-			string value_text = std::to_string(it->second);
 			// Round the value to two decimal places and add to string
-			result += value_text.substr(0, value_text.find(".") + fS_Genotype::precision).c_str();
+			result += doubleToString(it->second, fS_Genotype::precision).c_str();
 		}
 		result += PARAM_END;
@@ -754,18 +599,4 @@
 }
 
-
-bool Node::changeSizeParam(string key, bool ensureCircleSection)
-{
-	double oldValue = getParam(key);
-	params[key] = GenoOperators::mutateCreep('f', params[key], minValues.at(key), maxValues.at(key), true);
-	if (!ensureCircleSection || isPartSizeValid())
-		return true;
-	else
-	{
-		params[key] = oldValue;
-		return false;
-	}
-}
-
 void Node::getAllNodes(vector<Node *> &allNodes)
 {
@@ -782,11 +613,20 @@
 }
 
-fS_Genotype::fS_Genotype(const string &genotype)
+fS_Genotype::fS_Genotype(const string &geno)
 {
 	try
 	{
-		string geno = genotype.c_str();
-		Substring substring(geno.c_str(), 0, geno.length());
-		startNode = new Node(substring, nullptr);
+		GenotypeParams genotypeParams;
+		genotypeParams.modifierMultiplier = 1.1;
+
+		size_t modeSeparatorIndex = geno.find(MODE_SEPARATOR);
+		if (modeSeparatorIndex == string::npos)
+			throw fS_Exception("Genotype parameters missing", 0);
+
+		genotypeParams.modifierMultiplier = fS_stod(geno, 0, &modeSeparatorIndex);
+
+		int genoStart = modeSeparatorIndex + 1;
+		Substring substring(geno.c_str(), genoStart, geno.length() - genoStart);
+		startNode = new Node(substring, nullptr, genotypeParams);
 		validateNeuroInputs();
 	}
@@ -859,5 +699,9 @@
 {
 	SString geno;
-	geno.reserve(100);     // Provide a small buffer from the start to improve performance
+	geno.reserve(100);
+
+	geno += doubleToString(startNode->genotypeParams.modifierMultiplier, fS_Genotype::precision).c_str();
+	geno += MODE_SEPARATOR;
+
 	startNode->getGeno(geno);
 	return geno;
