Changeset 1000 for cpp/frams/genetics/fS/fS_general.cpp

Timestamp:

07/13/20 13:53:25 (4 years ago)

Author:

Maciej Komosinski

Message:

Improved the fS encoding

File:

• cpp/frams/genetics/fS/fS_general.cpp (modified) (12 diffs)

cpp/frams/genetics/fS/fS_general.cpp

@@ -11 +11 @@
 #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 +69 @@
 }
 
-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 +121 @@
 }
 
-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 +300 @@
 }
 
-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 +315 @@
         {
                 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 +343 @@
         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 +427 @@
                 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 +465 @@
                 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 +578 @@
                         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 +599 @@
 }
 
-
-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 +613 @@
 }
 
-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 +699 @@
 {
         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;