Changeset 1017 for cpp/frams/genetics


Ignore:
Timestamp:
07/20/20 16:37:38 (4 years ago)
Author:
Maciej Komosinski
Message:

fS: faster collision detection, depends on "geometry" algorithms

Location:
cpp/frams/genetics
Files:
8 edited

Legend:

Unmodified
Added
Removed

  • cpp/frams/genetics/defgenoconv.cpp

    r958 r1017  
    104104#endif
    105105#ifdef USE_GENCONV_fS0
    106         addConverter(new GenoConv_fS0); //solids
     106        addConverter(new GenoConv_fS0s); //solids
    107107#endif
    108108

  • cpp/frams/genetics/fS/fS_conv.cpp

    r1006 r1017  
    55#include "fS_conv.h"
    66
    7 SString GenoConv_fS0::convert(SString &i, MultiMap *map, bool using_checkpoints)
     7SString GenoConv_fS0s::convert(SString &i, MultiMap *map, bool using_checkpoints)
    88{
    99        fS_Genotype *genotype;
    … …  
    1414        catch (fS_Exception &e)
    1515        {
    16                 logPrintf("GenoConv_fS0", "convert", LOG_ERROR, e.what());
     16                logPrintf("GenoConv_fS0s", "convert", LOG_ERROR, e.what());
    1717                return SString();
    1818        }

  • cpp/frams/genetics/fS/fS_conv.h

    r1006 r1017  
    1212 * Genotype converter from fS to f0s.
    1313 */
    14 class GenoConv_fS0 : public GenoConverter
     14class GenoConv_fS0s : public GenoConverter
    1515{
    1616public:
    17         GenoConv_fS0() : GenoConverter()
     17        GenoConv_fS0s() : GenoConverter()
    1818        {
    1919                name = "Solid encoding";
    … …  
    2727        SString convert(SString &i, MultiMap *map, bool using_checkpoints);
    2828
    29         ~GenoConv_fS0()
     29        ~GenoConv_fS0s()
    3030        {};
    3131};

  • cpp/frams/genetics/fS/fS_general.cpp

    r1006 r1017  
    1616int fS_Genotype::precision = 4;
    1717bool fS_Genotype::TURN_WITH_ROTATION = false;
     18std::map<string, double> Node::minValues;
     19std::map<string, double> Node::defaultValues;
     20std::map<string, double> Node::maxValues;
    1821
    1922void Node::prepareParams()
    2023{
    21         defaultValues = {
    22                         {INGESTION, Model::getDefPart().ingest},
    23                         {FRICTION,  Model::getDefPart().friction},
    24                         {STIFFNESS, Model::getDefJoint().stif},
    25                         {ROT_X,     0.0},
    26                         {ROT_Y,     0.0},
    27                         {ROT_Z,     0.0},
    28                         {RX,        0.0},
    29                         {RY,        0.0},
    30                         {RZ,        0.0},
    31                         {SIZE,      1.0},
    32                         {SIZE_X,    Model::getDefPart().scale.x},
    33                         {SIZE_Y,    Model::getDefPart().scale.y},
    34                         {SIZE_Z,    Model::getDefPart().scale.z}
    35         };
     24        if(minValues.empty())
     25        {
     26                minValues = {
     27                                {INGESTION, Model::getMinPart().ingest},
     28                                {FRICTION,  Model::getMinPart().friction},
     29                                {ROT_X,     -M_PI},
     30                                {ROT_Y,     -M_PI},
     31                                {ROT_Z,     -M_PI},
     32                                {RX,        -M_PI},
     33                                {RY,        -M_PI},
     34                                {RZ,        -M_PI},
     35                                {SIZE,      0.01},
     36                                {SIZE_X,    Model::getMinPart().scale.x},
     37                                {SIZE_Y,    Model::getMinPart().scale.y},
     38                                {SIZE_Z,    Model::getMinPart().scale.z}
     39                };
     40        }
     41
     42        if(maxValues.empty())
     43        {
     44                maxValues = {
     45                                {INGESTION, Model::getMaxPart().ingest},
     46                                {FRICTION,  Model::getMaxPart().friction},
     47                                {ROT_X,     M_PI},
     48                                {ROT_Y,     M_PI},
     49                                {ROT_Z,     M_PI},
     50                                {RX,        M_PI},
     51                                {RY,        M_PI},
     52                                {RZ,        M_PI},
     53                                {SIZE,      100.0},
     54                                {SIZE_X,    Model::getMaxPart().scale.x},
     55                                {SIZE_Y,    Model::getMaxPart().scale.y},
     56                                {SIZE_Z,    Model::getMaxPart().scale.z}
     57                };
     58        }
     59        if(defaultValues.empty())
     60        {
     61                defaultValues = {
     62                                {INGESTION, Model::getDefPart().ingest},
     63                                {FRICTION,  Model::getDefPart().friction},
     64                                {ROT_X,     0.0},
     65                                {ROT_Y,     0.0},
     66                                {ROT_Z,     0.0},
     67                                {RX,        0.0},
     68                                {RY,        0.0},
     69                                {RZ,        0.0},
     70                                {SIZE,      1.0},
     71                                {SIZE_X,    Model::getDefPart().scale.x},
     72                                {SIZE_Y,    Model::getDefPart().scale.y},
     73                                {SIZE_Z,    Model::getDefPart().scale.z}
     74                };
     75        }
    3676}
    3777
    … …  
    5898        fr = _state->fr;
    5999        s = _state->s;
    60         stif = _state->stif;
    61100}
    62101
    … …  
    259298        if(paramsEndIndex == -1)
    260299                throw fS_Exception("Lacking param end sign", restOfGenotype.start);
     300
    261301        for (int i = 0; i < int(separators.size()) - 1; i++)
    262302        {
    … …  
    297337}
    298338
    299 double Node::getParam(string key)
     339double Node::getParam(const string &key)
    300340{
    301341        auto item = params.find(key);
    302342        if (item != params.end())
    303343                return item->second;
    304         else
    305                 return defaultValues.at(key);
    306 }
    307 
    308 
    309 void Node::getState(State *_state)
     344        return defaultValues.at(key);
     345}
     346
     347double Node::getParam(const string &key, double defaultValue)
     348{
     349        auto item = params.find(key);
     350        if (item != params.end())
     351                return item->second;
     352        return defaultValue;
     353}
     354
     355
     356void Node::getState(State *_state, bool calculateLocation)
    310357{
    311358        if (state != nullptr)
    … …  
    328375                else if (mod == MODIFIERS[2])
    329376                        state->s *= multiplier;
    330                 else if (mod == MODIFIERS[3])
    331                         state->stif *= multiplier;
    332         }
    333 
    334         if (parent != nullptr)
     377        }
     378
     379        if (parent != nullptr && calculateLocation)
    335380        {
    336381                // Rotate
    … …  
    341386        }
    342387        for (int i = 0; i < int(children.size()); i++)
    343                 children[i]->getState(state);
     388                children[i]->getState(state, calculateLocation);
    344389}
    345390
    … …  
    388433}
    389434
    390 Pt3D Node::calculateSize()
     435void Node::calculateSize(Pt3D &scale)
    391436{
    392437        double sizeMultiplier = getParam(SIZE) * state->s;
    393         double sx = getParam(SIZE_X) * sizeMultiplier;
    394         double sy = getParam(SIZE_Y) * sizeMultiplier;
    395         double sz = getParam(SIZE_Z) * sizeMultiplier;
    396         return Pt3D(sx, sy, sz);
     438        scale.x = getParam(SIZE_X) * sizeMultiplier;
     439        scale.y = getParam(SIZE_Y) * sizeMultiplier;
     440        scale.z = getParam(SIZE_Z) * sizeMultiplier;
    397441}
    398442
    … …  
    400444{
    401445        double result;
    402         Pt3D size = calculateSize();
     446        Pt3D size;
     447        calculateSize(size);
    403448        double radiiProduct = size.x * size.y * size.z;
    404449        switch (partType)
    … …  
    421466bool Node::isPartSizeValid()
    422467{
    423         Pt3D size = calculateSize();
     468        Pt3D size;
     469        calculateSize(size);
    424470        double volume = calculateVolume();
    425471        Part_MinMaxDef minP = Model::getMinPart();
    … …  
    449495Pt3D Node::getVectorRotation()
    450496{
    451         return Pt3D(getParam(ROT_X), getParam(ROT_Y), getParam(ROT_Z));
     497        return Pt3D(getParam(ROT_X, 0.0), getParam(ROT_Y, 0.0), getParam(ROT_Z, 0.0));
    452498}
    453499
    454500Pt3D Node::getRotation()
    455501{
    456         Pt3D rotation = Pt3D(getParam(RX), getParam(RY), getParam(RZ));
     502        Pt3D rotation = Pt3D(getParam(RX, 0.0), getParam(RY, 0.0), getParam(RZ, 0.0));
    457503        if(fS_Genotype::TURN_WITH_ROTATION)
    458504                rotation += getVectorRotation();
    … …  
    492538{
    493539        part = new Part(partType);
    494         part->p = Pt3D(state->location.x,
    495                                    state->location.y,
    496                                    state->location.z);
     540        part->p = Pt3D(state->location);
    497541
    498542        part->friction = getParam(FRICTION) * state->fr;
    499543        part->ingest = getParam(INGESTION) * state->ing;
    500         Pt3D size = calculateSize();
    501         part->scale.x = size.x;
    502         part->scale.y = size.y;
    503         part->scale.z = size.z;
     544        calculateSize(part->scale);
    504545        part->setRot(getRotation());
    505546}
    … …  
    508549{
    509550        Joint *j = new Joint();
    510         j->stif = getParam(STIFFNESS) * state->stif;
    511         j->rotstif = j->stif;
    512 
    513551        j->attachToParts(parent->part, part);
    514552        switch (joint)
    … …  
    650688}
    651689
    652 void fS_Genotype::getState()
     690void fS_Genotype::getState(bool calculateLocation)
    653691{
    654692        State *initialState = new State(Pt3D(0), Pt3D(1, 0, 0));
    655         startNode->getState(initialState);
     693        startNode->getState(initialState, calculateLocation);
    656694}
    657695
    … …  
    662700        model.open(using_checkpoints);
    663701
    664         getState();
     702        getState(true);
    665703        startNode->buildModel(model, nullptr);
    666704        buildNeuroConnections(model);
    … …  
    808846int fS_Genotype::checkValidityOfPartSizes()
    809847{
    810         getState();
     848        getState(false);
    811849        vector<Node*> nodes = getAllNodes();
    812850        for (int i = 0; i < int(nodes.size()); i++)

  • cpp/frams/genetics/fS/fS_general.h

    r1006 r1017  
    5454#define RZ "rz"
    5555//@}
    56 /** @name Macros and values used in collision detection */
    57 //@{
    58 #define DISJOINT 0
    59 #define COLLISION 1
    60 #define ADJACENT 2
    61 //@}
     56
    6257
    6358#define HINGE_X 'b'
    … …  
    8782const string ALL_JOINTS = "abc";
    8883const int JOINT_COUNT = JOINTS.length();
    89 const string MODIFIERS = "IFST";
     84const string MODIFIERS = "IFS";
    9085const char SIZE_MODIFIER = 's';
    91 const vector<string> PARAMS {INGESTION, FRICTION, ROT_X, ROT_Y, ROT_Z, RX, RY, RZ, SIZE, SIZE_X, SIZE_Y, SIZE_Z,
    92                                                          STIFFNESS};
     86const vector<string> PARAMS {INGESTION, FRICTION, ROT_X, ROT_Y, ROT_Z, RX, RY, RZ, SIZE, SIZE_X, SIZE_Y, SIZE_Z};
    9387const vector<string> SIZE_PARAMS {SIZE, SIZE_X, SIZE_Y, SIZE_Z};
    9488
    … …  
    214208        double ing = 1.0;      /// Ingestion multiplier
    215209        double s = 1.0;      /// Size multipliers
    216         double stif = 1.0;      /// Stiffness multipliers
    217210
    218211        State(State *_state); /// Derive the state from parent
    … …  
    272265        Part *part;     /// A part object built from node. Used in building the Model
    273266        int partCodeLen; /// The length of substring that directly describes the corresponding part
    274         std::map<string, double> defaultValues;
    275267        GenotypeParams genotypeParams;
    276268
    … …  
    335327         * @param _state state of the parent
    336328         */
    337         void getState(State *_state);
     329        void getState(State *_state, bool calculateLocation);
    338330
    339331        /**
    … …  
    370362
    371363public:
     364        static std::map<string, double> minValues;
     365        static std::map<string, double> defaultValues;
     366        static std::map<string, double> maxValues;
    372367        char joint = DEFAULT_JOINT;           /// Set of all joints
    373368        Part::Shape partType;  /// The type of the part
    … …  
    389384         * @return The effective size
    390385         */
    391         Pt3D calculateSize();
     386        void calculateSize(Pt3D &scale);
    392387
    393388        /**
    … …  
    407402         * @return the param value
    408403         */
    409         double getParam(string key);
     404        double getParam(const string &key);
     405        double getParam(const string &key, double defaultValue);
    410406};
    411407
    … …  
    454450        ~fS_Genotype();
    455451
    456         void getState();
     452        void getState(bool calculateLocation);
    457453
    458454        /**

  • cpp/frams/genetics/fS/fS_oper.cpp

    r1006 r1017  
    2424                                {"fS_mut_mod_neuro_conn",   0, 0, "Modify neuron connection",    "f 0 100 10", FIELD(prob[FS_MOD_NEURO_CONNECTION]), "mutation: probability of changing a neuron connection",},
    2525                                {"fS_mut_add_neuro_conn",   0, 0, "Add neuron connection",       "f 0 100 10", FIELD(prob[FS_ADD_NEURO_CONNECTION]), "mutation: probability of adding a neuron connection",},
    26                                 {"fS_mut_rem neuro_conn",   0, 0, "Remove neuron connection",    "f 0 100 10", FIELD(prob[FS_REM_NEURO_CONNECTION]), "mutation: probability of removing a neuron connection",},
     26                                {"fS_mut_rem_neuro_conn",   0, 0, "Remove neuron connection",    "f 0 100 10", FIELD(prob[FS_REM_NEURO_CONNECTION]), "mutation: probability of removing a neuron connection",},
    2727                                {"fS_mut_mod_neuro_params", 0, 0, "Modify neuron params",        "f 0 100 10", FIELD(prob[FS_MOD_NEURO_PARAMS]),     "mutation: probability of changing a neuron param",},
    2828                                {"fS_circle_section",       0, 0, "Ensure circle section",       "d 0 1 1",    FIELD(ensureCircleSection),           "Ensure that ellipsoids and cylinders have circle cross-section"},
    … …  
    3535#undef FIELDSTRUCT
    3636
    37 
    38 void GenoOper_fS::prepareParams()
    39 {
    40         minValues = {
    41                         {INGESTION, Model::getMinPart().ingest},
    42                         {FRICTION,  Model::getMinPart().friction},
    43                         {STIFFNESS, 0.1},
    44                         {ROT_X,     -M_PI},
    45                         {ROT_Y,     -M_PI},
    46                         {ROT_Z,     -M_PI},
    47                         {RX,        -M_PI},
    48                         {RY,        -M_PI},
    49                         {RZ,        -M_PI},
    50                         {SIZE,      0.01},
    51                         {SIZE_X,    Model::getMinPart().scale.x},
    52                         {SIZE_Y,    Model::getMinPart().scale.y},
    53                         {SIZE_Z,    Model::getMinPart().scale.z}
    54         };
    55 
    56         maxValues = {
    57                         {INGESTION, Model::getMaxPart().ingest},
    58                         {FRICTION,  Model::getMaxPart().friction},
    59                         {STIFFNESS, 0.5},
    60                         {ROT_X,     M_PI},
    61                         {ROT_Y,     M_PI},
    62                         {ROT_Z,     M_PI},
    63                         {RX,        M_PI},
    64                         {RY,        M_PI},
    65                         {RZ,        M_PI},
    66                         {SIZE,      100.0},
    67                         {SIZE_X,    Model::getMaxPart().scale.x},
    68                         {SIZE_Y,    Model::getMaxPart().scale.y},
    69                         {SIZE_Z,    Model::getMaxPart().scale.z}
    70         };
    71 }
    72 
    7337GenoOper_fS::GenoOper_fS()
    7438{
    75         prepareParams();
    7639        par.setParamTab(genooper_fS_paramtab);
    7740        par.select(this);
    … …  
    332295bool GenoOper_fS::addPart(fS_Genotype &geno, const vector <Part::Shape> &availablePartShapes, bool mutateSize)
    333296{
    334         geno.getState();
     297        geno.getState(false);
    335298        Node *node = geno.chooseNode();
    336299        char partType = SHAPE_TO_GENE.at(availablePartShapes[rndUint(availablePartShapes.size())]);
    … …  
    375338        if (mutateSize)
    376339        {
    377                 geno.getState();
     340                geno.getState(false);
    378341                mutateSizeParam(newNode, SIZE_X, true);
    379342                mutateSizeParam(newNode, SIZE_Y, true);
    … …  
    387350        Node *randomNode, *selectedChild;
    388351        // Choose a parent with children
    389         for (int i = 0; i < mutationTries; i++)
     352        // It may be difficult to choose a eligible node, so the number of tries should be high
     353        for (int i = 0; i < 10 * mutationTries; i++)
    390354        {
    391355                randomNode = geno.chooseNode();
    … …  
    425389#endif
    426390
    427                 geno.getState();
     391                geno.getState(false);
    428392                double sizeMultiplier = randomNode->getParam(SIZE) * randomNode->state->s;
    429393                double relativeVolume = randomNode->calculateVolume() / pow(sizeMultiplier, 3.0);
    … …  
    485449        bool isRadius = isRadiusOfBase || key == SIZE_X;
    486450        if (ensureCircleSection && isRadius)
    487         if (ensureCircleSection && isRadius)
    488451        {
    489452                if (randomNode->partType == Part::Shape::SHAPE_ELLIPSOID)
    … …  
    493456        }
    494457        // Add modified default value for param
    495         randomNode->params[key] = mutateCreep('f', randomNode->defaultValues.at(key), minValues.at(key), maxValues.at(key), true);
    496         return true;
     458        randomNode->params[key] = randomNode->defaultValues.at(key);
     459        geno.getState(false);
     460        return mutateParamValue(randomNode, key);
    497461}
    498462
    … …  
    508472                        auto it = randomNode->params.begin();
    509473                        advance(it, rndUint(paramCount));
    510                         randomNode->params.erase(it->first);
    511                         return true;
     474                        string key = it->first;
     475                        double value = it->second;
     476
     477                        randomNode->params.erase(key);
     478                        if(geno.checkValidityOfPartSizes() == 0)
     479                                return true;
     480                        else
     481                        {
     482                                randomNode->params[key] = value;
     483                        }
    512484                }
    513485        }
    … …  
    515487}
    516488
     489
     490bool GenoOper_fS::mutateParamValue(Node *node, string key)
     491{
     492        // Do not allow invalid changes in part size
     493        if (std::find(SIZE_PARAMS.begin(), SIZE_PARAMS.end(), key) == SIZE_PARAMS.end())
     494        {
     495                node->params[key] = GenoOperators::mutateCreep('f', node->getParam(key), Node::minValues.at(key), Node::maxValues.at(key), true);
     496                return true;
     497        } else
     498                return mutateSizeParam(node, key, ensureCircleSection);
     499}
     500
    517501bool GenoOper_fS::changeParam(fS_Genotype &geno)
    518502{
    519         geno.getState();
     503        geno.getState(false);
    520504        for (int i = 0; i < mutationTries; i++)
    521505        {
    … …  
    526510                        auto it = randomNode->params.begin();
    527511                        advance(it, rndUint(paramCount));
    528 
    529                         // Do not allow invalid changes in part size
    530                         if (std::find(SIZE_PARAMS.begin(), SIZE_PARAMS.end(), it->first) == SIZE_PARAMS.end())
    531                         {
    532                                 it->second = GenoOperators::mutateCreep('f', it->second, minValues.at(it->first), maxValues.at(it->first), true);
    533                                 return true;
    534                         } else
    535                                 return mutateSizeParam(randomNode, it->first, ensureCircleSection);
     512                        return mutateParamValue(randomNode, it->first);
    536513                }
    537514        }
    … …  
    543520        Node *randomNode = geno.chooseNode();
    544521        char randomModifier = MODIFIERS[rndUint(MODIFIERS.length())];
     522        int oldValue = randomNode->modifiers[randomModifier];
     523
    545524        randomNode->modifiers[randomModifier] += rndUint(2) == 0 ? 1 : -1;
    546525
    … …  
    548527        if (isSizeMod && geno.checkValidityOfPartSizes() != 0)
    549528        {
    550                 randomNode->modifiers[randomModifier]++;
     529                randomNode->modifiers[randomModifier] = oldValue;
    551530                return false;
    552531        }
    … …  
    715694        }
    716695
    717         double min = std::max(minValues.at(key), valueAtMinVolume);
    718         double max = std::min(maxValues.at(key), valueAtMaxVolume);
     696        double min = std::max(Node::minValues.at(key), valueAtMinVolume);
     697        double max = std::min(Node::maxValues.at(key), valueAtMaxVolume);
    719698
    720699        node->params[key] = GenoOperators::mutateCreep('f', node->getParam(key), min, max, true);

  • cpp/frams/genetics/fS/fS_oper.h

    r1006 r1017  
    4242        paInt useElli, useCub,  useCyl;
    4343        paInt strongAddPart;
    44 
    45         std::map<string, double> minValues;
    46         std::map<string, double> maxValues;
    4744
    4845        GenoOper_fS();
    … …  
    118115
    119116        /**
     117         * Changes the value of specified parameter.
     118         * The state of the node must be previously calculated
     119         * @param node - the node on which parameter is modified
     120         * @param key - the key of parameter
     121         * @return
     122         */
     123        bool mutateParamValue(Node *node, string key);
     124
     125        /**
    120126         * Performs change modifier mutation on genotype
    121127         * @return true if mutation succeeded, false otherwise
    … …  
    144150         */
    145151        bool mutateSizeParam(Node *node, string key, bool ensureCircleSection);
    146 
    147         void prepareParams();
    148152};
    149153

  • cpp/frams/genetics/fS/part_distance_estimator.h

    r1006 r1017  
    55#ifndef _PART_DISTANCE_ESTIMATOR_H_
    66#define _PART_DISTANCE_ESTIMATOR_H_
     7
     8#include "frams/model/geometry/meshbuilder.h"
    79
    810class fS_Utils
    … …  
    4446class PartDistanceEstimator
    4547{
    46         /**
    47          * Used in finding the proper distance between the parts
    48          * distance between spheres / sphere radius
    49          * That default value can be changed in certain cases
    50          * */
    51         static constexpr float SPHERE_RELATIVE_DISTANCE = 0.5;
    52         /**
    53          * Used in finding the proper distance between the parts
    54          * The maximal allowed value for
    55          * maximal radius of the node / sphere radius
    56          */
    57         static const int MAX_DIAMETER_QUOTIENT = 30;
    58         /**
    59          * The tolerance of the value of distance between parts
    60          */
    61         static constexpr double SPHERE_DISTANCE_TOLERANCE = 0.96;
    6248
    63         static constexpr double SPHERE_SIZE_QUOTIENT = 0.25;
     49public:
     50        static constexpr double PRECISION = 0.05;
     51        static constexpr double RELATIVE_DENSITY = 5.0;
    6452
    6553
    66         static bool isInsidePart(Part::Shape shape, const Pt3D &partRadii, const Pt3D &center, double sphereRadius)
     54        static Part *buildTemporaryPart(Part::Shape shape, const Pt3D &scale, const Pt3D &rotations)
    6755        {
    68                 if(sphereRadius >= fS_Utils::min3(partRadii))
    69                         return true; // TODO Special case
    70                 Pt3D mostRemote = Pt3D(fabs(center.x), fabs(center.y), fabs(center.z)) + Pt3D(sphereRadius);
    71                 bool isInEllipsis;
    72 
    73                 bool result = false;
    74 
    75                 switch (shape)
    76                 {
    77                         case Part::Shape::SHAPE_CUBOID:
    78                                 if(mostRemote.x <= partRadii.x && mostRemote.y <= partRadii.y && mostRemote.z <= partRadii.z)
    79                                         result = true;
    80                                 break;
    81                         case Part::Shape::SHAPE_CYLINDER:
    82                                 isInEllipsis = pow(center.y / (partRadii.y - sphereRadius), 2) + pow(center.z / (partRadii.z - sphereRadius), 2) <= 1.0;
    83                                 if (mostRemote.x <= partRadii.x && isInEllipsis)
    84                                         result = true;
    85                                 break;
    86                         case Part::Shape::SHAPE_ELLIPSOID:
    87                                 if(pow(center.x / (partRadii.x - sphereRadius), 2) + pow(center.y / (partRadii.y - sphereRadius), 2) + pow(center.z / (partRadii.z - sphereRadius), 2) <= 1.0)
    88                                         result = true;
    89                                 break;
    90                         default:
    91                                 logMessage("fS", "calculateVolume", LOG_ERROR, "Invalid part type");
    92                 }
    93                 return result;
     56                Part *tmpPart = new Part(shape);
     57                tmpPart->scale = scale;
     58                tmpPart->setRot(rotations);
     59                return tmpPart;
    9460        }
    9561
    96 public:
    97         static vector<Pt3D> findSphereCenters(Part::Shape shape, double &sphereRadius, const Pt3D &radii, const Pt3D &rotations)
     62        static vector <Pt3D> findSphereCenters(Part *part)
    9863        {
    99                 double sphereRelativeDistance = SPHERE_RELATIVE_DISTANCE;
    100                 double minRadius = fS_Utils::min3(radii);
    101                 if(minRadius <= 0)
    102                         throw fS_Exception("Invalid part size in PartDistanceEstimator", 0);
    103                 double maxRadius = fS_Utils::max3(radii);
    104                 sphereRadius = SPHERE_SIZE_QUOTIENT * minRadius;
    105                 if (MAX_DIAMETER_QUOTIENT < maxRadius / sphereRadius)
     64                // Divide by maximal radius to avoid long computations
     65                MeshBuilder::PartSurface surface(RELATIVE_DENSITY / fS_Utils::max3(part->scale));
     66                surface.initialize(part);
     67
     68                vector <Pt3D> centers;
     69                Pt3D point;
     70                while (surface.tryGetNext(point))
    10671                {
    107                         // When max radius is much bigger than sphereRadius and there are to many spheresZ
    108                         sphereRelativeDistance = 1.0;   // Make the spheres adjacent to speed up the computation
    109                         sphereRadius = maxRadius / MAX_DIAMETER_QUOTIENT;
    110                 }
    111                 else if(shape == Part::Shape::SHAPE_ELLIPSOID)
    112                         sphereRadius = minRadius;
    113 
    114                 double sphereDiameter = 2 * sphereRadius;
    115 
    116                 double radiiArr[3]{radii.x, radii.y, radii.z};
    117                 vector<double> coordinates[3];
    118                 for (int dim = 0; dim < 3; dim++)
    119                 {
    120                         double spaceForSphereCenters = 2 * radiiArr[dim] - sphereDiameter;
    121                         if (spaceForSphereCenters > 0)
    122                         {
    123                                 int lastIndex = ceil(spaceForSphereCenters / (sphereDiameter * sphereRelativeDistance));
    124                                 for (int i = 0; i <= lastIndex; i++)
    125                                 {
    126                                         coordinates[dim].push_back(spaceForSphereCenters * (double(i) / lastIndex - 0.5));
    127                                 }
    128                         }
    129                         else
    130                                 coordinates[dim].push_back(0.0);
    131                 }
    132 
    133                 vector<Pt3D> centers;
    134                 for (int xi = 0; xi < int(coordinates[0].size()); xi++)
    135                 {
    136                         for (int yi = 0; yi < int(coordinates[1].size()); yi++)
    137                         {
    138                                 for (int zi = 0; zi < int(coordinates[2].size()); zi++)
    139                                 {
    140                                                 Pt3D center = Pt3D(coordinates[0][xi], coordinates[1][yi], coordinates[2][zi]);
    141                                                 if (isInsidePart(shape, radii, center, sphereRadius))
    142                                                 {
    143                                                         fS_Utils::rotateVector(center, rotations);
    144                                                         centers.push_back(center);
    145                                                 }
    146                                 }
    147                         }
     72                        centers.push_back(point);
    14873                }
    14974                return centers;
    15075        }
    15176
    152         static int isCollision(vector<Pt3D> &centersParent, vector<Pt3D> &centers, Pt3D &vectorBetweenParts,
    153                                         double distanceThreshold)
     77        static bool isCollision(Part *parentPart, vector <Pt3D> &centers, Pt3D &vectorBetweenParts)
    15478        {
    155                 double upperThresholdSq = distanceThreshold * distanceThreshold;
    156                 double lowerThresholdSq = pow(SPHERE_DISTANCE_TOLERANCE * distanceThreshold, 2);
    157                 double distanceSq;
    158                 double dx, dy, dz;
    159                 bool existsAdjacent = false;
    160                 Pt3D *tmpPoint;
    161                 for (int sc = 0; sc < int(centers.size()); sc++)
     79                static double CBRT_3 = std::cbrt(3);
     80                double maxParentReachSq = pow(CBRT_3 * fS_Utils::max3(parentPart->scale), 2);
     81                for (int i = 0; i < int(centers.size()); i++)
    16282                {
    163                         Pt3D shiftedSphere = Pt3D(centers[sc]);
    164                         shiftedSphere += vectorBetweenParts;
    165                         for (int psc = 0; psc < int(centersParent.size()); psc++)
    166                         {
    167                                 tmpPoint = &centersParent[psc];
    168                                 dx = shiftedSphere.x - tmpPoint->x;
    169                                 dy = shiftedSphere.y - tmpPoint->y;
    170                                 dz = shiftedSphere.z - tmpPoint->z;
    171                                 distanceSq = dx * dx + dy * dy + dz * dz;
    172 
    173                                 if (distanceSq <= upperThresholdSq)
    174                                 {
    175                                         if (distanceSq >= lowerThresholdSq)
    176                                                 existsAdjacent = true;
    177                                         else
    178                                         {
    179                                                 return COLLISION;
    180                                         }
    181                                 }
    182                         }
     83                        Pt3D shifted = centers[i] + vectorBetweenParts;
     84                        double distanceToCenterSq = shifted.x * shifted.x + shifted.y * shifted.y + shifted.z * shifted.z;
     85                        if (distanceToCenterSq <= maxParentReachSq && GeometryUtils::isPointInsidePart(shifted, parentPart))
     86                                return true;
    18387                }
    184                 if (existsAdjacent)
    185                         return ADJACENT;
    186                 else
    187                         return DISJOINT;
     88                return false;
    18889        }
    18990};
    … …  
    19192double Node::getDistance()
    19293{
    193         Pt3D size = calculateSize();
    194         Pt3D parentSize = parent->calculateSize();      // Here we are sure that parent is not nullptr
    195         double parentSphereRadius, sphereRadius;
    196         vector<Pt3D> centersParent = PartDistanceEstimator::findSphereCenters(parent->partType, parentSphereRadius, parentSize, parent->getRotation());
    197         vector<Pt3D> centers = PartDistanceEstimator::findSphereCenters(partType, sphereRadius, size, getRotation());
     94        Pt3D size;
     95        calculateSize(size);
     96        Pt3D parentSize;
     97        parent->calculateSize(parentSize);    // Here we are sure that parent is not nullptr
     98        Part *tmpPart = PartDistanceEstimator::buildTemporaryPart(partType, size, getRotation());
     99        Part *parentTmpPart = PartDistanceEstimator::buildTemporaryPart(parent->partType, parentSize, parent->getRotation());
    198100
    199         double distanceThreshold = sphereRadius + parentSphereRadius;
     101        vector <Pt3D> centers = PartDistanceEstimator::findSphereCenters(tmpPart);
     102
    200103        double minDistance = 0.0;
    201104        double maxDistance = 2 * (fS_Utils::max3(parentSize) + fS_Utils::max3(size));
    202105        double currentDistance = fS_Utils::avg(maxDistance, minDistance);
    203         int result = -1;
    204         int iterationNo = 0;
    205         while (result != ADJACENT)
     106        int collisionDetected = false;
     107        while (maxDistance - minDistance > PartDistanceEstimator::PRECISION)
    206108        {
    207                 iterationNo++;
    208109                Pt3D vectorBetweenParts = state->v * currentDistance;
    209                 result = PartDistanceEstimator::isCollision(centersParent, centers, vectorBetweenParts, distanceThreshold);
     110                collisionDetected = PartDistanceEstimator::isCollision(parentTmpPart, centers, vectorBetweenParts);
    210111
    211                 if (result == DISJOINT)
     112                if (collisionDetected)
     113                {
     114                        minDistance = currentDistance;
     115                        currentDistance = fS_Utils::avg(maxDistance, currentDistance);
     116                } else
    212117                {
    213118                        maxDistance = currentDistance;
    214119                        currentDistance = fS_Utils::avg(currentDistance, minDistance);
    215                 } else if (result == COLLISION)
    216                 {
    217                         minDistance = currentDistance;
    218                         currentDistance = fS_Utils::avg(maxDistance, currentDistance);
    219120                }
    220 
    221                 if(maxDistance <= 0 || iterationNo > 1000)
    222                         throw fS_Exception("Computing of distances between parts failed", 0);
    223                 if (currentDistance > maxDistance)
    224                 {
    225                         throw fS_Exception("Internal error; the maximal distance between parts exceeded.", 0);
    226                 }
    227                 if (currentDistance < minDistance)
    228                         throw fS_Exception("Internal error; the minimal distance between parts exceeded.", 0);
    229 
    230121        }
    231 
     122        delete tmpPart;
     123        delete parentTmpPart;
    232124        return currentDistance;
    233125}
Note: See TracChangeset for help on using the changeset viewer.